[ { "path": ".gitignore", "content": "*.data\n*.pyc\n*.npy\n*.npz\n*.pth\n*.bak\n" }, { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(cerebro)\n\n# Catkin components\nfind_package(catkin REQUIRED COMPONENTS\n cv_bridge\n image_transport\n roscpp\n rospy\n std_msgs\n message_generation\n)\n\n\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\nset( CMAKE_CXX_STANDARD 11 )\nfind_package(Threads)\nfind_package(Eigen3 REQUIRED)\nfind_package(Ceres REQUIRED)\nfind_package(OpenCV 3 REQUIRED)\n#set( CMAKE_CXX_FLAGS \"-fpermissive -std=c++11 -O3 -Wall -Wextra -pedantic\" )\n\n# Removing the dependence on theia for the production code.\n# If you want to compile unit tests, some of those need theia-sfm as dependence.\nfind_package(Theia REQUIRED)\ninclude_directories(${THEIA_INCLUDE_DIRS})\n\nfind_package(Boost REQUIRED COMPONENTS filesystem program_options system)\ninclude_directories(${Boost_INCLUDE_DIRS})\n\n\n#----------START gperftools\n# install : sudo apt-get install google-perftools libgoogle-perftools-dev\n# get cmake file: https://raw.githubusercontent.com/vast-io/vast/master/cmake/FindGperftools.cmake\n# save to '/usr/local/lib/cmake'\n#list(APPEND CMAKE_MODULE_PATH \"/usr/local/lib/cmake\")\n#find_package(Gperftools REQUIRED)\n#message( \"----- GPERF\")\n#message( \"----- \"${GPERFTOOLS_LIBRARIES} )\nset (CMAKE_CXX_FLAGS \" -Wl,-no-as-needed\")\n#set(CMAKE_BUILD_TYPE Debug)\nset(CMAKE_BUILD_TYPE Release)\n#set(CMAKE_BUILD_TYPE RelWithDebInfo)\n\n#----------END GPERFTOOLS\n\noption(COMPILE_UNIT_TEST \"Compile unit tests\" OFF) #OFF by default. catkin_make -DCOMPILE_UNIT_TEST=ON will compile it. OFF will skip it\n\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a run_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\nadd_message_files(\n FILES\n LoopEdge.msg\n)\n\n## Generate services in the 'srv' folder\nadd_service_files(\n FILES\n WholeImageDescriptorCompute.srv\n )\n\n\n## Generate added messages and services with any dependencies listed here\n generate_messages(\n DEPENDENCIES\n std_msgs\n sensor_msgs\n )\n\n\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if you package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n INCLUDE_DIRS include\n# LIBRARIES cerebro\n# CATKIN_DEPENDS cv_bridge image_transport roscpp rospy std_msgs\n# DEPENDS system_lib\nCATKIN_DEPENDS message_runtime\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(include)\ninclude_directories(\n ${catkin_INCLUDE_DIRS}\n ${CERES_INCLUDE_DIRS}\n ${EIGEN3_INCLUDE_DIRS}\n ${OpenCV_INCLUDE_DIRS}\n)\n\n\ninclude_directories(\n src/utils/camodocal/include\n src/utils/\n )\nFILE(GLOB CamodocalCameraModelSources\n src/utils/camodocal/src/chessboard/Chessboard.cc\n src/utils/camodocal/src/calib/CameraCalibration.cc\n src/utils/camodocal/src/camera_models/Camera.cc\n src/utils/camodocal/src/camera_models/CameraFactory.cc\n src/utils/camodocal/src/camera_models/CostFunctionFactory.cc\n src/utils/camodocal/src/camera_models/PinholeCamera.cc\n src/utils/camodocal/src/camera_models/CataCamera.cc\n src/utils/camodocal/src/camera_models/EquidistantCamera.cc\n src/utils/camodocal/src/camera_models/ScaramuzzaCamera.cc\n src/utils/camodocal/src/sparse_graph/Transform.cc\n src/utils/camodocal/src/gpl/gpl.cc\n src/utils/camodocal/src/gpl/EigenQuaternionParameterization.cc\n )\n\nFILE( GLOB KuseUtilsSources\n src/utils/PoseManipUtils.cpp\n src/utils/RosMarkerUtils.cpp\n src/utils/RawFileIO.cpp\n src/utils/MiscUtils.cpp\n src/utils/Plot2Mat.cpp\n src/utils/CameraGeometry.cpp\n src/utils/PointFeatureMatching.cpp\n src/utils/GMSMatcher/gms_matcher.cpp\n)\n\n\n## Declare a C++ executable\n\nadd_executable(\n cerebro_node\n src/cerebro_node.cpp\n src/PinholeCamera.cpp\n src/DataManager.cpp\n src/DataNode.cpp\n src/Cerebro.cpp\n src/Visualization.cpp\n src/ProcessedLoopCandidate.cpp\n src/DlsPnpWithRansac.cpp\n src/HypothesisManager.cpp\n src/ImageDataManager.cpp\n ${KuseUtilsSources}\n ${CamodocalCameraModelSources}\n)\n\n# this is needed to indicate this this executable depends on the messages of the pkg.\nadd_dependencies(cerebro_node cerebro_generate_messages_cpp)\n\n\ntarget_link_libraries(cerebro_node\n ${catkin_LIBRARIES}\n ${OpenCV_LIBRARIES}\n ${CMAKE_THREAD_LIBS_INIT}\n ${CERES_LIBRARIES}\n ${Boost_LIBRARIES}\n ${THEIA_LIBRARIES}\n\n #${GPERFTOOLS_LIBRARIES}\n #/usr/local/lib/libfaiss.so\n)\n\nif(COMPILE_UNIT_TEST)\n #add some compilation flags\n\n\n ## Unit Tests\n add_executable (\n unittest_termcolor\n src/unittest/unittest_termcolor.cpp\n )\n\n add_executable (\n unittest_rosservice_client\n src/unittest/unittest_rosservice_client.cpp\n )\n\n add_executable (\n unittest_camodocal_proj\n src/unittest/unittest_camodocal_proj.cpp\n ${KuseUtilsSources}\n ${CamodocalCameraModelSources}\n )\n\n\n add_executable (\n unittest_camera_geom_class_usage\n src/unittest/unittest_camera_geom_class_usage.cpp\n ${KuseUtilsSources}\n ${CamodocalCameraModelSources}\n )\n\n\n add_executable (\n unittest_theia\n src/unittest/unittest_theia.cpp\n ${KuseUtilsSources}\n ${CamodocalCameraModelSources}\n )\n\n add_executable (\n unittest_pose_tester\n src/unittest/unittest_pose_tester.cpp\n ${KuseUtilsSources}\n ${CamodocalCameraModelSources}\n src/DlsPnpWithRansac.cpp\n )\n\n\n add_executable (\n unittest_plot2mat\n src/unittest/unittest_plot2mat.cpp\n src/utils/Plot2Mat.cpp\n )\n\n\n target_link_libraries(unittest_rosservice_client\n ${catkin_LIBRARIES}\n ${OpenCV_LIBRARIES}\n ${CMAKE_THREAD_LIBS_INIT}\n ${CERES_LIBRARIES}\n )\n\n\n target_link_libraries(unittest_camodocal_proj\n ${catkin_LIBRARIES}\n ${OpenCV_LIBRARIES}\n ${CMAKE_THREAD_LIBS_INIT}\n ${CERES_LIBRARIES}\n )\n\n target_link_libraries(unittest_camera_geom_class_usage\n ${catkin_LIBRARIES}\n ${OpenCV_LIBRARIES}\n ${CMAKE_THREAD_LIBS_INIT}\n ${CERES_LIBRARIES}\n )\n\n\n\n target_link_libraries(unittest_theia\n ${catkin_LIBRARIES}\n ${OpenCV_LIBRARIES}\n ${CMAKE_THREAD_LIBS_INIT}\n ${CERES_LIBRARIES}\n ${THEIA_LIBRARIES}\n )\n\n target_link_libraries(unittest_pose_tester\n ${catkin_LIBRARIES}\n ${OpenCV_LIBRARIES}\n ${CMAKE_THREAD_LIBS_INIT}\n ${CERES_LIBRARIES}\n ${THEIA_LIBRARIES}\n )\n\n target_link_libraries(unittest_plot2mat\n ${catkin_LIBRARIES}\n ${OpenCV_LIBRARIES}\n )\n\n\nelse()\n #add some other compilation flags\nendif(COMPILE_UNIT_TEST)\nunset(COMPILE_UNIT_TEST CACHE) # <---- this is the important!!\n\n\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# install(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables and/or libraries for installation\n# install(TARGETS cerebro cerebro_node\n# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark cpp header files for installation\n# install(DIRECTORY include/${PROJECT_NAME}/\n# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n# FILES_MATCHING PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n# )\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_cerebro.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n" }, { "path": "README.md", "content": "# VINS-Fusion with Cerebro\nThis is the cerebro module for VINS-Fusion. The aim of this project\nis better loop detection and recover from kidnap. The cerebro node connects to\nthe vins_estimator nodes of VINS-Fusion\n(with ros interface). The DataManager class handles\nall the incoming data. Visualization handles all the visualization.\nCerebro class handles the loop closure intelligence part. It publishes a LoopMsg\nwhich contains timestamps of the identified loopcandidate along with the\ncomputed relative pose between the pair. The pose computation needs a stereo pair\nfor reliable pose computation.\nThis is a multi-threaded object oriented implementation and I observe a CPU load factor\nof about 2.0. A separate node handles pose graph solver (it is in [github-repo](https://github.com/mpkuse/solve_keyframe_pose_graph) ).\n\nThis repo also support for saving the trajectories to file and later\nloading it from file for relocalization. See [launch/realsense_vinsfusion_ondrone_teach.launch](launch/realsense_vinsfusion_ondrone_teach.launch) on how to do it.\n\nIf you use this work in your research, please cite:\nManohar Kuse and Shaojie Shen, *“Learning Whole-Image Descriptors for Real-time Loop Detection and Kidnap Recovery under Large Viewpoint Difference“*, https://arxiv.org/abs/1904.06962\n\n## Highlight Video\n[![IMAGE ALT TEXT](http://img.youtube.com/vi/lDzDHZkInos/0.jpg)](http://www.youtube.com/watch?v=lDzDHZkInos \"Video Title\")\n\n\n## MyntEye Demo (Using VINS-Fusion as Odometry Estimator)\nWe showcase the kidnap recovery and relocalization.\n[![IMAGE ALT TEXT](http://img.youtube.com/vi/3YQF4_v7AEg/0.jpg)](http://www.youtube.com/watch?v=3YQF4_v7AEg \"Video Title\")\n\n\n[![IMAGE ALT TEXT](http://img.youtube.com/vi/sTd_rZdW4DQ/0.jpg)](http://www.youtube.com/watch?v=sTd_rZdW4DQ \"Video Title\")\n\n\n## In Plane Rotation Test (Uses Realsense Camera)\n[![IMAGE ALT TEXT](http://img.youtube.com/vi/8bsRCNF2rnA/0.jpg)](http://www.youtube.com/watch?v=8bsRCNF2rnA \"Video Title\")\n\n## Relocalization from Previously Constructed Trajectories.\nWe show the accurary of the system by plotting a previously constructed surfel map (in gray color)\nand the newly constructed surfel map (rainbox colored). We can observe that the boxes for example in the environment overlap. Similarly other objects in the seen also overlap. \n[![IMAGE ALT TEXT](http://img.youtube.com/vi/OViEEB3rINo/0.jpg)](http://www.youtube.com/watch?v=OViEEB3rINo \"Video Title\")\n\n## MyntEye Demo (Using VINS-Mono as Odometry Estimator)\n[![IMAGE ALT TEXT](http://img.youtube.com/vi/KDRo9LpL6Hs/0.jpg)](http://www.youtube.com/watch?v=KDRo9LpL6Hs \"Video Title\")\n\n[![IMAGE ALT TEXT](http://img.youtube.com/vi/XvoCrLFq99I/0.jpg)](http://www.youtube.com/watch?v=XvoCrLFq99I \"Video Title\")\n\n\n\n## EuRoC MAV Dataset live merge MH-01, ... MH-05.\n[![IMAGE ALT TEXT](http://img.youtube.com/vi/mnnoAlAIsN8/0.jpg)](http://www.youtube.com/watch?v=mnnoAlAIsN8 \"Video Title\")\n\n\n## EuRoC MAV Dataset live merge V1_01, V1_02, V1_03, V2_01, V2_02\n\n[![IMAGE ALT TEXT](http://img.youtube.com/vi/rIaANkd74cQ/0.jpg)](http://www.youtube.com/watch?v=rIaANkd74cQ \"Video Title\")\n\n\n\n\nFor more demonstration, have a look at my [youtube playlist](https://www.youtube.com/playlist?list=PLWyydx20vdPzs5VVhZu0TGsReT7U17Fxp)\n\n\n\n## Visual-Inertial Datasets\n- [ETHZ EuroC Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets)\n- [TUM Visual Inertial Dataset](https://vision.in.tum.de/data/datasets/visual-inertial-dataset)\n- UPenn - [Penncosyvio](https://github.com/daniilidis-group/penncosyvio)\n- [ADVIO](https://github.com/AaltoVision/ADVIO) ARKIT, Tango logging.\n- Our MyntEye (Stereo+IMU) [One-drive-link](https://hkustconnect-my.sharepoint.com/:f:/g/personal/mpkuse_connect_ust_hk/EkTisuLkXLFBs_WHYkxoH2oBeVIkdLc3-5a_t1J9c_4wkg?e=h9cifx)\n\n## How to run - Docker\nI highly recommend the already deployed packages with docker.\nRun the roscore on your host pc and all the packages run inside\nof docker container. rviz runs on the host pc.\n\nI assume you have a PC with a graphics card and cuda9 working smoothly\nand nvidia-docker installed.\n```\n$(host) export ROS_HOSTNAME=`hostname`\n$(host) roscore\n# assume that host has the ip address 172.17.0.1 in docker-network aka docker0\n$(host) docker run --runtime=nvidia -it --rm \\\n --add-host `hostname`:172.17.0.1 \\\n --env ROS_MASTER_URI=http://`hostname`:11311/ \\\n --env CUDA_VISIBLE_DEVICES=0 \\\n --hostname happy_go \\\n --name happy_go \\\n mpkuse/kusevisionkit:ros-kinetic-vins-tf-faiss bash\n$(host) rviz # inside rviz open config cerebro/config/good-viz.rviz. If you open rviz in a new tab you might need to do set ROS_HOSTNAME again.\n$(docker) roslaunch cerebro mynteye_vinsfusion.launch\n OR\n$(docker) roslaunch cerebro euroc_vinsfusion.launch\n$(host) rosbag play 1.bag\n```\n\n\n\nIf you are unfamiliar with docker, you may want to read [my blog post](https://kusemanohar.wordpress.com/2018/10/03/docker-for-computer-vision-researchers/)\non using docker for computer vision researchers.\nYou might want to have a look at my test ros-package to ensure things work with docker [docker_ros_test](https://github.com/mpkuse/docker_ros_test).\nList of all my docker-images can be found [here](https://hub.docker.com/r/mpkuse/kusevisionkit).\nOther docker images that work:\n- mpkuse/kusevisionkit:ros-kinetic-vins-tf-faiss (preferred)\n- mpkuse/kusevisionkit:vins-kidnap\n\n\n## How to compile (from scratch)\nYou will need a) VINS-Fusion (with modification for reset by mpkuse), b) cerebro\nand c) solve_keyframe_pose_graph. Besure to setup a `catkin_ws` and make sure\nyour ROS works correctly.\n\n### Dependencies\n- ROS Kinetic\n- Eigen3\n- Ceres\n- OpenCV3 (should also work with 2.4 (not tested), 3.3 (tested Ok) and 3.4 (tested ok))\n- [Theia-sfm](http://theia-sfm.org/). *As of May2019 dependence on theia-sfm has been eliminated. The code is able to compile even when you do not have Theia-sfm installed. The pose estimation PNP/P3P has been implemented from scratch using ceres. *\n - [OpenImageIO](https://github.com/OpenImageIO/oiio) (Release-1.7.6RC1)
\n - [RocksDB](https://github.com/facebook/rocksdb) (v5.9.2)
\n - OpenExr
\n- Tensorflow (tested on 1.11.0)\n- Keras (atleast 2.2.4), I noticed issues in jsonmodel with 2.2.2, 2.2.2 still works though!\n\n\n### Get VINS-Fusion Working\nI recommend you use my fork of VINS-Fusion, in which I have fixed some bugs\nand added mechanism for reseting the VINS.\n```\ncd catkin_ws/src\n#git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git\ngit clone https://github.com/mpkuse/VINS-Fusion\ncd ../\ncatkin_make\nsource catkin_ws/devel/setup.bash\n```\n\nMake sure your vins-fusion can compile and run correctly. See vins-fusion github repo\nfor the latest information on prerequisites and compilation instructions.\nFor compatibility I recommend using my fork of vins-mono/vins-fusion. Some minor\nmodifications have been made by me for working with kidnap cases.\n\n### Cerebro\n```\ncd catkin_ws/src/\ngit clone https://github.com/mpkuse/cerebro\ncd ../\ncatkin_make\n```\n\nThis has 2 exectables. **a)** ros server that takes as input an image and\nreturns a image descriptor. **b)** cerebro_node, this\nfinds the loop candidates and computes the relative poses. I have also\nincluded my trained model (about 4 MB) in this package (located scripts/keras.model). The pose computation\nuses the stereo pair in this node. This node publishes the loopcandidate's relative pose\nwhich is expected to be consumed the pose-graph solver.\n\nIf you wish to train your own model, you may use [my learning code here](https://github.com/mpkuse/cartwheel_train).\n\n**Threads in cerebro_node:**\n- *Main Thread* : ros-callbacks\n- *data_association_th* : Sync the incoming image data and incoming data vins_estimator.\n- *desc_th* : Consumes the images to produce whole-image-descriptors.\n- *dot_product_th* : Dot product of current image descriptor with all the previous ones.\n- *loopcandidate_consumer_th* : Computes the relative pose at the loopcandidates. Publishes the loopEdge.\n- *kidnap_th* : Identifies kidnap. If kidnap publishes the reset signals for vins_estimator.\n- *viz_th* : Publishes the image-pair, and more things for debugging and analysis.\n- *dm_cleanup_th* : Deallocate/Store to file images to reduce RAM consumption.\n\n\n**Nvidia TX2**: Often times for live run, you might want to run the\nros-server on a Nvidia-TX2. I recommend compiling tensorflow from scratch.\nThe thing is prebuilt binaries may not be compatible with the version\nof CUDA and CUDNN on your device. Also some binaries may not be\ncompatible to arm (could likely be built for x86). Before you can\ncompile tensorflow you need java, bazel. See this [gist](https://gist.github.com/vellamike/7c26158c93e89ef155c1cc953bbba956). Also tools and repos from [jetsonhacks](https://github.com/jetsonhacks)\nmight come in handy.\n\n### Pose Graph Solver\nUse my pose graph solver, [github-repo](https://github.com/mpkuse/solve_keyframe_pose_graph).\nThe differences between this implementation and the\noriginal from VINS-Fusion is that mine can handle kidnap cases,\nhandles multiple world co-ordinate frames and it\nuses a switch-constraint formulation of the pose-graph problem.\n```\ncd catkin_ws/src/\ngit clone https://github.com/mpkuse/solve_keyframe_pose_graph\ncd ../\ncatkin_make\n```\n\n**Threads:**\n- *Main thread* : ros-callbacks for odometry poses (from vins_estimator) and LoopMsg (from cerebro).\n- *SLAM* : Monitors the node-poses and loop-edges, on new loop-edges constructs and solves the pose-graph optimization problem.\n- *th4* : Publish latest odometry.\n- *th5* : Display image to visualize disjoint set datastructure.\n- *th6* : Publish corrected poses, Different color for nodes in different co-ordinate systems.\n\n\n### vins_mono_debug_pkg (optional, needed only if you wish to debug vins-mono/vins-fusion)\nWith cerebro node it is possible to live run the vins and make it log all the\ndetails to file for further analysis/debugging. This might be useful\nfor researchers and other Ph.D. student to help VINS-Fusion improve further.\nsee [github/mpkuse/vins_mono](https://github.com/mpkuse/vins_mono_debug_pkg).\nIt basically contains unit tests and some standalone tools which might come in handy.\nIf you are looking to help improve VINS-fusion or cerebro also look at 'Development Guidelines'.\n\n## How to run the Full System\n```\nroslaunch cerebro mynteye_vinsfusion.launch\n```\nYou can get some of my bag files collected with the mynteye camera HERE. More\nexample launch files in folder `launch`, all the config files which contains\ncalibration info are in folder `config`.\n\n\n## Development Guidelines\nIf you are developing I still recommend using docker. with -v flags in docker you could mount your\npc's folders on the docker. I recommend keeping all the packages in folder `docker_ws_slam/catkin_ws/src`\non your host pc. And all the rosbags in folder `/media/mpkuse/Bulk_Data`. And then mount these\ntwo folders on the docker-container. Edit the following command as needed.\n\n\n```\ndocker run --runtime=nvidia -it -v /media/mpkuse/Bulk_Data/:/Bulk_Data -v /home/mpkuse/docker_ws_slam:/app --add-host `hostname`:172.17.0.1 --env ROS_MASTER_URI=http://`hostname`:11311/ --env CUDA_VISIBLE_DEVICES=0 --hostname happy_go --name happy_go mpkuse/kusevisionkit:ros-kinetic-vins bash\n```\n\n\nUse the following if you wish to use xhost(gui) from inside docker\n```\n$(host) xhost +local:root\n$(host)\ndocker run --runtime=nvidia -it -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix -v /media/mpkuse/Bulk_Data/:/Bulk_Data -v /home/mpkuse/docker_ws_slam:/app --add-host `hostname`:172.17.0.1 --env ROS_MASTER_URI=http://`hostname`:11311/ --env CUDA_VISIBLE_DEVICES=0 --hostname happy_go --name happy_go mpkuse/kusevisionkit:ros-kinetic-vins bash\n```\n\nOther docker images that work:\n- mpkuse/kusevisionkit:ros-kinetic-vins-tf-faiss (preferred)\n- mpkuse/kusevisionkit:vins-kidnap\n\nEach of my classes can export the data they hold as json objects and image files. Look at the\nend of `main()` in `cerebro_node.cpp` and modify as needed to extract more debug data. Similarly\nthe pose graph solver can also be debugged.\nIf writing a lot of debug data (usually taking more than 10sec) roslaunch force-kills the process.\nTo get around this issue, you need to increase the timeout:\n\n```\nFor kinetic this can be found in:\n/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch\nIn the file nodeprocess.py there will be 2 variables on line 57 and 58.\n\n_TIMEOUT_SIGINT = 15.0 #seconds\n\n_TIMEOUT_SIGTERM = 2.0 #seconds\n```\n\n\nFor streamlining printing messages\nI have preprocessor macros at the start of function implementation (of classes DataManager and Cerebro),\nread the comments there and edit as per need. Try to implement your algorithms in\nobject oriented way and using the producer-consumer paradigm. Look at my thread-mains for example. \n\nFinally, sensible PR with bug fixes, enhancements are welcome!\n![](doc/rosgraph.png)\n\n## Authors\nManohar Kuse \n" }, { "path": "config/echo__terminator_config_vins_fusion_cerebro", "content": "[global_config]\n[keybindings]\n[layouts]\n [[default]]\n [[[child0]]]\n fullscreen = False\n last_active_term = 1cc3df64-f997-4766-9317-0ce75d5dc2cd\n last_active_window = True\n maximised = True\n order = 0\n parent = \"\"\n position = 65:24\n size = 1855, 1056\n title = dji@MANIFOLD-2-C: ~\n type = Window\n [[[child1]]]\n order = 0\n parent = child0\n position = 525\n ratio = 0.5\n type = VPaned\n [[[child2]]]\n order = 0\n parent = child1\n position = 513\n ratio = 0.278167115903\n type = HPaned\n [[[child4]]]\n order = 1\n parent = child2\n position = 648\n ratio = 0.487275449102\n type = HPaned\n [[[child7]]]\n order = 1\n parent = child1\n position = 513\n ratio = 0.278167115903\n type = HPaned\n [[[child9]]]\n order = 1\n parent = child7\n position = 644\n ratio = 0.484281437126\n type = HPaned\n [[[terminal10]]]\n command = echo \"roslaunch cerebro realsense_vinsfusion_ondrone.launch\"; echo \"roslaunch cerebro mynteye_vinsfusion_ondrone.launch\"; bash\n order = 0\n parent = child9\n profile = default\n type = Terminal\n uuid = 8fc7a21c-3feb-468d-aae5-c363bc5c9f80\n [[[terminal11]]]\n command = echo \"roslaunch cerebro realsense_camera.launch\"; echo \"roslaunch mynt_eye_ros_wrapper mynteye.launch\"; echo \"rosbag play \"; bash\n order = 1\n parent = child9\n profile = default\n type = Terminal\n uuid = a63545ab-f3c1-4426-a8ef-173f57ca8f28\n [[[terminal3]]]\n command = echo \"roscore\"; bash\n order = 0\n parent = child2\n profile = default\n type = Terminal\n uuid = 27ae967a-b56c-4d92-a56e-67a2abe6b22d\n [[[terminal5]]]\n command = '''echo \"ssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks'\"; echo \"\"; echo \"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch tx2_whole_image_desc_server py3_tf_server_realsense.launch'\" ; echo \"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch tx2_whole_image_desc_server py3_tf_server_mynteye.launch'\"; bash'''\n order = 0\n parent = child4\n profile = default\n type = Terminal\n uuid = 6184900a-1468-4ffa-96a0-b1c00563ead7\n [[[terminal6]]]\n command = '''echo \"ssh dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && rosrun solve_keyframe_pose_graph keyframe_pose_graph_slam' \"; bash'''\n order = 1\n parent = child4\n profile = default\n type = Terminal\n uuid = 09e32ec9-5e11-4bb0-980d-4c1abeb77791\n [[[terminal8]]]\n command = echo \"rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz\"; bash\n order = 0\n parent = child7\n profile = default\n type = Terminal\n uuid = 1cc3df64-f997-4766-9317-0ce75d5dc2cd\n [[vins-cerebro]]\n [[[child0]]]\n fullscreen = False\n last_active_term = b015529f-251d-4563-81f8-86f2d24badbe\n last_active_window = True\n maximised = False\n order = 0\n parent = \"\"\n position = 289:106\n size = 642, 378\n title = dji@MANIFOLD-2-C: ~\n type = Window\n [[[terminal1]]]\n order = 0\n parent = child0\n profile = vins-cerebro\n type = Terminal\n uuid = b015529f-251d-4563-81f8-86f2d24badbe\n[plugins]\n[profiles]\n [[default]]\n background_image = None\n scrollback_lines = 5000\n [[vins-cerebro]]\n background_image = None\n" }, { "path": "config/euroc/euroc_config_no_extrinsic.yaml", "content": "%YAML:1.0\n\n#common parameters\nimu_topic: \"/imu0\"\nimage_topic: \"/cam0/image_raw\"\noutput_path: \"/home/tony-ws1/output/\"\n\n#camera calibration \nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n k1: -2.917e-01\n k2: 8.228e-02\n p1: 5.333e-05\n p2: -1.578e-04\nprojection_parameters:\n fx: 4.616e+02\n fy: 4.603e+02\n cx: 3.630e+02\n cy: 2.481e+02\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. \n#If you choose 0 or 1, you should write down the following matrix.\n\n#feature traker paprameters\nmax_cnt: 150 # max feature number in feature tracking\nmin_dist: 30 # min distance between two features \nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image \nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 1 # publish tracking image as topic\nequalize: 1 # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2\ngyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05\nacc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02\ngyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5\ng_norm: 9.81007 # gravity magnitude\n\n#loop closure parameters\nloop_closure: 1 # start loop closure\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\nfast_relocalization: 0 # useful in real-time and large project\npose_graph_save_path: \"/home/tony-ws1/output/pose_graph/\" # save and load path\n\n#unsynchronization parameters\nestimate_td: 0 # online estimate time offset between camera and imu\ntd: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#rolling shutter parameters\nrolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera\nrolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). \n\n#visualization parameters\nsave_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 \nvisualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results\nvisualize_camera_size: 0.4 # size of camera marker in RVIZ" }, { "path": "config/fly", "content": "[global_config]\n[keybindings]\n[layouts]\n [[default]]\n [[[child0]]]\n fullscreen = False\n last_active_term = 1cc3df64-f997-4766-9317-0ce75d5dc2cd\n last_active_window = True\n maximised = True\n order = 0\n parent = \"\"\n position = 65:24\n size = 1855, 1056\n title = dji@MANIFOLD-2-C: ~\n type = Window\n [[[child1]]]\n order = 0\n parent = child0\n position = 525\n ratio = 0.5\n type = VPaned\n [[[child2]]]\n order = 0\n parent = child1\n position = 513\n ratio = 0.278167115903\n type = HPaned\n [[[child4]]]\n order = 1\n parent = child2\n position = 648\n ratio = 0.487275449102\n type = HPaned\n [[[child7]]]\n order = 1\n parent = child1\n position = 513\n ratio = 0.278167115903\n type = HPaned\n [[[child9]]]\n order = 1\n parent = child7\n position = 644\n ratio = 0.484281437126\n type = HPaned\n [[[terminal10]]]\n command = echo \"roslaunch cerebro realsense_vinsfusion.launch\"; sleep 15s; roslaunch --wait cerebro realsense_vinsfusion.launch; bash\n order = 0\n parent = child9\n profile = default\n type = Terminal\n uuid = 8fc7a21c-3feb-468d-aae5-c363bc5c9f80\n [[[terminal11]]]\n command = echo \"roslaunch cerebro realsense_camera.launch\"; sleep 5s; roslaunch --wait cerebro realsense_camera.launch; bash\n order = 1\n parent = child9\n profile = default\n type = Terminal\n uuid = a63545ab-f3c1-4426-a8ef-173f57ca8f28\n [[[terminal3]]]\n command = echo \"roscore\"; roscore; bash\n order = 0\n parent = child2\n profile = default\n type = Terminal\n uuid = 27ae967a-b56c-4d92-a56e-67a2abe6b22d\n [[[terminal5]]]\n #command = '''echo \"ssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks'\"; echo \"\"; echo \"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch tx2_whole_image_desc_server py3_tf_server_realsense.launch'\" ; echo \"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch tx2_whole_image_desc_server py3_tf_server_mynteye.launch'\"; bash'''\n command = ''' coproc rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz ; ssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks'; ssh -t dji@dji-tx2 \"export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; export DJIROS_APPID='1014582' ; export DJIROS_ENCKEY='821c8c3abeb6e340637b01f908ddeefb6c76d18d5f289e5cdb3cd5756a17bef5'; source $HOME/catkin_ws/devel/setup.bash && roslaunch djiros djiros.launch\" ; bash '''\n order = 0\n parent = child4\n profile = default\n type = Terminal\n uuid = 6184900a-1468-4ffa-96a0-b1c00563ead7\n [[[terminal6]]]\n #command = '''echo \"ssh dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && rosrun solve_keyframe_pose_graph keyframe_pose_graph_slam' \"; bash'''\n command = \" sleep 3s; echo 'rosrun trajectory_node trajectory_node' ; export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; rosrun trajectory_node trajectory_node ; bash \"\n order = 1\n parent = child4\n profile = default\n type = Terminal\n uuid = 09e32ec9-5e11-4bb0-980d-4c1abeb77791\n [[[terminal8]]]\n command = echo \"roslaunch --wait machine_defined ctrl_md.launch\"; roslaunch --wait machine_defined ctrl_md.launch ; bash\n order = 0\n parent = child7\n profile = default\n type = Terminal\n uuid = 1cc3df64-f997-4766-9317-0ce75d5dc2cd\n [[vins-cerebro]]\n [[[child0]]]\n fullscreen = False\n last_active_term = b015529f-251d-4563-81f8-86f2d24badbe\n last_active_window = True\n maximised = False\n order = 0\n parent = \"\"\n position = 289:106\n size = 642, 378\n title = dji@MANIFOLD-2-C: ~\n type = Window\n [[[terminal1]]]\n order = 0\n parent = child0\n profile = vins-cerebro\n type = Terminal\n uuid = b015529f-251d-4563-81f8-86f2d24badbe\n[plugins]\n[profiles]\n [[default]]\n background_image = None\n scrollback_lines = 5000\n [[vins-cerebro]]\n background_image = None\n" }, { "path": "config/mynteye/camera_left.yaml", "content": "%YAML:1.0\n---\nmodel_type: MEI\ncamera_name: camera_left\nimage_width: 752\nimage_height: 480\nmirror_parameters:\n xi: 9.1080215802208964e-01\ndistortion_parameters:\n k1: -4.1813210333143819e-01\n k2: 1.7403431210939191e-01\n p1: 9.9413203823357906e-04\n p2: 7.3177696678596699e-04\nprojection_parameters:\n gamma1: 7.0261362770226128e+02\n gamma2: 7.0163099053315807e+02\n u0: 3.7079141993914720e+02\n v0: 2.3833755987148535e+02\n" }, { "path": "config/mynteye/camera_right.yaml", "content": "%YAML:1.0\n---\nmodel_type: MEI\ncamera_name: camera_right\nimage_width: 752\nimage_height: 480\nmirror_parameters:\n xi: 1.0430682060403200e+00\ndistortion_parameters:\n k1: -4.1052639675713765e-01\n k2: 1.5594870262700564e-01\n p1: 1.0690635619397511e-03\n p2: -1.2541037785533519e-04\nprojection_parameters:\n gamma1: 7.5079430544256968e+02\n gamma2: 7.5002280082558298e+02\n u0: 3.6187005259990548e+02\n v0: 2.4784480598161096e+02\n" }, { "path": "config/mynteye/extrinsics.yaml", "content": "%YAML:1.0\n---\ntransform:\n q_x: -1.8252509868889259e-04\n q_y: -1.6291774489779708e-03\n q_z: -1.2462127842978489e-03\n q_w: 9.9999787970731446e-01\n t_x: -1.2075905420832895e+02\n t_y: 5.4110610639412482e-01\n t_z: 2.4484815673909591e-01\n" }, { "path": "config/mynteye/mynteye_config.yaml", "content": "%YAML:1.0\n\n#common parameters\nimu_topic: \"/mynteye/imu/data_raw\"\nimage_topic: \"/mynteye/left/image_raw\"\noutput_path: \"/Bulk_Data/vins-mono-output/\"\n\n# Additional camera (stereo) for cerebro - added my mpkuse\nimage_topic_1: \"/mynteye/right/image_raw\"\ncamera_yaml_1: \"camera_right.yaml\" # this yaml file will be searched in directory where this yaml file is.\nextrinsic_1_T_0: \"extrinsics.yaml\" # TODO contains the right_T_left, ie. stereo baseline. I need right_T_left that a transform that transforms 3d points of left to 3d points of right. in other words, if you sir of right the position of left camera.\n\n#camera calibration\nmodel_type: MEI\ncamera_name: camera_left\nimage_width: 752\nimage_height: 480\nmirror_parameters:\n xi: 9.1080215802208964e-01\ndistortion_parameters:\n k1: -4.1813210333143819e-01\n k2: 1.7403431210939191e-01\n p1: 9.9413203823357906e-04\n p2: 7.3177696678596699e-04\nprojection_parameters:\n gamma1: 7.0261362770226128e+02\n gamma2: 7.0163099053315807e+02\n u0: 3.7079141993914720e+02\n v0: 2.3833755987148535e+02\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.\n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ 4.2739206953025244e-03, -9.9997227378122833e-01,\n -6.0979726705474944e-03, 9.9998919890819160e-01,\n 4.2626966744552242e-03, 1.8524265207998580e-03,\n -1.8263813521932205e-03, -6.1058239298286089e-03,\n 9.9997969141642784e-01 ]\nextrinsicTranslation: !!opencv-matrix\n rows: 3\n cols: 1\n dt: d\n data: [ 6.1040452082857912e-03, -4.8408978130397302e-02,\n 2.2594515206886469e-02 ]\n\n#feature traker paprameters\nmax_cnt: 150 # max feature number in feature tracking\nmin_dist: 25 # min distance between two features\nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 1 # publish tracking image as topic\nequalize: 1 # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04\ngyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004\nacc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02\ngyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5\ng_norm: 9.80766 # gravity magnitude\n\n#loop closure parameters\nloop_closure: 0 # start loop closure\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\nfast_relocalization: 0 # useful in real-time and large project\npose_graph_save_path: \"/home/tony-ws1/output/pose_graph/\" # save and load path\n\n#unsynchronization parameters\nestimate_td: 0 # online estimate time offset between camera and imu\ntd: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#rolling shutter parameters\nrolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera\nrolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).\n\n#visualization parameters\nsave_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0\nvisualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results\nvisualize_camera_size: 0.4 # size of camera marker in RVIZ\n" }, { "path": "config/mynteye__terminator_config_vins_fusion_cerebro", "content": "[global_config]\n[keybindings]\n[layouts]\n [[default]]\n [[[child0]]]\n fullscreen = False\n last_active_term = 1cc3df64-f997-4766-9317-0ce75d5dc2cd\n last_active_window = True\n maximised = True\n order = 0\n parent = \"\"\n position = 65:24\n size = 1855, 1056\n title = dji@MANIFOLD-2-C: ~\n type = Window\n [[[child1]]]\n order = 0\n parent = child0\n position = 525\n ratio = 0.5\n type = VPaned\n [[[child2]]]\n order = 0\n parent = child1\n position = 513\n ratio = 0.278167115903\n type = HPaned\n [[[child4]]]\n order = 1\n parent = child2\n position = 648\n ratio = 0.487275449102\n type = HPaned\n [[[child7]]]\n order = 1\n parent = child1\n position = 513\n ratio = 0.278167115903\n type = HPaned\n [[[child9]]]\n order = 1\n parent = child7\n position = 644\n ratio = 0.484281437126\n type = HPaned\n [[[terminal10]]]\n command = '''echo \"roslaunch cerebro realsense_vinsfusion_ondrone.launch\";\n\t\techo \"roslaunch cerebro mynteye_vinsfusion_ondrone.launch\";\n export KUSE_CMD=\"roslaunch cerebro mynteye_vinsfusion_ondrone.launch\";\n\t\troslaunch --wait cerebro mynteye_vinsfusion_ondrone.launch;\n\t\tbash'''\n order = 0\n parent = child9\n profile = default\n type = Terminal\n uuid = 8fc7a21c-3feb-468d-aae5-c363bc5c9f80\n [[[terminal11]]]\n command = '''#echo \"roslaunch cerebro realsense_camera.launch\";\n\t\techo \"roslaunch mynt_eye_ros_wrapper mynteye.launch\";\n\t\techo \"rosbag play \";\n export KUSE_CMD=\"roslaunch mynt_eye_ros_wrapper mynteye.launch or rosbag play\"\n bash'''\n order = 1\n parent = child9\n profile = default\n type = Terminal\n uuid = a63545ab-f3c1-4426-a8ef-173f57ca8f28\n [[[terminal3]]]\n command = echo \"roscore\"; exec \"roscore\"; bash\n order = 0\n parent = child2\n profile = default\n type = Terminal\n uuid = 27ae967a-b56c-4d92-a56e-67a2abe6b22d\n [[[terminal5]]]\n command = '''echo \"ssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks'\";\n\t\t\tssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks';\n\t\t\techo \"\";\n\t\t\t#echo \"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch --wait tx2_whole_image_desc_server py3_tf_server_realsense.launch'\";\n\t\t\techo \"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch --wait tx2_whole_image_desc_server py3_tf_server_mynteye.launch'\";\n export KUSE_CMD=\"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch --wait tx2_whole_image_desc_server py3_tf_server_mynteye.launch'\";\n\t\t\tssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch --wait tx2_whole_image_desc_server py3_tf_server_mynteye.launch'\n\t\t\tbash'''\n order = 0\n parent = child4\n profile = default\n type = Terminal\n uuid = 6184900a-1468-4ffa-96a0-b1c00563ead7\n [[[terminal6]]]\n command = '''echo \"ssh dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && rosrun solve_keyframe_pose_graph keyframe_pose_graph_slam'\";\n \texport KUSE_CMD=\"ssh dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && rosrun solve_keyframe_pose_graph keyframe_pose_graph_slam'\";\n\t\tssh dji@dji-tx2 \"export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && rosrun solve_keyframe_pose_graph keyframe_pose_graph_slam\";\n\t\tbash'''\n order = 1\n parent = child4\n profile = default\n type = Terminal\n uuid = 09e32ec9-5e11-4bb0-980d-4c1abeb77791\n [[[terminal8]]]\n command = '''echo \"rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz\";\n\t\trviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz\n\t\tbash'''\n order = 0\n parent = child7\n profile = default\n type = Terminal\n uuid = 1cc3df64-f997-4766-9317-0ce75d5dc2cd\n [[vins-cerebro]]\n [[[child0]]]\n fullscreen = False\n last_active_term = b015529f-251d-4563-81f8-86f2d24badbe\n last_active_window = True\n maximised = False\n order = 0\n parent = \"\"\n position = 289:106\n size = 642, 378\n title = dji@MANIFOLD-2-C: ~\n type = Window\n [[[terminal1]]]\n order = 0\n parent = child0\n profile = vins-cerebro\n type = Terminal\n uuid = b015529f-251d-4563-81f8-86f2d24badbe\n[plugins]\n[profiles]\n [[default]]\n background_image = None\n scrollback_lines = 5000\n [[vins-cerebro]]\n background_image = None\n" }, { "path": "config/mynteye_kannala_brandt/camera_left.yaml", "content": "%YAML:1.0\n---\nmodel_type: KANNALA_BRANDT\ncamera_name: camera_left\nimage_width: 752\nimage_height: 480\nprojection_parameters:\n k2: -1.9372136316513616e-02\n k3: -9.8140934466631590e-03\n k4: 5.2090950264238800e-03\n k5: -2.5384805224148016e-03\n mu: 3.6779833417894611e+02\n mv: 3.6735717027889308e+02\n u0: 3.7150938938370604e+02\n v0: 2.3994369620747401e+02\n" }, { "path": "config/mynteye_kannala_brandt/camera_right.yaml", "content": "%YAML:1.0\n---\nmodel_type: KANNALA_BRANDT\ncamera_name: camera_right\nimage_width: 752\nimage_height: 480\nprojection_parameters:\n k2: -1.6877708507279571e-02\n k3: -2.2125250367976849e-02\n k4: 2.5629804328110812e-02\n k5: -1.3352680211095494e-02\n mu: 3.6746513955323167e+02\n mv: 3.6703089274525593e+02\n u0: 3.6200946050656484e+02\n v0: 2.4919721516867821e+02\n" }, { "path": "config/mynteye_kannala_brandt/extrinsics.yaml", "content": "%YAML:1.0\n---\ntransform:\n q_x: -7.0955103716253032e-04\n q_y: -1.5775578725333590e-03\n q_z: -1.2732644461763854e-03\n q_w: 9.9999769332040711e-01\n t_x: -1.2006984141573309e+02\n t_y: 3.3956264524978619e-01\n t_z: -1.6784055634087214e-01\n" }, { "path": "config/mynteye_kannala_brandt/mynteye_config.yaml", "content": "%YAML:1.0\n\n#common parameters\nimu_topic: \"/mynteye/imu/data_raw\"\nimage_topic: \"/mynteye/left/image_raw\"\noutput_path: \"/Bulk_Data/vins-mono-output/\"\n\n# Additional camera (stereo) for cerebro - added my mpkuse\nimage_topic_1: \"/mynteye/right/image_raw\"\ncamera_yaml_1: \"camera_right.yaml\" # this yaml file will be searched in directory where this yaml file is.\nextrinsic_1_T_0: \"extrinsics.yaml\" # contains the right_T_left, ie. stereo baseline. I need right_T_left that a transform that transforms 3d points of left to 3d points of right. in other words, if you sir of right the position of left camera.\n\n# publish image pair on loopcandidate detection\n\n\n#camera calibration\nmodel_type: KANNALA_BRANDT\ncamera_name: camera_left\nimage_width: 752\nimage_height: 480\nprojection_parameters:\n k2: -1.9372136316513616e-02\n k3: -9.8140934466631590e-03\n k4: 5.2090950264238800e-03\n k5: -2.5384805224148016e-03\n mu: 3.6779833417894611e+02\n mv: 3.6735717027889308e+02\n u0: 3.7150938938370604e+02\n v0: 2.3994369620747401e+02\n\n\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.\n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ 4.2739206953025244e-03, -9.9997227378122833e-01,\n -6.0979726705474944e-03, 9.9998919890819160e-01,\n 4.2626966744552242e-03, 1.8524265207998580e-03,\n -1.8263813521932205e-03, -6.1058239298286089e-03,\n 9.9997969141642784e-01 ]\nextrinsicTranslation: !!opencv-matrix\n rows: 3\n cols: 1\n dt: d\n data: [ 6.1040452082857912e-03, -4.8408978130397302e-02,\n 2.2594515206886469e-02 ]\n\n#feature traker paprameters\nmax_cnt: 150 # max feature number in feature tracking\nmin_dist: 25 # min distance between two features\nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 1 # publish tracking image as topic\nequalize: 1 # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 10 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04\ngyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004\nacc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02\ngyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5\ng_norm: 9.80766 # gravity magnitude\n\n#loop closure parameters\nloop_closure: 1 # start loop closure\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\nfast_relocalization: 1 # useful in real-time and large project\npose_graph_save_path: \"/Bulk_Data/vins-mono-output/pose_graph/\" # save and load path\n\n#unsynchronization parameters\nestimate_td: 0 # online estimate time offset between camera and imu\ntd: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#rolling shutter parameters\nrolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera\nrolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).\n\n#visualization parameters\nsave_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0\nvisualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results\nvisualize_camera_size: 0.4 # size of camera marker in RVIZ\n" }, { "path": "config/mynteye_pinhole/camera_left.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera_left\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n k1: -3.0304011877067349e-01\n k2: 7.8611376314970768e-02\n p1: 1.3200427858719645e-03\n p2: -1.2942361409192899e-03\nprojection_parameters:\n fx: 3.7560167589977470e+02\n fy: 3.7552589588133230e+02\n cx: 3.7887177487131697e+02\n cy: 2.3402525474822104e+02\n" }, { "path": "config/mynteye_pinhole/camera_right.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera_right\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n k1: -3.0790744874410708e-01\n k2: 8.3037204977051693e-02\n p1: 1.1347651021695062e-03\n p2: -7.8671202943393290e-04\nprojection_parameters:\n fx: 3.7573448135250288e+02\n fy: 3.7536464618331070e+02\n cx: 3.6634215933104940e+02\n cy: 2.4325470902936044e+02\n" }, { "path": "config/mynteye_pinhole/extrinsics.yaml", "content": "%YAML:1.0\n---\ntransform:\n q_x: -7.8278125678242136e-04\n q_y: 2.4666921736014214e-03\n q_z: -1.4477856356187120e-03\n q_w: 9.9999560329032322e-01\n t_x: -1.1985630585227842e+02\n t_y: 1.2186198275182303e-01\n t_z: -1.4431533628123798e+00\n" }, { "path": "config/mynteye_pinhole/mynteye_config.yaml", "content": "%YAML:1.0\n\n#common parameters\nimu_topic: \"/mynteye/imu/data_raw\"\nimage_topic: \"/mynteye/left/image_raw\"\noutput_path: \"/Bulk_Data/vins-mono-output/\"\n\n# Additional camera (stereo) for cerebro - added my mpkuse\nimage_topic_1: \"/mynteye/right/image_raw\"\ncamera_yaml_1: \"camera_right.yaml\" # this yaml file will be searched in directory where this yaml file is.\nextrinsic_1_T_0: \"extrinsics.yaml\" # contains the right_T_left, ie. stereo baseline. I need right_T_left that a transform that transforms 3d points of left to 3d points of right. in other words, if you sir of right the position of left camera.\n\n#camera calibration\nmodel_type: PINHOLE\ncamera_name: camera_left\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n k1: -3.0304011877067349e-01\n k2: 7.8611376314970768e-02\n p1: 1.3200427858719645e-03\n p2: -1.2942361409192899e-03\nprojection_parameters:\n fx: 3.7560167589977470e+02\n fy: 3.7552589588133230e+02\n cx: 3.7887177487131697e+02\n cy: 2.3402525474822104e+02\n\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.\n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ 4.2739206953025244e-03, -9.9997227378122833e-01,\n -6.0979726705474944e-03, 9.9998919890819160e-01,\n 4.2626966744552242e-03, 1.8524265207998580e-03,\n -1.8263813521932205e-03, -6.1058239298286089e-03,\n 9.9997969141642784e-01 ]\nextrinsicTranslation: !!opencv-matrix\n rows: 3\n cols: 1\n dt: d\n data: [ 6.1040452082857912e-03, -4.8408978130397302e-02,\n 2.2594515206886469e-02 ]\n\n#feature traker paprameters\nmax_cnt: 130 # max feature number in feature tracking\nmin_dist: 25 # min distance between two features\nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 1 # publish tracking image as topic\nequalize: 1 # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04\ngyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004\nacc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02\ngyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5\ng_norm: 9.80766 # gravity magnitude\n\n#loop closure parameters\nloop_closure: 0 # start loop closure\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\nfast_relocalization: 0 # useful in real-time and large project\npose_graph_save_path: \"/home/tony-ws1/output/pose_graph/\" # save and load path\n\n#unsynchronization parameters\nestimate_td: 0 # online estimate time offset between camera and imu\ntd: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#rolling shutter parameters\nrolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera\nrolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).\n\n#visualization parameters\nsave_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0\nvisualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results\nvisualize_camera_size: 0.4 # size of camera marker in RVIZ\n" }, { "path": "config/realsense__terminator_config_vins_fusion_cerebro", "content": "[global_config]\n[keybindings]\n[layouts]\n [[default]]\n [[[child0]]]\n fullscreen = False\n last_active_term = 1cc3df64-f997-4766-9317-0ce75d5dc2cd\n last_active_window = True\n maximised = True\n order = 0\n parent = \"\"\n position = 65:24\n size = 1855, 1056\n title = dji@MANIFOLD-2-C: ~\n type = Window\n [[[child1]]]\n order = 0\n parent = child0\n position = 525\n ratio = 0.5\n type = VPaned\n [[[child2]]]\n order = 0\n parent = child1\n position = 513\n ratio = 0.278167115903\n type = HPaned\n [[[child4]]]\n order = 1\n parent = child2\n position = 648\n ratio = 0.487275449102\n type = HPaned\n [[[child7]]]\n order = 1\n parent = child1\n position = 513\n ratio = 0.278167115903\n type = HPaned\n [[[child9]]]\n order = 1\n parent = child7\n position = 644\n ratio = 0.484281437126\n type = HPaned\n [[[terminal10]]]\n command = '''echo \"roslaunch cerebro realsense_vinsfusion_ondrone.launch\";\n\t\troslaunch --wait cerebro realsense_vinsfusion_ondrone.launch\n\t\t#echo \"roslaunch cerebro mynteye_vinsfusion_ondrone.launch\";\n\t\t#roslaunch --wait cerebro mynteye_vinsfusion_ondrone.launch;\n\t\tbash'''\n order = 0\n parent = child9\n profile = default\n type = Terminal\n uuid = 8fc7a21c-3feb-468d-aae5-c363bc5c9f80\n [[[terminal11]]]\n command = '''echo \"roslaunch cerebro realsense_camera.launch\";\n\t\t#echo \"roslaunch mynt_eye_ros_wrapper mynteye.launch\";\n\t\techo \"rosbag play \"; bash'''\n order = 1\n parent = child9\n profile = default\n type = Terminal\n uuid = a63545ab-f3c1-4426-a8ef-173f57ca8f28\n [[[terminal3]]]\n command = echo \"roscore\"; exec \"roscore\"; bash\n order = 0\n parent = child2\n profile = default\n type = Terminal\n uuid = 27ae967a-b56c-4d92-a56e-67a2abe6b22d\n [[[terminal5]]]\n command = '''echo \"ssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks'\";\n\t\t\tssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks';\n\t\t\techo \"\";\n\t\t\t#echo \"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch --wait tx2_whole_image_desc_server py3_tf_server_realsense.launch'\";\n\t\t\tssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch --wait tx2_whole_image_desc_server py3_tf_server_realsense.launch'\n\t\t\t#echo \"ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch --wait tx2_whole_image_desc_server py3_tf_server_mynteye.launch'\";\n\t\t\t#ssh -t dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && roslaunch --wait tx2_whole_image_desc_server py3_tf_server_mynteye.launch'\n\t\t\tbash'''\n order = 0\n parent = child4\n profile = default\n type = Terminal\n uuid = 6184900a-1468-4ffa-96a0-b1c00563ead7\n [[[terminal6]]]\n command = '''echo \"ssh dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && rosrun solve_keyframe_pose_graph keyframe_pose_graph_slam'\";\n\t\tssh dji@dji-tx2 'export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; source $HOME/catkin_ws/devel/setup.bash && rosrun solve_keyframe_pose_graph keyframe_pose_graph_slam';\n\t\tbash'''\n order = 1\n parent = child4\n profile = default\n type = Terminal\n uuid = 09e32ec9-5e11-4bb0-980d-4c1abeb77791\n [[[terminal8]]]\n command = '''echo \"rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz\";\n\t\trviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz\n\t\tbash'''\n order = 0\n parent = child7\n profile = default\n type = Terminal\n uuid = 1cc3df64-f997-4766-9317-0ce75d5dc2cd\n [[vins-cerebro]]\n [[[child0]]]\n fullscreen = False\n last_active_term = b015529f-251d-4563-81f8-86f2d24badbe\n last_active_window = True\n maximised = False\n order = 0\n parent = \"\"\n position = 289:106\n size = 642, 378\n title = dji@MANIFOLD-2-C: ~\n type = Window\n [[[terminal1]]]\n order = 0\n parent = child0\n profile = vins-cerebro\n type = Terminal\n uuid = b015529f-251d-4563-81f8-86f2d24badbe\n[plugins]\n[profiles]\n [[default]]\n background_image = None\n scrollback_lines = 5000\n [[vins-cerebro]]\n background_image = None\n" }, { "path": "config/vinsfusion/djirosimu_realsense_d435i/README.md", "content": "The configuration when using realsense stereo camera and djiros's imu (ie. imu data from N3)\n" }, { "path": "config/vinsfusion/djirosimu_realsense_d435i/extrinsics.yaml", "content": "%YAML:1.0\n---\ntransform:\n q_x: 0.\n q_y: 0.\n q_z: 0.\n q_w: 1.\n t_x: -49.8\n t_y: 0.\n t_z: 0.\n#4.98 cm for the realsense d435i device. This device has hardware undistort so can safely use the pinhole model \n" }, { "path": "config/vinsfusion/djirosimu_realsense_d435i/left.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 640\nimage_height: 480\ndistortion_parameters:\n k1: 0.0\n k2: 0.0\n p1: 0.0\n p2: 0.0\nprojection_parameters:\n fx: 385.7544860839844\n fy: 385.7544860839844\n cx: 323.1204833984375\n cy: 236.7432098388672\n" }, { "path": "config/vinsfusion/djirosimu_realsense_d435i/realsense_stereo_imu_config.yaml", "content": "%YAML:1.0\n\n#common parameters\n#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;\nimu: 1\nnum_of_cam: 2\n\n# imu_topic: \"/camera/imu\"\nimu_topic: \"/djiros/imu\"\nimage0_topic: \"/camera/infra1/image_rect_raw\"\nimage1_topic: \"/camera/infra2/image_rect_raw\"\noutput_path: \"/home/dji/vinsfusion-output/\"\n\ncam0_calib: \"left.yaml\"\ncam1_calib: \"right.yaml\"\nimage_width: 640\nimage_height: 480\n\n\n\n# contains the right_T_left aka 1_T_0, ie. stereo baseline. I need right_T_left that a transform that transforms 3d points of left to 3d points of right. in other words, if you sure of right the position of left camera.\n# This is an optional argument even if using `num_of_cam:=2`. If I cannot\n# find this key I will use `body_T_cam0` and `body_T_cam1` to compute cam1_T_cam0.\n# But if this key exists, we will use these values as stereo baseline.\n# **In this file, I assume translation re specified ****in mm**** (and not in meters).**\nextrinsic_1_T_0: \"extrinsics.yaml\"\n\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n\n\nbody_T_cam0: !!opencv-matrix\n rows: 4\n cols: 4\n dt: d\n data: [ -1.0406548080980293e-02, -6.8339359975994052e-04,\n 9.9994561688635142e-01, 5.2600599061337287e-02,\n -9.9994461366801901e-01, -1.5656625000493030e-03,\n -1.0407607662295648e-02, 2.5510536758700533e-02,\n 1.5726898469127781e-03, -9.9999854083599726e-01,\n -6.6706260700977182e-04, 9.5887981235633201e-03, 0., 0., 0., 1. ]\n\n\n\nbody_T_cam1: !!opencv-matrix\n rows: 4\n cols: 4\n dt: d\n data: [ -1.3185778504660295e-02, 1.2513252883544768e-04,\n 9.9991305601390978e-01, 4.2975722207919849e-02,\n -9.9984703667423980e-01, 1.1490149582040754e-02,\n -1.3186345829069746e-02, -5.4397185804457168e-02,\n -1.1490800623434938e-02, -9.9993397822277874e-01,\n -2.6393179226591457e-05, 1.8167602224138771e-02, 0., 0., 0., 1. ]\n\n\n\n#Multiple thread support\nmultiple_thread: 1\n\n#feature traker paprameters\nmax_cnt: 150 # max feature number in feature tracking\nmin_dist: 30 # min distance between two features\nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 0 # publish tracking image as topic\nflow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04\ngyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05 0.004\nacc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.002\ngyr_w: 0.00005 # gyroscope bias random work noise standard deviation. #4.0e-5\ng_norm: 9.805 # gravity magnitude\n\n#unsynchronization parameters\nestimate_td: 1 # online estimate time offset between camera and imu\ntd: 0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#loop closure parameters\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\npose_graph_save_path: \"/home/dji/output/pose_graph/\" # save and load path\nsave_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0\n" }, { "path": "config/vinsfusion/djirosimu_realsense_d435i/right.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 640\nimage_height: 480\ndistortion_parameters:\n k1: 0.0\n k2: 0.0\n p1: 0.0\n p2: 0.0\nprojection_parameters:\n fx: 385.7544860839844\n fy: 385.7544860839844\n cx: 323.1204833984375\n cy: 236.7432098388672\n" }, { "path": "config/vinsfusion/euroc/cam0_pinhole.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n k1: -2.9545645106987750e-01\n k2: 8.6623215640186171e-02\n p1: 2.0132892276082517e-06\n p2: 1.3924531371276508e-05\nprojection_parameters:\n fx: 4.6115862106007575e+02\n fy: 4.5975286598073296e+02\n cx: 3.6265929181685937e+02\n cy: 2.4852105668448124e+02\n" }, { "path": "config/vinsfusion/euroc/cam1_pinhole.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n k1: -2.9294124381930947e-01\n k2: 8.4798002331543665e-02\n p1: -2.9984646536002372e-04\n p2: 3.0028216325237329e-04\nprojection_parameters:\n fx: 4.6009781682258682e+02\n fy: 4.5890983492218902e+02\n cx: 3.7314916359808268e+02\n cy: 2.5440734973672119e+02\n" }, { "path": "config/vinsfusion/euroc/euroc_stereo_imu_config.yaml", "content": "%YAML:1.0\n\n#common parameters\n#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;\nimu: 1\nnum_of_cam: 2\n\nimu_topic: \"/imu0\"\nimage0_topic: \"/cam0/image_raw\"\nimage1_topic: \"/cam1/image_raw\"\noutput_path: \"~/output/\"\n\n#cam0_calib: \"cam0_mei.yaml\"\n#cam1_calib: \"cam1_mei.yaml\"\ncam0_calib: \"cam0_pinhole.yaml\"\ncam1_calib: \"cam1_pinhole.yaml\"\nimage_width: 752\nimage_height: 480\n\n# contains the right_T_left aka 1_T_0, ie. stereo baseline. I need right_T_left that a transform that transforms 3d points of left to 3d points of right. in other words, if you sure of right the position of left camera.\n# This is an optional argument even if using `num_of_cam:=2`. If I cannot\n# find this key I will use `body_T_cam0` and `body_T_cam1` to compute cam1_T_cam0.\n# But if this key exists, we will use these values as stereo baseline.\n# **In this file, I assume translation re specified ****in mm**** (and not in meters).**\nextrinsic_1_T_0: \"extrinsics.yaml\"\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n\nbody_T_cam0: !!opencv-matrix\n rows: 4\n cols: 4\n dt: d\n data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,\n 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,\n -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,\n 0, 0, 0, 1]\n\nbody_T_cam1: !!opencv-matrix\n rows: 4\n cols: 4\n dt: d\n data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,\n 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,\n -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,\n 0, 0, 0, 1]\n\n#Multiple thread support\nmultiple_thread: 1\n\n#feature traker paprameters\nmax_cnt: 150 # max feature number in feature tracking\nmin_dist: 30 # min distance between two features\nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 1 # publish tracking image as topic\nflow_back: 0 # perform forward and backward optical flow to improve feature tracking accuracy\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.1 # accelerometer measurement noise standard deviation.\ngyr_n: 0.01 # gyroscope measurement noise standard deviation.\nacc_w: 0.001 # accelerometer bias random work noise standard deviation.\ngyr_w: 0.0001 # gyroscope bias random work noise standard deviation.\ng_norm: 9.81007 # gravity magnitude\n\n#unsynchronization parameters\nestimate_td: 0 # online estimate time offset between camera and imu\ntd: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#loop closure parameters\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\npose_graph_save_path: \"~/output/pose_graph/\" # save and load path\nsave_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0\n" }, { "path": "config/vinsfusion/euroc/extrinsics.yaml", "content": "%YAML:1.0\n---\n # this is computed for euric using body_T_cam0 and body_T_cam1.\ntransform:\n q_x: -0.00704531\n q_y: 0.000179855\n q_z: -0.00115733\n q_w: 0.999974\n t_x: -110.074\n t_y: 0.399122\n t_z: -0.853703\n" }, { "path": "config/vinsfusion/mynteye/camera_left.yaml", "content": "%YAML:1.0\n---\nmodel_type: KANNALA_BRANDT\ncamera_name: camera_left\nimage_width: 752\nimage_height: 480\nprojection_parameters:\n k2: -1.9372136316513616e-02\n k3: -9.8140934466631590e-03\n k4: 5.2090950264238800e-03\n k5: -2.5384805224148016e-03\n mu: 3.6779833417894611e+02\n mv: 3.6735717027889308e+02\n u0: 3.7150938938370604e+02\n v0: 2.3994369620747401e+02\n" }, { "path": "config/vinsfusion/mynteye/camera_left_pinhole.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera_left\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n k1: -3.0304011877067349e-01\n k2: 7.8611376314970768e-02\n p1: 1.3200427858719645e-03\n p2: -1.2942361409192899e-03\nprojection_parameters:\n fx: 3.7560167589977470e+02\n fy: 3.7552589588133230e+02\n cx: 3.7887177487131697e+02\n cy: 2.3402525474822104e+02\n" }, { "path": "config/vinsfusion/mynteye/camera_right.yaml", "content": "%YAML:1.0\n---\nmodel_type: KANNALA_BRANDT\ncamera_name: camera_right\nimage_width: 752\nimage_height: 480\nprojection_parameters:\n k2: -1.6877708507279571e-02\n k3: -2.2125250367976849e-02\n k4: 2.5629804328110812e-02\n k5: -1.3352680211095494e-02\n mu: 3.6746513955323167e+02\n mv: 3.6703089274525593e+02\n u0: 3.6200946050656484e+02\n v0: 2.4919721516867821e+02\n" }, { "path": "config/vinsfusion/mynteye/camera_right_pinhole.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera_right\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n k1: -3.0790744874410708e-01\n k2: 8.3037204977051693e-02\n p1: 1.1347651021695062e-03\n p2: -7.8671202943393290e-04\nprojection_parameters:\n fx: 3.7573448135250288e+02\n fy: 3.7536464618331070e+02\n cx: 3.6634215933104940e+02\n cy: 2.4325470902936044e+02\n" }, { "path": "config/vinsfusion/mynteye/extrinsics.yaml", "content": "%YAML:1.0\n---\ntransform:\n q_x: -7.0955103716253032e-04\n q_y: -1.5775578725333590e-03\n q_z: -1.2732644461763854e-03\n q_w: 9.9999769332040711e-01\n t_x: -1.2006984141573309e+02\n t_y: 3.3956264524978619e-01\n t_z: -1.6784055634087214e-01\n" }, { "path": "config/vinsfusion/mynteye/mynteye_stereo_imu_config.yaml", "content": "%YAML:1.0\n\n#common parameters\n#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;\nimu: 1\nnum_of_cam: 2\n\nimu_topic: \"/mynteye/imu/data_raw\"\nimage0_topic: \"/mynteye/left/image_raw\"\nimage1_topic: \"/mynteye/right/image_raw\"\noutput_path: \"/Bulk_Data/vins-mono-output/\"\n\n#cam0_calib: \"camera_left_pinhole.yaml\"\n#cam1_calib: \"camera_right_pinhole.yaml\"\ncam0_calib: \"camera_left.yaml\"\ncam1_calib: \"camera_right.yaml\"\nimage_width: 752\nimage_height: 480\n\n# contains the right_T_left aka 1_T_0, ie. stereo baseline. I need right_T_left that a transform that transforms 3d points of left to 3d points of right. in other words, if you sure of right the position of left camera.\n# This is an optional argument even if using `num_of_cam:=2`. If I cannot\n# find this key I will use `body_T_cam0` and `body_T_cam1` to compute cam1_T_cam0.\n# But if this key exists, we will use these values as stereo baseline.\n# **In this file, I assume translation re specified ****in mm**** (and not in meters).**\nextrinsic_1_T_0: \"extrinsics.yaml\"\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n\nbody_T_cam0: !!opencv-matrix\n rows: 4\n cols: 4\n dt: d\n data: [4.2739206953025244e-03, -9.9997227378122833e-01,-6.0979726705474944e-03,6.1040452082857912e-03,\n 9.9998919890819160e-01, 4.2626966744552242e-03, 1.8524265207998580e-03,-4.8408978130397302e-02,\n -1.8263813521932205e-03, -6.1058239298286089e-03, 9.9997969141642784e-01,2.2594515206886469e-02,\n 0,0,0,1]\n\n# ===> imu_T_cam0 * inv( cam1_T_cam0 )\nbody_T_cam1: !!opencv-matrix\n rows: 4\n cols: 4\n dt: d\n data: [ 0.00174443, -0.99998733, -0.0046694 , 0.00665227,\n 0.99998578, 0.00172106, 0.00500326, 0.07165957,\n -0.00499517, -0.00467806, 0.9999766 , 0.02216417,\n 0. , 0. , 0. , 1. ]\n\n#Multiple thread support\nmultiple_thread: 1\n\n#feature traker paprameters\nmax_cnt: 100 # max feature number in feature tracking\nmin_dist: 30 # min distance between two features\nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.1 # ransac threshold (pixel)\nshow_track: 0 # publish tracking image as topic\nflow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy\n\n#optimization parameters\nmax_solver_time: 0.05 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 12 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04\ngyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004\nacc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02\ngyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5\ng_norm: 9.80766 # gravity magnitude\n\n#unsynchronization parameters\nestimate_td: 0 # online estimate time offset between camera and imu\ntd: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#loop closure parameters\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\npose_graph_save_path: \"~/output/pose_graph/\" # save and load path\nsave_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0\n" }, { "path": "config/vinsfusion/realsense_d435i/extrinsics.yaml", "content": "%YAML:1.0\n---\ntransform:\n q_x: 0.\n q_y: 0.\n q_z: 0.\n q_w: 1.\n t_x: -49.8\n t_y: 0.\n t_z: 0.\n#4.98 cm for the realsense d435i device. This device has hardware undistort so can safely use the pinhole model \n" }, { "path": "config/vinsfusion/realsense_d435i/left.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 640\nimage_height: 480\ndistortion_parameters:\n k1: 0.0\n k2: 0.0\n p1: 0.0\n p2: 0.0\nprojection_parameters:\n fx: 385.7544860839844\n fy: 385.7544860839844\n cx: 323.1204833984375\n cy: 236.7432098388672\n" }, { "path": "config/vinsfusion/realsense_d435i/realsense_stereo_imu_config.yaml", "content": "%YAML:1.0\n\n#common parameters\n#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;\nimu: 1\nnum_of_cam: 2\n\n#imu_topic: \"/camera/imu\"\nimu_topic: \"/djiros/imu\"\nimage0_topic: \"/camera/infra1/image_rect_raw\"\nimage1_topic: \"/camera/infra2/image_rect_raw\"\noutput_path: \"/home/dji/output/\"\n\ncam0_calib: \"left.yaml\"\ncam1_calib: \"right.yaml\"\nimage_width: 640\nimage_height: 480\n\n\n\n# contains the right_T_left aka 1_T_0, ie. stereo baseline. I need right_T_left that a transform that transforms 3d points of left to 3d points of right. in other words, if you sure of right the position of left camera.\n# This is an optional argument even if using `num_of_cam:=2`. If I cannot\n# find this key I will use `body_T_cam0` and `body_T_cam1` to compute cam1_T_cam0.\n# But if this key exists, we will use these values as stereo baseline.\n# **In this file, I assume translation re specified ****in mm**** (and not in meters).**\nextrinsic_1_T_0: \"extrinsics.yaml\"\n\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n\nbody_T_cam0: !!opencv-matrix\n rows: 4\n cols: 4\n dt: d\n data: [ -1.0406548080980293e-02, -6.8339359975994052e-04,\n 9.9994561688635142e-01, 5.2600599061337287e-02,\n -9.9994461366801901e-01, -1.5656625000493030e-03,\n -1.0407607662295648e-02, 2.5510536758700533e-02,\n 1.5726898469127781e-03, -9.9999854083599726e-01,\n -6.6706260700977182e-04, 9.5887981235633201e-03, 0., 0., 0., 1. ]\n\n\n\nbody_T_cam1: !!opencv-matrix\n rows: 4\n cols: 4\n dt: d\n data: [ -1.3185778504660295e-02, 1.2513252883544768e-04,\n 9.9991305601390978e-01, 4.2975722207919849e-02,\n -9.9984703667423980e-01, 1.1490149582040754e-02,\n -1.3186345829069746e-02, -5.4397185804457168e-02,\n -1.1490800623434938e-02, -9.9993397822277874e-01,\n -2.6393179226591457e-05, 1.8167602224138771e-02, 0., 0., 0., 1. ]\n\n#Multiple thread support\nmultiple_thread: 1\n\n#feature traker paprameters\nmax_cnt: 100 # max feature number in feature tracking\nmin_dist: 30 # min distance between two features\nfreq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 0 # publish tracking image as topic\nflow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2 0.04\ngyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05 0.004\nacc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.002\ngyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5\ng_norm: 9.805 # gravity magnitude\n\n#unsynchronization parameters\nestimate_td: 0 # online estimate time offset between camera and imu\ntd: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#loop closure parameters\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\npose_graph_save_path: \"/home/dji/output/pose_graph/\" # save and load path\nsave_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0\n" }, { "path": "config/vinsfusion/realsense_d435i/right.yaml", "content": "%YAML:1.0\n---\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 640\nimage_height: 480\ndistortion_parameters:\n k1: 0.0\n k2: 0.0\n p1: 0.0\n p2: 0.0\nprojection_parameters:\n fx: 385.7544860839844\n fy: 385.7544860839844\n cx: 323.1204833984375\n cy: 236.7432098388672\n" }, { "path": "config/vinsmono_debug_config.yaml", "content": "%YAML:1.0\n\n#######\n## This is config file for running vins-mono. We use this for imu_T_camera calibration.\n## Set the estimate_extrinsic to 2. If vins mono works on, just copy (written in `output_path`)\n#######\n\n# imu_topic: \"/camera/imu\"\nimu_topic: \"/djiros/imu\"\nimage_topic: \"/camera/infra1/image_rect_raw\"\n#image_topic: \"/camera/infra2/image_rect_raw\"\n\n# output_path: \"/home/tony-ws1/output/\"\noutput_path: \"/home/dji/output/\"\n\n#camera calibration - left , right same for realsense\nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 640\nimage_height: 480\ndistortion_parameters:\n k1: 0.0\n k2: 0.0\n p1: 0.0\n p2: 0.0\nprojection_parameters:\n fx: 385.7544860839844\n fy: 385.7544860839844\n cx: 323.1204833984375\n cy: 236.7432098388672\n\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.\n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ -1.0406548080980293e-02, -6.8339359975994052e-04,\n 9.9994561688635142e-01, -9.9994461366801901e-01,\n -1.5656625000493030e-03, -1.0407607662295648e-02,\n 1.5726898469127781e-03, -9.9999854083599726e-01,\n -6.6706260700977182e-04 ]\nextrinsicTranslation: !!opencv-matrix\n rows: 3\n cols: 1\n dt: d\n data: [ 5.2600599061337287e-02, 2.5510536758700533e-02,\n 9.5887981235633201e-03 ]\n\n\n#feature traker paprameters\nmax_cnt: 200 # max feature number in feature tracking\nmin_dist: 25 # min distance between two features\nfreq: 15 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image\nF_threshold: 1.0 # ransac threshold (pixel)\nshow_track: 1 # publish tracking image as topic\nequalize: 1 # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8 # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters The more accurate parameters you provide, the better performance\nacc_n: 0.05 # accelerometer measurement noise standard deviation. #0.2 0.04\ngyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004\nacc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.02\ngyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5\ng_norm: 9.805 # gravity magnitude\n\n#loop closure parameters\nloop_closure: 0 # start loop closure\nload_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'\nfast_relocalization: 0 # useful in real-time and large project\npose_graph_save_path: \"/home/tony-ws1/output/pose_graph/\" # save and load path\n\n#unsynchronization parameters\nestimate_td: 1 # online estimate time offset between camera and imu\ntd: 0.038 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)\n\n#rolling shutter parameters\nrolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera\nrolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).\n\n#visualization parameters\nsave_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0\nvisualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results\nvisualize_camera_size: 0.4 # size of camera marker in RVIZ\n" }, { "path": "launch/debugging_vinsmono.launch", "content": "\n \n\t \n\n \n \n \n \n\n \n \n \n \n\n \n\n\n" }, { "path": "launch/euroc_vinsfusion.launch", "content": "\n\n \n \n \n\n\n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n\n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n\n\n \n \n \n \n \n\n \n \n \n \n \n \n\n\n \n \n \n \n \n\n \n \n \n \n \n\n\n\n" }, { "path": "launch/keras_server.launch", "content": "\n\n\n\n\n\n\n\n\n\n\n\n\n \n \n \n\n \n \n \n \n \n\n\n\n" }, { "path": "launch/mynteye_vinsfusion.launch", "content": "\n\n \n \n\n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n\n\n \n \n \n \n \n \n \n \n\n \n \n \n \n\n\n\n\n\n\n \n\n\n \n\n\n \n \n \n \n\n \n \n \n \n \n \n\n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n\n\n" }, { "path": "launch/mynteye_vinsfusion_loadstate.launch", "content": "\n\n \n \n\n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n\n\n \n \n \n \n \n \n \n \n\n \n \n \n \n\n\n\n\n\n\n \n\n\n \n\n\n \n \n \n \n\n \n \n \n \n \n \n\n\n \n \n\n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n\n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "launch/mynteye_vinsfusion_ondrone.launch", "content": "\n\n \n \n\n\n\n \n \n \n \n\n \n \n \n \n \n \n\n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n\n\n" }, { "path": "launch/mynteye_vinsfusion_savestate.launch", "content": "\n\n \n \n\n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n\n\n \n \n \n \n \n \n \n \n\n \n \n \n \n\n\n\n\n\n\n \n\n\n \n\n\n \n \n \n \n\n \n \n \n \n \n \n\n\n \n \n\n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n\n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "launch/realsense_camera.launch", "content": "\n \n \n \n \n \n \n\n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n /camera/stereo_module/emitter_enabled: false\n \n\n \n \n \n \n \n \n \n\n \n \n \n \n \n\n \n \n \n\n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "launch/realsense_vinsfusion.launch", "content": "\n\n \n \n\n\n \n \n\n\n \n \n \n\n\n \n\n \n\n\n \n \n \n \n\n \n \n \n \n \n \n\n\n \n \n \n \n\n\n \n\n\n \n \n\n \n\n \n \n \n \n \n \n \n \n\n\n \n \n \n\n \n \n \n\n \n \n \n\n \n\n\n \n \n\n \n \n\n\n" }, { "path": "launch/realsense_vinsfusion_ondrone_repeat.launch", "content": "\n\n \n \n\n\n \n \n\n \n \n \n\n\n \n\n \n\n\n \n \n \n \n\n \n \n \n \n \n \n\n\n \n \n \n \n\n \n \n\n \n\n \n \n \n \n \n \n \n \n \n \n\n\n \n \n \n\n \n \n \n \n\n \n \n \n\n \n \n\n \n \n\n\n \n \n\n\n" }, { "path": "launch/realsense_vinsfusion_ondrone_teach.launch", "content": "\n\n \n \n\n\n \n \n\n \n \n \n\n\n \n\n \n\n\n \n \n \n \n\n \n \n \n \n \n \n\n\n \n \n \n \n\n \n \n\n \n\n \n \n \n \n \n \n \n \n \n \n\n\n \n \n \n\n \n \n \n \n\n \n \n \n\n \n \n\n \n \n\n\n \n \n\n\n" }, { "path": "msg/LoopEdge.msg", "content": "time timestamp0\ntime timestamp1\ngeometry_msgs/Pose pose_1T0 # pose of 0 as observed from 1. \nfloat32 weight\nstring description\n" }, { "path": "package.xml", "content": "\n\n cerebro\n 0.0.1\n The cerebro package. There is a multi-threaded node which stores all the incoming info from vins.\n There is a service to compute image level descriptor. Geometry is also a separate service.\n\n \n \n \n mpkuse\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n catkin\n cv_bridge\n image_transport\n roscpp\n rospy\n std_msgs\n message_generation\n\n cv_bridge\n image_transport\n roscpp\n rospy\n std_msgs\n message_runtime\n\n\n \n \n \n\n \n\n" }, { "path": "rviz/dev-config.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /Ref1\n - /Ref1/Grid1\n - /Feat Tracker1/Input Image1\n - /Feat Tracker1/Input Image1/Status1\n - /VIO1\n - /VIO1/Odom In Cam-frame-of-ref1/Cam Odometry1\n - /Cerebro1\n - /Cerebro1/Marker1\n - /Cerebro1/Marker1/Namespaces1\n - /AR_demo1\n - /AR_demo1/AR_image_odm1/Status1\n - /Solve Pose Graph Opt1\n - /Solve Pose Graph Opt1/Marker1\n - /Solve Pose Graph Opt1/Marker1/Status1\n - /Solve Pose Graph Opt1/Marker1/Namespaces1\n - /Solve Pose Graph Opt1/Debug Node Marker1/Namespaces1\n Splitter Ratio: 0.62647754\n Tree Height: 697\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.588679016\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: Tracked Features\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded: ~\n Splitter Ratio: 0.5\n Tree Height: 141\nVisualization Manager:\n Class: \"\"\n Displays:\n - Class: rviz/Group\n Displays:\n - Class: rviz/Axes\n Enabled: true\n Length: 1\n Name: Axes\n Radius: 0.200000003\n Reference Frame: \n Value: true\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.0299999993\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 20\n Reference Frame: \n Value: true\n Enabled: true\n Name: Ref\n - Class: rviz/Group\n Displays:\n - Class: rviz/Image\n Enabled: false\n Image Topic: /cam0/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Input Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Image\n Enabled: true\n Image Topic: /feature_tracker/feature_img\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Tracked Features\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Name: Feat Tracker\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 85; 255\n Enabled: true\n Head Diameter: 0.300000012\n Head Length: 0.200000003\n Length: 0.300000012\n Line Style: Lines\n Line Width: 0.0299999993\n Name: VIO Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Style: None\n Radius: 0.0299999993\n Shaft Diameter: 0.100000001\n Shaft Length: 0.100000001\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /vins_estimator/camera_pose_visual\n Name: Visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 1.46719348\n Min Value: -0.613372922\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 100\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: KF PointCloud\n Position Transformer: XYZ\n Queue Size: 100\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.00999999978\n Style: Points\n Topic: /vins_estimator/keyframe_point\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 100\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: PointCloud\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.00999999978\n Style: Points\n Topic: /vins_estimator/point_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 170; 85; 255\n Enabled: false\n Keep: 100\n Length: 1\n Name: imu_T_cam\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/extrinsic\n Value: false\n - Class: rviz/Group\n Displays:\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 255; 25; 0\n Enabled: true\n Keep: 100\n Length: 1\n Name: Cam Odometry\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/camera_pose\n Value: true\n Enabled: false\n Name: Odom In Cam-frame-of-ref\n - Class: rviz/Group\n Displays:\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 0; 255; 0\n Enabled: true\n Keep: 100\n Length: 1\n Name: KF Odometry\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/keyframe_pose\n Value: true\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 0; 170; 255\n Enabled: true\n Keep: 100\n Length: 1\n Name: IMU Odometry\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/odometry\n Value: true\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 255; 25; 0\n Enabled: true\n Keep: 100\n Length: 1\n Name: IMU Prop 200hz\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/imu_propagate\n Value: true\n Enabled: false\n Name: Odom In IMU-Frame-of-ref\n - Class: rviz/Group\n Displays:\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 255; 25; 0\n Enabled: false\n Keep: 100\n Length: 1\n Name: vinsmono-reloc-odom\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/relo_relative_pose\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 85; 255\n Enabled: true\n Head Diameter: 0.300000012\n Head Length: 0.200000003\n Length: 0.300000012\n Line Style: Lines\n Line Width: 0.0299999993\n Name: vinsmono-relocalized_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Style: None\n Radius: 0.0299999993\n Shaft Diameter: 0.100000001\n Shaft Length: 0.100000001\n Topic: /vins_estimator/relocalization_path\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /pose_graph/match_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: vins-monomatch-images\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /pose_graph/pose_graph\n Name: vinsmono loop viz\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n Enabled: false\n Name: vins-mono relocalized\n Enabled: false\n Name: VIO\n - Class: rviz/Group\n Displays:\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /cerebro_node/viz/framedata\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Image\n Enabled: true\n Image Topic: /cerebro_node/viz/imagepaire\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: imagepaire\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Name: Cerebro\n - Class: rviz/Group\n Displays:\n - Class: rviz/Image\n Enabled: true\n Image Topic: /ar_demo_node3_corrected/AR_image_corrected\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: AR_image_corrected\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /ar_demo_node3_odom/AR_image_odom\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: AR_image_odm\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /points_and_lines/AR_object\n Name: AR_object\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/InteractiveMarkers\n Enable Transparency: true\n Enabled: true\n Name: InteractiveMarkers\n Show Axes: false\n Show Descriptions: true\n Show Visual Aids: false\n Update Topic: /interactive_marker_server/update\n Value: true\n Enabled: true\n Name: AR_demo\n - Class: rviz/Group\n Displays:\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /keyframe_pose_graph_slam_node/viz/visualization_marker\n Name: Marker\n Namespaces:\n latest_odometry: true\n world##_cam_visual: true\n world#-1: true\n world#-2: true\n world#-3: true\n world#-4: true\n world#0: true\n world#1: true\n world#2: true\n world#3: true\n world#4: true\n Queue Size: 100\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /debug_pose_graph_solver/visualization_marker\n Name: Debug Node Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n Enabled: true\n Name: Solve Pose Graph Opt\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Fixed Frame: world\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Topic: /initialpose\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 46.7297897\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.0599999987\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 3.76500964\n Y: 2.58213377\n Z: -2.90765786\n Focal Shape Fixed Size: false\n Focal Shape Size: 0.0500000007\n Name: Current View\n Near Clip Distance: 0.00999999978\n Pitch: 1.0647974\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 4.69420815\n Saved:\n - Class: rviz/Orbit\n Distance: 49.6446877\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.0599999987\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 22.8474731\n Y: 20.4583664\n Z: -6.84013748\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.0500000007\n Name: Orbit\n Near Clip Distance: 0.00999999978\n Pitch: 1.16479671\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 5.02046442\nWindow Geometry:\n AR_image_corrected:\n collapsed: false\n AR_image_odm:\n collapsed: false\n Displays:\n collapsed: false\n Height: 1003\n Hide Left Dock: false\n Hide Right Dock: false\n Input Image:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Tracked Features:\n collapsed: false\n Width: 1918\n X: 0\n Y: 24\n imagepaire:\n collapsed: false\n vins-monomatch-images:\n collapsed: false\n" }, { "path": "rviz/good-viz.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /Ref1\n - /Ref1/Axes1\n - /Feat Tracker1/Input Image1\n - /Feat Tracker1/Input Image1/Status1\n - /VIO1\n - /VIO1/Odom In Cam-frame-of-ref1/Cam Odometry1\n - /Cerebro1\n - /Cerebro1/Marker1\n - /Cerebro1/Marker1/Namespaces1\n - /AR_demo1\n - /Solve Pose Graph Opt1\n - /Solve Pose Graph Opt1/Marker1\n - /Solve Pose Graph Opt1/Marker1/Namespaces1\n - /Solve Pose Graph Opt1/Debug Node Marker1/Namespaces1\n - /Ref21\n - /Ref21/Grid1/Offset1\n Splitter Ratio: 0.62647754\n Tree Height: 805\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.588679016\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: Tracked Features\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded: ~\n Splitter Ratio: 0.5\n Tree Height: 359\nVisualization Manager:\n Class: \"\"\n Displays:\n - Class: rviz/Group\n Displays:\n - Class: rviz/Axes\n Enabled: true\n Length: 1\n Name: Axes\n Radius: 0.200000003\n Reference Frame: \n Value: true\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.0299999993\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 20\n Reference Frame: \n Value: true\n Enabled: true\n Name: Ref\n - Class: rviz/Group\n Displays:\n - Class: rviz/Image\n Enabled: false\n Image Topic: /cam0/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Input Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Image\n Enabled: true\n Image Topic: /feature_tracker/feature_img\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Tracked Features\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Name: Feat Tracker\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 85; 255\n Enabled: true\n Head Diameter: 0.300000012\n Head Length: 0.200000003\n Length: 0.300000012\n Line Style: Lines\n Line Width: 0.0299999993\n Name: VIO Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Style: None\n Radius: 0.0299999993\n Shaft Diameter: 0.100000001\n Shaft Length: 0.100000001\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /vins_estimator/camera_pose_visual\n Name: Visual\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 1.46719348\n Min Value: -0.613372922\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 100\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: KF PointCloud\n Position Transformer: XYZ\n Queue Size: 100\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.00999999978\n Style: Points\n Topic: /vins_estimator/keyframe_point\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 100\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: PointCloud\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.00999999978\n Style: Points\n Topic: /vins_estimator/point_cloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 170; 85; 255\n Enabled: false\n Keep: 100\n Length: 1\n Name: imu_T_cam\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/extrinsic\n Value: false\n - Class: rviz/Group\n Displays:\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 255; 25; 0\n Enabled: true\n Keep: 100\n Length: 1\n Name: Cam Odometry\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/camera_pose\n Value: true\n Enabled: false\n Name: Odom In Cam-frame-of-ref\n - Class: rviz/Group\n Displays:\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 0; 255; 0\n Enabled: true\n Keep: 100\n Length: 1\n Name: KF Odometry\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/keyframe_pose\n Value: true\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 0; 170; 255\n Enabled: true\n Keep: 100\n Length: 1\n Name: IMU Odometry\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/odometry\n Value: true\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 255; 25; 0\n Enabled: true\n Keep: 100\n Length: 1\n Name: IMU Prop 200hz\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/imu_propagate\n Value: true\n Enabled: false\n Name: Odom In IMU-Frame-of-ref\n - Class: rviz/Group\n Displays:\n - Angle Tolerance: 0.100000001\n Class: rviz/Odometry\n Color: 255; 25; 0\n Enabled: false\n Keep: 100\n Length: 1\n Name: vinsmono-reloc-odom\n Position Tolerance: 0.100000001\n Topic: /vins_estimator/relo_relative_pose\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 85; 255\n Enabled: true\n Head Diameter: 0.300000012\n Head Length: 0.200000003\n Length: 0.300000012\n Line Style: Lines\n Line Width: 0.0299999993\n Name: vinsmono-relocalized_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Style: None\n Radius: 0.0299999993\n Shaft Diameter: 0.100000001\n Shaft Length: 0.100000001\n Topic: /vins_estimator/relocalization_path\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /pose_graph/match_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: vins-monomatch-images\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /pose_graph/pose_graph\n Name: vinsmono loop viz\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n Enabled: false\n Name: vins-mono relocalized\n Enabled: false\n Name: VIO\n - Class: rviz/Group\n Displays:\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /cerebro_node/viz/framedata\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /cerebro_node/viz/imagepaire\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: imagepaire\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Name: Cerebro\n - Class: rviz/Group\n Displays:\n - Class: rviz/Image\n Enabled: true\n Image Topic: /ar_demo_node3_corrected/AR_image_corrected\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: AR_image_corrected\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /ar_demo_node3_odom/AR_image_odom\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: AR_image_odm\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /points_and_lines/AR_object\n Name: AR_object\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/InteractiveMarkers\n Enable Transparency: true\n Enabled: true\n Name: InteractiveMarkers\n Show Axes: false\n Show Descriptions: true\n Show Visual Aids: false\n Update Topic: /interactive_marker_server/update\n Value: true\n Enabled: false\n Name: AR_demo\n - Class: rviz/Group\n Displays:\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /keyframe_pose_graph_slam_node/viz/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /keyframe_pose_graph_slam_node/viz/disjoint_set_status_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: disjoint_set_status_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /debug_pose_graph_solver/visualization_marker\n Name: Debug Node Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n Enabled: true\n Name: Solve Pose Graph Opt\n - Class: rviz/Group\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 40; 164; 44\n Enabled: true\n Line Style:\n Line Width: 0.0299999993\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 30\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 20\n Reference Frame: \n Value: true\n Enabled: true\n Name: Ref2\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Fixed Frame: world\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Topic: /initialpose\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 49.2109833\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.0599999987\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 3.41268897\n Y: -0.305300832\n Z: -1.72543097\n Focal Shape Fixed Size: false\n Focal Shape Size: 0.0500000007\n Name: Current View\n Near Clip Distance: 0.00999999978\n Pitch: 0.884797573\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 3.05431199\n Saved:\n - Class: rviz/Orbit\n Distance: 49.6446877\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.0599999987\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 22.8474731\n Y: 20.4583664\n Z: -6.84013748\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.0500000007\n Name: Orbit\n Near Clip Distance: 0.00999999978\n Pitch: 1.16479671\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 5.02046442\nWindow Geometry:\n AR_image_corrected:\n collapsed: false\n AR_image_odm:\n collapsed: false\n Displays:\n collapsed: false\n Height: 1003\n Hide Left Dock: false\n Hide Right Dock: false\n Input Image:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Tracked Features:\n collapsed: false\n Width: 1918\n X: 0\n Y: 24\n disjoint_set_status_image:\n collapsed: false\n imagepaire:\n collapsed: false\n vins-monomatch-images:\n collapsed: false\n" }, { "path": "rviz/vins-fusion.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /raw_input_image1\n - /Ref1\n - /Ref1/Axes1\n - /Ref1/Grid1\n - /Feat Tracker1/Input Image1\n - /AR_demo1\n - /AR_demo1/AR_image_corrected1\n - /AR_demo1/AR_image_odm1\n - /AR_demo1/ModelViz1\n - /AR_demo1/ModelViz1/Status1\n - /AR_demo1/ModelViz1/Namespaces1\n - /Cerebro1\n - /Cerebro1/Marker1\n - /Cerebro1/Marker1/Namespaces1\n - /Solve Pose Graph Opt1/Marker1\n - /Solve Pose Graph Opt1/Marker1/Namespaces1\n - /Solve Pose Graph Opt1/Debug Node Marker1/Namespaces1\n - /VINS-fusion1/MarkerArray1/Namespaces1\n - /VINS-fusion1/VIO Path1\n - /VINS-fusion1/VIO Path1/Offset1\n - /VINS-fusion1/After Loop Fusion1\n - /VINS-fusion1/Loop Fusion Edges1\n - /Marker1\n - /Surfel Mapping1/PointCloud21\n Splitter Ratio: 0.62647754\n Tree Height: 853\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.588679016\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: raw_input_image\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded: ~\n Splitter Ratio: 0.5\n Tree Height: 359\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\nVisualization Manager:\n Class: \"\"\n Displays:\n - Class: rviz/Image\n Enabled: true\n Image Topic: /camera/infra1/image_rect_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: raw_input_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Group\n Displays:\n - Class: rviz/Axes\n Enabled: false\n Length: 1\n Name: Axes\n Radius: 0.200000003\n Reference Frame: \n Value: false\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.0299999993\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 20\n Reference Frame: \n Value: true\n Enabled: true\n Name: Ref\n - Class: rviz/Group\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 40; 164; 44\n Enabled: true\n Line Style:\n Line Width: 0.0299999993\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 30\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 20\n Reference Frame: \n Value: true\n Enabled: true\n Name: Ref2\n - Class: rviz/Group\n Displays:\n - Class: rviz/Image\n Enabled: true\n Image Topic: /cam0/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Input Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /feature_tracker/feature_img\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Tracked Features\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: false\n Name: Feat Tracker\n - Class: rviz/Group\n Displays:\n - Class: rviz/Image\n Enabled: true\n Image Topic: /ar_demo_node3_odom/AR_image_corrected\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: AR_image_corrected\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /ar_demo_node3_odom/AR_image_odom\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: AR_image_odm\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /points_and_lines/AR_object\n Name: AR_object\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: x\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 9.04478359\n Min Color: 0; 0; 0\n Min Intensity: 3.50075197\n Name: debug ptcld\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.0199999996\n Style: Flat Squares\n Topic: /unit_test_estimate_ground_plane/pt_cld\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /ar_demo_node3_odom/models\n Name: ModelViz\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/InteractiveMarkers\n Enable Transparency: true\n Enabled: true\n Name: InteractiveMarkers\n Show Axes: false\n Show Descriptions: true\n Show Visual Aids: false\n Update Topic: /interactive_marker_server/update\n Value: true\n Enabled: true\n Name: AR_demo\n - Class: rviz/Group\n Displays:\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /cerebro_node/viz/framedata\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Image\n Enabled: true\n Image Topic: /cerebro_node/viz/imagepaire\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: cerebro - imagepaire\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Name: Cerebro\n - Class: rviz/Group\n Displays:\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /keyframe_pose_graph_slam_node/viz/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /keyframe_pose_graph_slam_node/viz/disjoint_set_status_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: disjoint_set_status_image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /debug_pose_graph_solver/visualization_marker\n Name: Debug Node Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n Enabled: true\n Name: Solve Pose Graph Opt\n - Class: rviz/Group\n Displays:\n - Class: rviz/Image\n Enabled: true\n Image Topic: /loop_fusion/match_image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: DBOW Match Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /vins_estimator/camera_pose_visual\n Name: MarkerArray\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 85; 255\n Enabled: true\n Head Diameter: 0.300000012\n Head Length: 0.200000003\n Length: 0.300000012\n Line Style: Lines\n Line Width: 0.0299999993\n Name: VIO Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Style: None\n Radius: 0.0299999993\n Shaft Diameter: 0.100000001\n Shaft Length: 0.100000001\n Topic: /vins_estimator/path\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 85; 255\n Enabled: true\n Head Diameter: 0.300000012\n Head Length: 0.200000003\n Length: 0.300000012\n Line Style: Lines\n Line Width: 0.0299999993\n Name: After Loop Fusion\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Style: None\n Radius: 0.0299999993\n Shaft Diameter: 0.100000001\n Shaft Length: 0.100000001\n Topic: /loop_fusion/pose_graph_path\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /loop_fusion/pose_graph\n Name: Loop Fusion Edges\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n Enabled: false\n Name: VINS-fusion\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /keyframe_pose_graph_slam_node/hz200/visualization_marker\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: z\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 0.863711178\n Min Color: 0; 0; 0\n Min Intensity: -2.19427705\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.0500000007\n Style: Flat Squares\n Topic: /surfel_fusion/pointcloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 255\n Min Color: 0; 0; 0\n Min Intensity: 17\n Name: current\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.00999999978\n Style: Flat Squares\n Topic: /surfel_fusion/raw_pointcloud\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: z\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: true\n Max Color: 255; 255; 255\n Max Intensity: 1.4563067\n Min Color: 0; 0; 0\n Min Intensity: -1.5742451\n Name: filtered\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.00999999978\n Style: Points\n Topic: /map_server/filtered_map\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n Enabled: true\n Name: Surfel Mapping\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Fixed Frame: world\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Topic: /initialpose\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 88.2317963\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.0599999987\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 14.8281975\n Y: -0.740946651\n Z: 0.155234724\n Focal Shape Fixed Size: false\n Focal Shape Size: 0.0500000007\n Name: Current View\n Near Clip Distance: 0.00999999978\n Pitch: 1.29479516\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 4.72152758\n Saved:\n - Class: rviz/Orbit\n Distance: 49.6446877\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.0599999987\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 22.8474731\n Y: 20.4583664\n Z: -6.84013748\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.0500000007\n Name: Orbit\n Near Clip Distance: 0.00999999978\n Pitch: 1.16479671\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 5.02046442\nWindow Geometry:\n AR_image_corrected:\n collapsed: false\n AR_image_odm:\n collapsed: false\n DBOW Match Image:\n collapsed: false\n Displays:\n collapsed: false\n Height: 1056\n Hide Left Dock: false\n Hide Right Dock: false\n Input Image:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Tracked Features:\n collapsed: false\n Views:\n collapsed: false\n Width: 1855\n X: 65\n Y: 24\n cerebro - imagepaire:\n collapsed: false\n disjoint_set_status_image:\n collapsed: false\n raw_input_image:\n collapsed: false\n" }, { "path": "scripts/TerminalColors.py", "content": "class bcolors:\n HEADER = '\\033[95m'\n OKBLUE = '\\033[94m'\n OKGREEN = '\\033[92m'\n WARNING = '\\033[93m'\n FAIL = '\\033[91m'\n ENDC = '\\033[0m'\n BOLD = '\\033[1m'\n UNDERLINE = '\\033[4m'\n\n\n# USage\n#```\n# from TerminalColors import bcolors\n# tcol = bcolors()\n# print tcol.OKGREEN, \"hello in green color\", tcol.ENDC\n#```\n" }, { "path": "scripts/keras.models/.gitignore", "content": "" }, { "path": "scripts/keras.models/Apr2019/gray_conv6_K16__centeredinput/model.json", "content": "\"{\\\"class_name\\\": \\\"Model\\\", \\\"keras_version\\\": \\\"2.2.4\\\", \\\"config\\\": {\\\"layers\\\": [{\\\"class_name\\\": \\\"InputLayer\\\", \\\"config\\\": {\\\"dtype\\\": \\\"float32\\\", \\\"batch_input_shape\\\": [null, 240, 320, 1], \\\"name\\\": \\\"input_1\\\", \\\"sparse\\\": false}, \\\"inbound_nodes\\\": [], \\\"name\\\": \\\"input_1\\\"}, {\\\"class_name\\\": \\\"ZeroPadding2D\\\", \\\"config\\\": {\\\"padding\\\": [[0, 1], [0, 1]], \\\"trainable\\\": true, \\\"name\\\": \\\"conv1_pad\\\", \\\"data_format\\\": \\\"channels_last\\\"}, \\\"inbound_nodes\\\": [[[\\\"input_1\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv1_pad\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv1\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"valid\\\", \\\"strides\\\": [2, 2], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": {\\\"class_name\\\": \\\"L1L2\\\", \\\"config\\\": {\\\"l2\\\": 9.999999747378752e-05, \\\"l1\\\": 0.0}}, \\\"filters\\\": 32, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv1_pad\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv1\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv1_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv1\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv1_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv1_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv1_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv1_relu\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_1\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv1_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_1\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_1_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_1\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_1_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_1_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_1_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_1_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_1\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": {\\\"class_name\\\": \\\"L1L2\\\", \\\"config\\\": {\\\"l2\\\": 9.999999747378752e-05, \\\"l1\\\": 0.0}}, \\\"filters\\\": 64, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_1_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_1\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_1_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_1\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_1_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_1_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_1_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_1_relu\\\"}, {\\\"class_name\\\": \\\"ZeroPadding2D\\\", \\\"config\\\": {\\\"padding\\\": [[0, 1], [0, 1]], \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pad_2\\\", \\\"data_format\\\": \\\"channels_last\\\"}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_1_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pad_2\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_2\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"valid\\\", \\\"strides\\\": [2, 2], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pad_2\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_2\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_2_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_2\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_2_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_2_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_2_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_2_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_2\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": {\\\"class_name\\\": \\\"L1L2\\\", \\\"config\\\": {\\\"l2\\\": 9.999999747378752e-05, \\\"l1\\\": 0.0}}, \\\"filters\\\": 128, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_2_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_2\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_2_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_2\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_2_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_2_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_2_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_2_relu\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_3\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_2_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_3\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_3_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_3\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_3_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_3_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_3_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_3_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_3\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": {\\\"class_name\\\": \\\"L1L2\\\", \\\"config\\\": {\\\"l2\\\": 9.999999747378752e-05, \\\"l1\\\": 0.0}}, \\\"filters\\\": 128, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_3_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_3\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_3_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_3\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_3_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_3_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_3_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_3_relu\\\"}, {\\\"class_name\\\": \\\"ZeroPadding2D\\\", \\\"config\\\": {\\\"padding\\\": [[0, 1], [0, 1]], \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pad_4\\\", \\\"data_format\\\": \\\"channels_last\\\"}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_3_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pad_4\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_4\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"valid\\\", \\\"strides\\\": [2, 2], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pad_4\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_4\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_4_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_4\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_4_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_4_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_4_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_4_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_4\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": {\\\"class_name\\\": \\\"L1L2\\\", \\\"config\\\": {\\\"l2\\\": 9.999999747378752e-05, \\\"l1\\\": 0.0}}, \\\"filters\\\": 256, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_4_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_4\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_4_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_4\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_4_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_4_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_4_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_4_relu\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_5\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_4_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_5\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_5_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_5\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_5_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_5_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_5_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_5_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_5\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": {\\\"class_name\\\": \\\"L1L2\\\", \\\"config\\\": {\\\"l2\\\": 9.999999747378752e-05, \\\"l1\\\": 0.0}}, \\\"filters\\\": 256, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_5_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_5\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_5_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_5\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_5_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_5_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_5_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_5_relu\\\"}, {\\\"class_name\\\": \\\"ZeroPadding2D\\\", \\\"config\\\": {\\\"padding\\\": [[0, 1], [0, 1]], \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pad_6\\\", \\\"data_format\\\": \\\"channels_last\\\"}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_5_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pad_6\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_6\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"valid\\\", \\\"strides\\\": [2, 2], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pad_6\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_6\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_6_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_6\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_6_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_6_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_6_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_6_relu\\\"}, {\\\"class_name\\\": \\\"NetVLADLayer\\\", \\\"config\\\": {\\\"num_clusters\\\": 16, \\\"trainable\\\": true, \\\"name\\\": \\\"net_vlad_layer_1\\\"}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_6_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"net_vlad_layer_1\\\"}], \\\"input_layers\\\": [[\\\"input_1\\\", 0, 0]], \\\"output_layers\\\": [[\\\"net_vlad_layer_1\\\", 0, 0]], \\\"name\\\": \\\"model_1\\\"}, \\\"backend\\\": \\\"tensorflow\\\"}\"" }, { "path": "scripts/keras.models/README.md", "content": "# Pretrained Models for Place Recognition.\n\nThis folder contains tested models for place recognition. The models have been tested for real-time\nperformance.\n\nthe files a) mobilenet_conv7_allpairloss.keras b) model.json c) modelarch_and_weights.h5 d) core_model.keras e) xcore.png are associated with the default model.\n" }, { "path": "scripts/keras.models/model.json", "content": "\"{\\\"class_name\\\": \\\"Model\\\", \\\"keras_version\\\": \\\"2.2.4\\\", \\\"config\\\": {\\\"layers\\\": [{\\\"class_name\\\": \\\"InputLayer\\\", \\\"config\\\": {\\\"dtype\\\": \\\"float32\\\", \\\"batch_input_shape\\\": [null, 480, 752, 3], \\\"name\\\": \\\"input_1\\\", \\\"sparse\\\": false}, \\\"inbound_nodes\\\": [], \\\"name\\\": \\\"input_1\\\"}, {\\\"class_name\\\": \\\"ZeroPadding2D\\\", \\\"config\\\": {\\\"padding\\\": [[0, 1], [0, 1]], \\\"trainable\\\": true, \\\"name\\\": \\\"conv1_pad\\\", \\\"data_format\\\": \\\"channels_last\\\"}, \\\"inbound_nodes\\\": [[[\\\"input_1\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv1_pad\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv1\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"valid\\\", \\\"strides\\\": [2, 2], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": null, \\\"filters\\\": 32, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv1_pad\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv1\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv1_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv1\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv1_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv1_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv1_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv1_relu\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_1\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv1_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_1\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_1_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_1\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_1_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_1_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_1_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_1_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_1\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": null, \\\"filters\\\": 64, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_1_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_1\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_1_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_1\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_1_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_1_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_1_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_1_relu\\\"}, {\\\"class_name\\\": \\\"ZeroPadding2D\\\", \\\"config\\\": {\\\"padding\\\": [[0, 1], [0, 1]], \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pad_2\\\", \\\"data_format\\\": \\\"channels_last\\\"}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_1_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pad_2\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_2\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"valid\\\", \\\"strides\\\": [2, 2], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pad_2\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_2\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_2_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_2\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_2_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_2_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_2_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_2_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_2\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": null, \\\"filters\\\": 128, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_2_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_2\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_2_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_2\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_2_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_2_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_2_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_2_relu\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_3\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_2_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_3\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_3_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_3\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_3_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_3_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_3_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_3_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_3\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": null, \\\"filters\\\": 128, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_3_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_3\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_3_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_3\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_3_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_3_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_3_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_3_relu\\\"}, {\\\"class_name\\\": \\\"ZeroPadding2D\\\", \\\"config\\\": {\\\"padding\\\": [[0, 1], [0, 1]], \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pad_4\\\", \\\"data_format\\\": \\\"channels_last\\\"}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_3_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pad_4\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_4\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"valid\\\", \\\"strides\\\": [2, 2], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pad_4\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_4\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_4_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_4\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_4_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_4_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_4_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_4_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_4\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": null, \\\"filters\\\": 256, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_4_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_4\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_4_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_4\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_4_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_4_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_4_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_4_relu\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_5\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_4_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_5\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_5_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_5\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_5_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_5_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_5_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_5_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_5\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": null, \\\"filters\\\": 256, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_5_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_5\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_5_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_5\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_5_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_5_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_5_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_5_relu\\\"}, {\\\"class_name\\\": \\\"ZeroPadding2D\\\", \\\"config\\\": {\\\"padding\\\": [[0, 1], [0, 1]], \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pad_6\\\", \\\"data_format\\\": \\\"channels_last\\\"}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_5_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pad_6\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_6\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"valid\\\", \\\"strides\\\": [2, 2], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pad_6\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_6\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_6_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_6\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_6_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_6_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_6_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_6_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_6\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": null, \\\"filters\\\": 512, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_6_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_6\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_6_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_6\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_6_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_6_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_6_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_6_relu\\\"}, {\\\"class_name\\\": \\\"DepthwiseConv2D\\\", \\\"config\\\": {\\\"use_bias\\\": false, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_7\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"depth_multiplier\\\": 1, \\\"depthwise_constraint\\\": null, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"depthwise_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"depthwise_regularizer\\\": null, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [3, 3]}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_6_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_7\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_dw_7_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_7\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_7_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_dw_7_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_7_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_dw_7_relu\\\"}, {\\\"class_name\\\": \\\"Conv2D\\\", \\\"config\\\": {\\\"kernel_constraint\\\": null, \\\"kernel_initializer\\\": {\\\"class_name\\\": \\\"VarianceScaling\\\", \\\"config\\\": {\\\"distribution\\\": \\\"uniform\\\", \\\"scale\\\": 1.0, \\\"seed\\\": null, \\\"mode\\\": \\\"fan_avg\\\"}}, \\\"name\\\": \\\"conv_pw_7\\\", \\\"bias_regularizer\\\": null, \\\"bias_constraint\\\": null, \\\"activation\\\": \\\"linear\\\", \\\"trainable\\\": true, \\\"data_format\\\": \\\"channels_last\\\", \\\"padding\\\": \\\"same\\\", \\\"strides\\\": [1, 1], \\\"dilation_rate\\\": [1, 1], \\\"kernel_regularizer\\\": null, \\\"filters\\\": 512, \\\"bias_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"use_bias\\\": false, \\\"activity_regularizer\\\": null, \\\"kernel_size\\\": [1, 1]}, \\\"inbound_nodes\\\": [[[\\\"conv_dw_7_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_7\\\"}, {\\\"class_name\\\": \\\"BatchNormalization\\\", \\\"config\\\": {\\\"beta_constraint\\\": null, \\\"gamma_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"moving_mean_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"name\\\": \\\"conv_pw_7_bn\\\", \\\"epsilon\\\": 0.001, \\\"trainable\\\": true, \\\"moving_variance_initializer\\\": {\\\"class_name\\\": \\\"Ones\\\", \\\"config\\\": {}}, \\\"beta_initializer\\\": {\\\"class_name\\\": \\\"Zeros\\\", \\\"config\\\": {}}, \\\"scale\\\": true, \\\"axis\\\": -1, \\\"gamma_constraint\\\": null, \\\"gamma_regularizer\\\": null, \\\"beta_regularizer\\\": null, \\\"momentum\\\": 0.99, \\\"center\\\": true}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_7\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_7_bn\\\"}, {\\\"class_name\\\": \\\"ReLU\\\", \\\"config\\\": {\\\"threshold\\\": 0.0, \\\"max_value\\\": 6.0, \\\"trainable\\\": true, \\\"name\\\": \\\"conv_pw_7_relu\\\", \\\"negative_slope\\\": 0.0}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_7_bn\\\", 0, 0, {}]]], \\\"name\\\": \\\"conv_pw_7_relu\\\"}, {\\\"class_name\\\": \\\"NetVLADLayer\\\", \\\"config\\\": {\\\"num_clusters\\\": 16, \\\"trainable\\\": true, \\\"name\\\": \\\"net_vlad_layer_1\\\"}, \\\"inbound_nodes\\\": [[[\\\"conv_pw_7_relu\\\", 0, 0, {}]]], \\\"name\\\": \\\"net_vlad_layer_1\\\"}], \\\"input_layers\\\": [[\\\"input_1\\\", 0, 0]], \\\"output_layers\\\": [[\\\"net_vlad_layer_1\\\", 0, 0]], \\\"name\\\": \\\"model_1\\\"}, \\\"backend\\\": \\\"tensorflow\\\"}\"" }, { "path": "scripts/keras_helpers.py", "content": "from keras import backend as K\nfrom keras.engine.topology import Layer\nimport keras\nimport code\nimport numpy as np\n\nimport cv2\nimport code\n\n\n#---------------------------------------------------------------------------------\n# My Layers\n# NetVLADLayer\n#---------------------------------------------------------------------------------\n\n# # Writing your own custom layers\n# class MyLayer(Layer):\n#\n# def __init__(self, output_dim, **kwargs):\n# self.output_dim = output_dim\n# super(MyLayer, self).__init__(**kwargs)\n#\n# def build(self, input_shape):\n# # Create a trainable weight variable for this layer.\n# self.kernel = self.add_weight(name='kernel',\n# shape=(input_shape[1], self.output_dim),\n# initializer='uniform',\n# trainable=True)\n# super(MyLayer, self).build(input_shape) # Be sure to call this at the end\n#\n# def call(self, x):\n# return [K.dot(x, self.kernel), K.dot(x, self.kernel)]\n#\n# def compute_output_shape(self, input_shape):\n# return [(input_shape[0], self.output_dim), (input_shape[0], self.output_dim)]\n#\n#\n# class NetVLADLayer( Layer ):\n#\n# def __init__( self, num_clusters, **kwargs ):\n# self.num_clusters = num_clusters\n# super(NetVLADLayer, self).__init__(**kwargs)\n#\n# def build( self, input_shape ):\n# self.K = self.num_clusters\n# self.D = input_shape[-1]\n#\n# self.kernel = self.add_weight( name='kernel',\n# shape=(1,1,self.D,self.K),\n# initializer='uniform',\n# trainable=True )\n#\n# self.bias = self.add_weight( name='bias',\n# shape=(1,1,self.K),\n# initializer='uniform',\n# trainable=True )\n#\n# self.C = self.add_weight( name='cluster_centers',\n# shape=[1,1,1,self.D,self.K],\n# initializer='uniform',\n# trainable=True)\n#\n# def call( self, x ):\n# # soft-assignment.\n# s = K.conv2d( x, self.kernel, padding='same' ) + self.bias\n# a = K.softmax( s )\n# self.amap = K.argmax( a, -1 )\n# print 'amap.shape', self.amap.shape\n#\n# # Dims used hereafter: batch, H, W, desc_coeff, cluster\n# a = K.expand_dims( a, -2 )\n# # print 'a.shape=',a.shape\n#\n# # Core\n# v = K.expand_dims(x, -1) + self.C\n# # print 'v.shape', v.shape\n# v = a * v\n# # print 'v.shape', v.shape\n# v = K.sum(v, axis=[1, 2])\n# # print 'v.shape', v.shape\n# v = K.permute_dimensions(v, pattern=[0, 2, 1])\n# # print 'v.shape', v.shape\n# #v.shape = None x K x D\n#\n# # Normalize v (Intra Normalization)\n# v = K.l2_normalize( v, axis=-1 )\n# v = K.batch_flatten( v )\n# v = K.l2_normalize( v, axis=-1 )\n#\n# return [v, self.amap]\n#\n# def compute_output_shape( self, input_shape ):\n# # return (input_shape[0], self.v.shape[-1].value )\n# # return [(input_shape[0], self.K*self.D ), (input_shape[0], self.amap.shape[1].value, self.amap.shape[2].value) ]\n# return [(input_shape[0], self.K*self.D ), (input_shape[0], input_shape[1], input_shape[2]) ]\n\n\n#--------------------------------------------------------------------------------\n# Base CNNs\n#--------------------------------------------------------------------------------\n\n# def make_vgg( input_img ):\n# r_l2=keras.regularizers.l2(0.01)\n# r_l1=keras.regularizers.l1(0.01)\n#\n# x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( input_img )\n# x_64 = keras.layers.normalization.BatchNormalization()( x_64 )\n# x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )\n# x_64 = keras.layers.normalization.BatchNormalization()( x_64 )\n# x_64 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_64 )\n#\n#\n# x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )\n# x_128 = keras.layers.normalization.BatchNormalization()( x_128 )\n# x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )\n# x_128 = keras.layers.normalization.BatchNormalization()( x_128 )\n# x_128 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_128 )\n#\n#\n# # x_256 = keras.layers.Conv2D( 256, (3,3), padding='same', activation='relu' )( x_128 )\n# # x_256 = keras.layers.normalization.BatchNormalization()( x_256 )\n# # x_256 = keras.layers.Conv2D( 256, (3,3), padding='same', activation='relu' )( x_256 )\n# # x_256 = keras.layers.normalization.BatchNormalization()( x_256 )\n# # x_256 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_256 )\n#\n# #\n# # x_512 = keras.layers.Conv2D( 512, (3,3), padding='same', activation='relu' )( x_256 )\n# # # BN\n# # x_512 = keras.layers.Conv2D( 512, (3,3), padding='same', activation='relu' )( x_512 )\n# # # BN\n# # x_512 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_512 )\n#\n#\n# x = keras.layers.Conv2DTranspose( 32, (5,5), strides=4, padding='same' )( x_128 )\n# # x = x_128\n#\n# return x\n#\n#\n# def make_upsampling_vgg( input_img ):\n# r_l2=keras.regularizers.l2(0.01)\n# r_l1=keras.regularizers.l1(0.01)\n#\n# x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( input_img )\n# x_64 = keras.layers.normalization.BatchNormalization()( x_64 )\n# x_64 = keras.layers.Conv2D( 64, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )\n# x_64 = keras.layers.normalization.BatchNormalization()( x_64 )\n#\n# x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )\n# x_128 = keras.layers.normalization.BatchNormalization()( x_128 )\n# x_128 = keras.layers.Conv2D( 128, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )\n# x_128 = keras.layers.normalization.BatchNormalization()( x_128 )\n#\n# x_256 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )\n# x_256 = keras.layers.normalization.BatchNormalization()( x_256 )\n# x_256 = keras.layers.Conv2D( 128, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_256 )\n# x_256 = keras.layers.normalization.BatchNormalization()( x_256 )\n#\n# z = keras.layers.Conv2DTranspose( 32, (11,11), strides=8, padding='same' )( x_256 )\n# x = keras.layers.Conv2DTranspose( 32, (9,9), strides=4, padding='same' )( x_128 )\n# y = keras.layers.Conv2DTranspose( 32, (7,7), strides=2, padding='same' )( x_64 )\n#\n# out = keras.layers.Add()( [x,y,z] )\n# return out\n#\n# def make_from_vgg19( input_img, trainable=True, weights=None ):\n# base_model = keras.applications.vgg19.VGG19(weights=weights, include_top=False, input_tensor=input_img)\n#\n# for l in base_model.layers:\n# l.trainable = trainable\n#\n# base_model_out = base_model.get_layer('block2_pool').output\n#\n# z = keras.layers.Conv2DTranspose( 32, (9,9), strides=4, padding='same' )( base_model_out )\n# return z\n#\n# def make_from_vgg19_multiconvup( input_img, trainable=True, weights=None ):\n# base_model = keras.applications.vgg19.VGG19(weights=weights, include_top=False, input_tensor=input_img)\n#\n# for l in base_model.layers:\n# l.trainable = trainable\n# #TODO : add kernel regularizers and activity_regularizer to conv layers\n#\n# base_model_out = base_model.get_layer('block2_pool').output\n#\n# up_conv_out = keras.layers.Conv2DTranspose( 32, (9,9), strides=2, padding='same', activation='relu' )( base_model_out )\n# up_conv_out = keras.layers.normalization.BatchNormalization()( up_conv_out )\n#\n# up_conv_out = keras.layers.Conv2DTranspose( 32, (9,9), strides=2, padding='same', activation='relu' )( up_conv_out )\n# up_conv_out = keras.layers.normalization.BatchNormalization()( up_conv_out )\n#\n#\n# return up_conv_out\n#\n#\n# def make_from_mobilenet( input_img=None, weights=None ):\n# # input_img = keras.layers.BatchNormalization()(input_img)\n#\n# base_model = keras.applications.mobilenet.MobileNet( weights=weights, include_top=False, input_tensor=input_img )\n# # base_model = keras.applications.mobilenet.MobileNet( weights=None, include_top=False, input_tensor=input_img )\n# # keras.utils.plot_model( base_model, to_file='base_model.png', show_shapes=True )\n#\n# # Pull out a layer from original network\n# base_model_out = base_model.get_layer( 'conv_pw_7_relu').output # can also try conv_pw_7_relu etc.\n#\n# # Up-sample\n# # Basic idea is to try upsampling without using the transposed-conv layers. Instead use either of\n# # a) upsampling2d\n# # b) depth to space\n# # followed by CBR (conv-BN-relu)\n# # TODO\n# ups_out = base_model_out\n# # ups_out = keras.layers.UpSampling2D( size=(4,4) )( base_model_out )\n#\n# # model = keras.models.Model( inputs=input_img, outputs=ups_out )\n# # model.summary()\n# # keras.utils.plot_model( model, to_file='base_model.png', show_shapes=True )\n# # code.interact( local=locals() )\n#\n# return ups_out\n#\n#\n\n\n\n\n#--------------------------------------------------------------------------------\n# Base CNNs\n#--------------------------------------------------------------------------------\n\ndef make_vgg( input_img ):\n r_l2=keras.regularizers.l2(0.01)\n r_l1=keras.regularizers.l1(0.01)\n\n x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( input_img )\n x_64 = keras.layers.normalization.BatchNormalization()( x_64 )\n x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )\n x_64 = keras.layers.normalization.BatchNormalization()( x_64 )\n x_64 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_64 )\n\n\n x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )\n x_128 = keras.layers.normalization.BatchNormalization()( x_128 )\n x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )\n x_128 = keras.layers.normalization.BatchNormalization()( x_128 )\n x_128 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_128 )\n\n\n # x_256 = keras.layers.Conv2D( 256, (3,3), padding='same', activation='relu' )( x_128 )\n # x_256 = keras.layers.normalization.BatchNormalization()( x_256 )\n # x_256 = keras.layers.Conv2D( 256, (3,3), padding='same', activation='relu' )( x_256 )\n # x_256 = keras.layers.normalization.BatchNormalization()( x_256 )\n # x_256 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_256 )\n\n #\n # x_512 = keras.layers.Conv2D( 512, (3,3), padding='same', activation='relu' )( x_256 )\n # # BN\n # x_512 = keras.layers.Conv2D( 512, (3,3), padding='same', activation='relu' )( x_512 )\n # # BN\n # x_512 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_512 )\n\n\n x = keras.layers.Conv2DTranspose( 32, (5,5), strides=4, padding='same' )( x_128 )\n # x = x_128\n\n return x\n\n\ndef make_upsampling_vgg( input_img ):\n r_l2=keras.regularizers.l2(0.01)\n r_l1=keras.regularizers.l1(0.01)\n\n x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( input_img )\n x_64 = keras.layers.normalization.BatchNormalization()( x_64 )\n x_64 = keras.layers.Conv2D( 64, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )\n x_64 = keras.layers.normalization.BatchNormalization()( x_64 )\n\n x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )\n x_128 = keras.layers.normalization.BatchNormalization()( x_128 )\n x_128 = keras.layers.Conv2D( 128, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )\n x_128 = keras.layers.normalization.BatchNormalization()( x_128 )\n\n x_256 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )\n x_256 = keras.layers.normalization.BatchNormalization()( x_256 )\n x_256 = keras.layers.Conv2D( 128, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_256 )\n x_256 = keras.layers.normalization.BatchNormalization()( x_256 )\n\n z = keras.layers.Conv2DTranspose( 32, (11,11), strides=8, padding='same' )( x_256 )\n x = keras.layers.Conv2DTranspose( 32, (9,9), strides=4, padding='same' )( x_128 )\n y = keras.layers.Conv2DTranspose( 32, (7,7), strides=2, padding='same' )( x_64 )\n\n out = keras.layers.Add()( [x,y,z] )\n return out\n\n\n\ndef make_from_vgg19_multiconvup( input_img, trainable=False ):\n base_model = keras.applications.vgg19.VGG19(weights='imagenet', include_top=False, input_tensor=input_img)\n\n for l in base_model.layers:\n l.trainable = trainable\n #TODO : add kernel regularizers and activity_regularizer to conv layers\n\n base_model_out = base_model.get_layer('block2_pool').output\n\n up_conv_out = keras.layers.Conv2DTranspose( 32, (9,9), strides=2, padding='same', activation='relu' )( base_model_out )\n up_conv_out = keras.layers.normalization.BatchNormalization()( up_conv_out )\n\n up_conv_out = keras.layers.Conv2DTranspose( 32, (9,9), strides=2, padding='same', activation='relu' )( up_conv_out )\n up_conv_out = keras.layers.normalization.BatchNormalization()( up_conv_out )\n\n return up_conv_out\n\n\ndef make_from_mobilenet( input_img, weights=None, layer_name='conv_pw_7_relu' ):\n # input_img = keras.layers.BatchNormalization()(input_img)\n\n base_model = keras.applications.mobilenet.MobileNet( weights=weights, include_top=False, input_tensor=input_img )\n\n # Pull out a layer from original network\n base_model_out = base_model.get_layer( layer_name ).output # can also try conv_pw_7_relu etc.\n\n return base_model_out\n\n\n\n\n\ndef make_from_vgg16( input_img, weights='imagenet', trainable=False, layer_name='block2_pool' ):\n base_model = keras.applications.vgg16.VGG16(weights=weights, include_top=False, input_tensor=input_img)\n\n for l in base_model.layers:\n l.trainable = trainable\n\n base_model_out = base_model.get_layer(layer_name).output\n return base_model_out\n" }, { "path": "scripts/predict_utils.py", "content": "import keras\nimport json\nimport pprint\nimport numpy as np\nimport cv2\nimport code\nfrom keras import backend as K\nfrom keras.engine.topology import Layer\n\n# This was copied from the training code's CustomNets.py\nclass NetVLADLayer( Layer ):\n\n def __init__( self, num_clusters, **kwargs ):\n self.num_clusters = num_clusters\n super(NetVLADLayer, self).__init__(**kwargs)\n\n def build( self, input_shape ):\n self.K = self.num_clusters\n self.D = input_shape[-1]\n\n self.kernel = self.add_weight( name='kernel',\n shape=(1,1,self.D,self.K),\n initializer='uniform',\n trainable=True )\n\n self.bias = self.add_weight( name='bias',\n shape=(1,1,self.K),\n initializer='uniform',\n trainable=True )\n\n self.C = self.add_weight( name='cluster_centers',\n shape=[1,1,1,self.D,self.K],\n initializer='uniform',\n trainable=True)\n\n def call( self, x ):\n # soft-assignment.\n s = K.conv2d( x, self.kernel, padding='same' ) + self.bias\n a = K.softmax( s )\n self.amap = K.argmax( a, -1 )\n # print 'amap.shape', self.amap.shape\n\n # Dims used hereafter: batch, H, W, desc_coeff, cluster\n a = K.expand_dims( a, -2 )\n # print 'a.shape=',a.shape\n\n # Core\n v = K.expand_dims(x, -1) + self.C\n # print 'v.shape', v.shape\n v = a * v\n # print 'v.shape', v.shape\n v = K.sum(v, axis=[1, 2])\n # print 'v.shape', v.shape\n v = K.permute_dimensions(v, pattern=[0, 2, 1])\n # print 'v.shape', v.shape\n #v.shape = None x K x D\n\n # Normalize v (Intra Normalization)\n v = K.l2_normalize( v, axis=-1 )\n v = K.batch_flatten( v )\n v = K.l2_normalize( v, axis=-1 )\n\n # return [v, self.amap]\n return v\n\n def compute_output_shape( self, input_shape ):\n\n # return [(input_shape[0], self.K*self.D ), (input_shape[0], input_shape[1], input_shape[2]) ]\n return (input_shape[0], self.K*self.D )\n\n def get_config( self ):\n pass\n # base_config = super(NetVLADLayer, self).get_config()\n # return dict(list(base_config.items()))\n\n # As suggested by: https://github.com/keras-team/keras/issues/4871#issuecomment-269731817\n config = {'num_clusters': self.num_clusters}\n base_config = super(NetVLADLayer, self).get_config()\n return dict(list(base_config.items()) + list(config.items()))\n\n\n\nclass GhostVLADLayer( Layer ):\n\n def __init__( self, num_clusters, num_ghost_clusters, **kwargs ):\n self.num_clusters = num_clusters\n self.num_ghost_clusters = num_ghost_clusters\n super(GhostVLADLayer, self).__init__(**kwargs)\n\n def build( self, input_shape ):\n # self.K = self.num_clusters\n self.K = self.num_clusters + self.num_ghost_clusters\n self.D = input_shape[-1]\n\n self.kernel = self.add_weight( name='kernel',\n shape=(1,1,self.D,self.K),\n initializer='uniform',\n trainable=True )\n\n self.bias = self.add_weight( name='bias',\n shape=(1,1,self.K),\n initializer='uniform',\n trainable=True )\n\n self.C = self.add_weight( name='cluster_centers',\n shape=[1,1,1,self.D,self.K],\n initializer='uniform',\n trainable=True)\n\n def call( self, x ):\n # soft-assignment.\n s = K.conv2d( x, self.kernel, padding='same' ) + self.bias\n a = K.softmax( s )\n self.amap = K.argmax( a, -1 )\n # print 'amap.shape', self.amap.shape\n\n # Dims used hereafter: batch, H, W, desc_coeff, cluster\n a = K.expand_dims( a, -2 )\n # print 'a.shape=',a.shape\n\n # Core\n v = K.expand_dims(x, -1) + self.C\n # print 'v.shape', v.shape\n v = a * v\n # print 'v.shape', v.shape\n v = K.sum(v, axis=[1, 2])\n # print 'v.shape', v.shape\n v = K.permute_dimensions(v, pattern=[0, 2, 1])\n # print 'v.shape', v.shape\n #v.shape = None x K x D\n\n # Normalize v (Intra Normalization)\n v = v[:,0:self.num_clusters,:]\n # print 'after ghosting v.shape', v.shape\n v = K.l2_normalize( v, axis=-1 )\n v = K.batch_flatten( v )\n v = K.l2_normalize( v, axis=-1 )\n\n\n # return [v, self.amap]\n return v\n\n def compute_output_shape( self, input_shape ):\n\n # return [(input_shape[0], self.num_clusters*self.D ), (input_shape[0], input_shape[1], input_shape[2]) ]\n return (input_shape[0], self.num_clusters*self.D )\n\n def get_config( self ):\n pass\n\n\n # As suggested by: https://github.com/keras-team/keras/issues/4871#issuecomment-269731817\n config = {'num_clusters': self.num_clusters, 'num_ghost_clusters': self.num_ghost_clusters}\n base_config = super(GhostVLADLayer, self).get_config()\n return dict(list(base_config.items()) + list(config.items()))\n\n\n#--------------------------- UTILS --------------------------------------------#\ndef open_json_file( fname ):\n print 'Load JSON file: ', fname\n jsonX = json.loads(open(fname).read())\n return jsonX\n\n\ndef change_model_inputshape(model, new_input_shape=(None, 40, 40, 3), verbose=False):\n \"\"\"\n Given a model and new input shape it changes all the allocations.\n\n Note: It uses custom_objects={'NetVLAD': NetVLADLayer}. If you have any other\n custom-layer change the code here accordingly.\n \"\"\"\n print '[change_model_inputshape], new_input_shape=', new_input_shape\n # replace input shape of first layer\n model._layers[0].batch_input_shape = new_input_shape\n\n # feel free to modify additional parameters of other layers, for example...\n # model._layers[2].pool_size = (8, 8)\n # model._layers[2].strides = (8, 8)\n\n # rebuild model architecture by exporting and importing via json\n new_model = keras.models.model_from_json(model.to_json(), custom_objects={'NetVLADLayer': NetVLADLayer, 'GhostVLADLayer':GhostVLADLayer} )\n # new_model.summary()\n\n # copy weights from old model to new one\n print 'copy weights from old model to new one....this usually takes upto 10 sec'\n for layer in new_model.layers:\n try:\n if verbose:\n print( 'transfer weights for layer.name='+layer.name )\n layer.set_weights(model.get_layer(name=layer.name).get_weights())\n except:\n print '[Almost certainly FATAL] '\n print 'If you see the warning like wieghts cannot be transfer, this is usually bad. In this case make sure the desired input dimensions and those in the modeljson file are compatible'\n print(\"Could not transfer weights for layer {}\".format(layer.name))\n quit()\n\n # test new model on a random input image\n # X = np.random.rand(new_input_shape[0], new_input_shape[1], new_input_shape[2], new_input_shape[3] )\n # y_pred = new_model.predict(X)\n # print('try predict with a random input_img with shape='+str(X.shape)+ str(y_pred) )\n\n return new_model\n\n#--------------------------- END UTILS ----------------------------------------#\n" }, { "path": "scripts/unittest/demo_superpoint.py", "content": "#!/usr/bin/env python\n#\n# %BANNER_BEGIN%\n# ---------------------------------------------------------------------\n# %COPYRIGHT_BEGIN%\n#\n# Magic Leap, Inc. (\"COMPANY\") CONFIDENTIAL\n#\n# Unpublished Copyright (c) 2018\n# Magic Leap, Inc., All Rights Reserved.\n#\n# NOTICE: All information contained herein is, and remains the property\n# of COMPANY. The intellectual and technical concepts contained herein\n# are proprietary to COMPANY and may be covered by U.S. and Foreign\n# Patents, patents in process, and are protected by trade secret or\n# copyright law. Dissemination of this information or reproduction of\n# this material is strictly forbidden unless prior written permission is\n# obtained from COMPANY. Access to the source code contained herein is\n# hereby forbidden to anyone except current COMPANY employees, managers\n# or contractors who have executed Confidentiality and Non-disclosure\n# agreements explicitly covering such access.\n#\n# The copyright notice above does not evidence any actual or intended\n# publication or disclosure of this source code, which includes\n# information that is confidential and/or proprietary, and is a trade\n# secret, of COMPANY. ANY REPRODUCTION, MODIFICATION, DISTRIBUTION,\n# PUBLIC PERFORMANCE, OR PUBLIC DISPLAY OF OR THROUGH USE OF THIS\n# SOURCE CODE WITHOUT THE EXPRESS WRITTEN CONSENT OF COMPANY IS\n# STRICTLY PROHIBITED, AND IN VIOLATION OF APPLICABLE LAWS AND\n# INTERNATIONAL TREATIES. THE RECEIPT OR POSSESSION OF THIS SOURCE\n# CODE AND/OR RELATED INFORMATION DOES NOT CONVEY OR IMPLY ANY RIGHTS\n# TO REPRODUCE, DISCLOSE OR DISTRIBUTE ITS CONTENTS, OR TO MANUFACTURE,\n# USE, OR SELL ANYTHING THAT IT MAY DESCRIBE, IN WHOLE OR IN PART.\n#\n# %COPYRIGHT_END%\n# ----------------------------------------------------------------------\n# %AUTHORS_BEGIN%\n#\n# Originating Authors: Daniel DeTone (ddetone)\n# Tomasz Malisiewicz (tmalisiewicz)\n#\n# %AUTHORS_END%\n# --------------------------------------------------------------------*/\n# %BANNER_END%\n\n\nimport argparse\nimport glob\nimport numpy as np\nimport os\nimport time\nimport code\n\nimport cv2\nimport torch\n\n# Stub to warn about opencv version.\nif int(cv2.__version__[0]) < 3: # pragma: no cover\n print('Warning: OpenCV 3 is not installed')\n\n# Jet colormap for visualization.\nmyjet = np.array([[0. , 0. , 0.5 ],\n [0. , 0. , 0.99910873],\n [0. , 0.37843137, 1. ],\n [0. , 0.83333333, 1. ],\n [0.30044276, 1. , 0.66729918],\n [0.66729918, 1. , 0.30044276],\n [1. , 0.90123457, 0. ],\n [1. , 0.48002905, 0. ],\n [0.99910873, 0.07334786, 0. ],\n [0.5 , 0. , 0. ]])\n\nclass SuperPointNet(torch.nn.Module):\n \"\"\" Pytorch definition of SuperPoint Network. \"\"\"\n def __init__(self):\n super(SuperPointNet, self).__init__()\n self.relu = torch.nn.ReLU(inplace=True)\n self.pool = torch.nn.MaxPool2d(kernel_size=2, stride=2)\n c1, c2, c3, c4, c5, d1 = 64, 64, 128, 128, 256, 256\n # Shared Encoder.\n self.conv1a = torch.nn.Conv2d(1, c1, kernel_size=3, stride=1, padding=1)\n self.conv1b = torch.nn.Conv2d(c1, c1, kernel_size=3, stride=1, padding=1)\n self.conv2a = torch.nn.Conv2d(c1, c2, kernel_size=3, stride=1, padding=1)\n self.conv2b = torch.nn.Conv2d(c2, c2, kernel_size=3, stride=1, padding=1)\n self.conv3a = torch.nn.Conv2d(c2, c3, kernel_size=3, stride=1, padding=1)\n self.conv3b = torch.nn.Conv2d(c3, c3, kernel_size=3, stride=1, padding=1)\n self.conv4a = torch.nn.Conv2d(c3, c4, kernel_size=3, stride=1, padding=1)\n self.conv4b = torch.nn.Conv2d(c4, c4, kernel_size=3, stride=1, padding=1)\n # Detector Head.\n self.convPa = torch.nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1)\n self.convPb = torch.nn.Conv2d(c5, 65, kernel_size=1, stride=1, padding=0)\n # Descriptor Head.\n self.convDa = torch.nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1)\n self.convDb = torch.nn.Conv2d(c5, d1, kernel_size=1, stride=1, padding=0)\n\n def forward(self, x):\n \"\"\" Forward pass that jointly computes unprocessed point and descriptor\n tensors.\n Input\n x: Image pytorch tensor shaped N x 1 x H x W.\n Output\n semi: Output point pytorch tensor shaped N x 65 x H/8 x W/8.\n desc: Output descriptor pytorch tensor shaped N x 256 x H/8 x W/8.\n \"\"\"\n # Shared Encoder.\n x = self.relu(self.conv1a(x))\n x = self.relu(self.conv1b(x))\n x = self.pool(x)\n x = self.relu(self.conv2a(x))\n x = self.relu(self.conv2b(x))\n x = self.pool(x)\n x = self.relu(self.conv3a(x))\n x = self.relu(self.conv3b(x))\n x = self.pool(x)\n x = self.relu(self.conv4a(x))\n x = self.relu(self.conv4b(x))\n # Detector Head.\n cPa = self.relu(self.convPa(x))\n semi = self.convPb(cPa)\n # Descriptor Head.\n cDa = self.relu(self.convDa(x))\n desc = self.convDb(cDa)\n dn = torch.norm(desc, p=2, dim=1) # Compute the norm.\n desc = desc.div(torch.unsqueeze(dn, 1)) # Divide by norm to normalize.\n return semi, desc\n\n\nclass SuperPointFrontend(object):\n \"\"\" Wrapper around pytorch net to help with pre and post image processing. \"\"\"\n def __init__(self, weights_path, nms_dist, conf_thresh, nn_thresh,\n cuda=False):\n self.name = 'SuperPoint'\n self.cuda = cuda\n self.nms_dist = nms_dist\n self.conf_thresh = conf_thresh\n self.nn_thresh = nn_thresh # L2 descriptor distance for good match.\n self.cell = 8 # Size of each output cell. Keep this fixed.\n self.border_remove = 4 # Remove points this close to the border.\n\n # Load the network in inference mode.\n self.net = SuperPointNet()\n if cuda:\n # Train on GPU, deploy on GPU.\n self.net.load_state_dict(torch.load(weights_path))\n self.net = self.net.cuda()\n else:\n # Train on GPU, deploy on CPU.\n self.net.load_state_dict(torch.load(weights_path,\n map_location=lambda storage, loc: storage))\n self.net.eval()\n\n def nms_fast(self, in_corners, H, W, dist_thresh):\n \"\"\"\n Run a faster approximate Non-Max-Suppression on numpy corners shaped:\n 3xN [x_i,y_i,conf_i]^T\n\n Algo summary: Create a grid sized HxW. Assign each corner location a 1, rest\n are zeros. Iterate through all the 1's and convert them either to -1 or 0.\n Suppress points by setting nearby values to 0.\n\n Grid Value Legend:\n -1 : Kept.\n 0 : Empty or suppressed.\n 1 : To be processed (converted to either kept or supressed).\n\n NOTE: The NMS first rounds points to integers, so NMS distance might not\n be exactly dist_thresh. It also assumes points are within image boundaries.\n\n Inputs\n in_corners - 3xN numpy array with corners [x_i, y_i, confidence_i]^T.\n H - Image height.\n W - Image width.\n dist_thresh - Distance to suppress, measured as an infinty norm distance.\n Returns\n nmsed_corners - 3xN numpy matrix with surviving corners.\n nmsed_inds - N length numpy vector with surviving corner indices.\n \"\"\"\n grid = np.zeros((H, W)).astype(int) # Track NMS data.\n inds = np.zeros((H, W)).astype(int) # Store indices of points.\n # Sort by confidence and round to nearest int.\n inds1 = np.argsort(-in_corners[2,:])\n corners = in_corners[:,inds1]\n rcorners = corners[:2,:].round().astype(int) # Rounded corners.\n # Check for edge case of 0 or 1 corners.\n if rcorners.shape[1] == 0:\n return np.zeros((3,0)).astype(int), np.zeros(0).astype(int)\n if rcorners.shape[1] == 1:\n out = np.vstack((rcorners, in_corners[2])).reshape(3,1)\n return out, np.zeros((1)).astype(int)\n # Initialize the grid.\n for i, rc in enumerate(rcorners.T):\n grid[rcorners[1,i], rcorners[0,i]] = 1\n inds[rcorners[1,i], rcorners[0,i]] = i\n # Pad the border of the grid, so that we can NMS points near the border.\n pad = dist_thresh\n grid = np.pad(grid, ((pad,pad), (pad,pad)), mode='constant')\n # Iterate through points, highest to lowest conf, suppress neighborhood.\n count = 0\n for i, rc in enumerate(rcorners.T):\n # Account for top and left padding.\n pt = (rc[0]+pad, rc[1]+pad)\n if grid[pt[1], pt[0]] == 1: # If not yet suppressed.\n grid[pt[1]-pad:pt[1]+pad+1, pt[0]-pad:pt[0]+pad+1] = 0\n grid[pt[1], pt[0]] = -1\n count += 1\n # Get all surviving -1's and return sorted array of remaining corners.\n keepy, keepx = np.where(grid==-1)\n keepy, keepx = keepy - pad, keepx - pad\n inds_keep = inds[keepy, keepx]\n out = corners[:, inds_keep]\n values = out[-1, :]\n inds2 = np.argsort(-values)\n out = out[:, inds2]\n out_inds = inds1[inds_keep[inds2]]\n return out, out_inds\n\n def run(self, img):\n \"\"\" Process a numpy image to extract points and descriptors.\n Input\n img - HxW numpy float32 input image in range [0,1].\n Output\n corners - 3xN numpy array with corners [x_i, y_i, confidence_i]^T.\n desc - 256xN numpy array of corresponding unit normalized descriptors.\n heatmap - HxW numpy heatmap in range [0,1] of point confidences.\n \"\"\"\n assert img.ndim == 2, 'Image must be grayscale.'\n assert img.dtype == np.float32, 'Image must be float32.'\n H, W = img.shape[0], img.shape[1]\n inp = img.copy()\n inp = (inp.reshape(1, H, W))\n inp = torch.from_numpy(inp)\n inp = torch.autograd.Variable(inp).view(1, 1, H, W)\n if self.cuda:\n inp = inp.cuda()\n # Forward pass of network.\n outs = self.net.forward(inp)\n semi, coarse_desc = outs[0], outs[1]\n # Convert pytorch -> numpy.\n semi = semi.data.cpu().numpy().squeeze()\n # --- Process points.\n dense = np.exp(semi) # Softmax.\n dense = dense / (np.sum(dense, axis=0)+.00001) # Should sum to 1.\n # Remove dustbin.\n nodust = dense[:-1, :, :]\n # Reshape to get full resolution heatmap.\n Hc = int(H / self.cell)\n Wc = int(W / self.cell)\n nodust = nodust.transpose(1, 2, 0)\n heatmap = np.reshape(nodust, [Hc, Wc, self.cell, self.cell])\n heatmap = np.transpose(heatmap, [0, 2, 1, 3])\n heatmap = np.reshape(heatmap, [Hc*self.cell, Wc*self.cell])\n xs, ys = np.where(heatmap >= self.conf_thresh) # Confidence threshold.\n if len(xs) == 0:\n return np.zeros((3, 0)), None, None\n pts = np.zeros((3, len(xs))) # Populate point data sized 3xN.\n pts[0, :] = ys\n pts[1, :] = xs\n pts[2, :] = heatmap[xs, ys]\n pts, _ = self.nms_fast(pts, H, W, dist_thresh=self.nms_dist) # Apply NMS.\n inds = np.argsort(pts[2,:])\n pts = pts[:,inds[::-1]] # Sort by confidence.\n # Remove points along border.\n bord = self.border_remove\n toremoveW = np.logical_or(pts[0, :] < bord, pts[0, :] >= (W-bord))\n toremoveH = np.logical_or(pts[1, :] < bord, pts[1, :] >= (H-bord))\n toremove = np.logical_or(toremoveW, toremoveH)\n pts = pts[:, ~toremove]\n # --- Process descriptor.\n D = coarse_desc.shape[1]\n if pts.shape[1] == 0:\n desc = np.zeros((D, 0))\n else:\n # Interpolate into descriptor map using 2D point locations.\n samp_pts = torch.from_numpy(pts[:2, :].copy())\n samp_pts[0, :] = (samp_pts[0, :] / (float(W)/2.)) - 1.\n samp_pts[1, :] = (samp_pts[1, :] / (float(H)/2.)) - 1.\n samp_pts = samp_pts.transpose(0, 1).contiguous()\n samp_pts = samp_pts.view(1, 1, -1, 2)\n samp_pts = samp_pts.float()\n if self.cuda:\n samp_pts = samp_pts.cuda()\n desc = torch.nn.functional.grid_sample(coarse_desc, samp_pts)\n desc = desc.data.cpu().numpy().reshape(D, -1)\n desc /= np.linalg.norm(desc, axis=0)[np.newaxis, :]\n return pts, desc, heatmap\n\n\nclass PointTracker(object):\n \"\"\" Class to manage a fixed memory of points and descriptors that enables\n sparse optical flow point tracking.\n\n Internally, the tracker stores a 'tracks' matrix sized M x (2+L), of M\n tracks with maximum length L, where each row corresponds to:\n row_m = [track_id_m, avg_desc_score_m, point_id_0_m, ..., point_id_L-1_m].\n \"\"\"\n\n def __init__(self, max_length, nn_thresh):\n if max_length < 2:\n raise ValueError('max_length must be greater than or equal to 2.')\n self.maxl = max_length\n self.nn_thresh = nn_thresh\n self.all_pts = []\n for n in range(self.maxl):\n self.all_pts.append(np.zeros((2, 0)))\n self.last_desc = None\n self.tracks = np.zeros((0, self.maxl+2))\n self.track_count = 0\n self.max_score = 9999\n\n def nn_match_two_way(self, desc1, desc2, nn_thresh):\n \"\"\"\n Performs two-way nearest neighbor matching of two sets of descriptors, such\n that the NN match from descriptor A->B must equal the NN match from B->A.\n\n Inputs:\n desc1 - NxM numpy matrix of N corresponding M-dimensional descriptors.\n desc2 - NxM numpy matrix of N corresponding M-dimensional descriptors.\n nn_thresh - Optional descriptor distance below which is a good match.\n\n Returns:\n matches - 3xL numpy array, of L matches, where L <= N and each column i is\n a match of two descriptors, d_i in image 1 and d_j' in image 2:\n [d_i index, d_j' index, match_score]^T\n \"\"\"\n assert desc1.shape[0] == desc2.shape[0]\n if desc1.shape[1] == 0 or desc2.shape[1] == 0:\n return np.zeros((3, 0))\n if nn_thresh < 0.0:\n raise ValueError('\\'nn_thresh\\' should be non-negative')\n # Compute L2 distance. Easy since vectors are unit normalized.\n dmat = np.dot(desc1.T, desc2)\n dmat = np.sqrt(2-2*np.clip(dmat, -1, 1))\n # Get NN indices and scores.\n idx = np.argmin(dmat, axis=1)\n scores = dmat[np.arange(dmat.shape[0]), idx]\n # Threshold the NN matches.\n keep = scores < nn_thresh\n # Check if nearest neighbor goes both directions and keep those.\n idx2 = np.argmin(dmat, axis=0)\n keep_bi = np.arange(len(idx)) == idx2[idx]\n keep = np.logical_and(keep, keep_bi)\n idx = idx[keep]\n scores = scores[keep]\n # Get the surviving point indices.\n m_idx1 = np.arange(desc1.shape[1])[keep]\n m_idx2 = idx\n # Populate the final 3xN match data structure.\n matches = np.zeros((3, int(keep.sum())))\n matches[0, :] = m_idx1\n matches[1, :] = m_idx2\n matches[2, :] = scores\n return matches\n\n def get_offsets(self):\n \"\"\" Iterate through list of points and accumulate an offset value. Used to\n index the global point IDs into the list of points.\n\n Returns\n offsets - N length array with integer offset locations.\n \"\"\"\n # Compute id offsets.\n offsets = []\n offsets.append(0)\n for i in range(len(self.all_pts)-1): # Skip last camera size, not needed.\n offsets.append(self.all_pts[i].shape[1])\n offsets = np.array(offsets)\n offsets = np.cumsum(offsets)\n return offsets\n\n def update(self, pts, desc):\n \"\"\" Add a new set of point and descriptor observations to the tracker.\n\n Inputs\n pts - 3xN numpy array of 2D point observations.\n desc - DxN numpy array of corresponding D dimensional descriptors.\n \"\"\"\n if pts is None or desc is None:\n print('PointTracker: Warning, no points were added to tracker.')\n return\n assert pts.shape[1] == desc.shape[1]\n # Initialize last_desc.\n if self.last_desc is None:\n self.last_desc = np.zeros((desc.shape[0], 0))\n # Remove oldest points, store its size to update ids later.\n remove_size = self.all_pts[0].shape[1]\n self.all_pts.pop(0)\n self.all_pts.append(pts)\n # Remove oldest point in track.\n self.tracks = np.delete(self.tracks, 2, axis=1)\n # Update track offsets.\n for i in range(2, self.tracks.shape[1]):\n self.tracks[:, i] -= remove_size\n self.tracks[:, 2:][self.tracks[:, 2:] < -1] = -1\n offsets = self.get_offsets()\n # Add a new -1 column.\n self.tracks = np.hstack((self.tracks, -1*np.ones((self.tracks.shape[0], 1))))\n # Try to append to existing tracks.\n matched = np.zeros((pts.shape[1])).astype(bool)\n matches = self.nn_match_two_way(self.last_desc, desc, self.nn_thresh)\n for match in matches.T:\n # Add a new point to it's matched track.\n id1 = int(match[0]) + offsets[-2]\n id2 = int(match[1]) + offsets[-1]\n found = np.argwhere(self.tracks[:, -2] == id1)\n if found.shape[0] > 0:\n matched[int(match[1])] = True\n row = int(found)\n self.tracks[row, -1] = id2\n if self.tracks[row, 1] == self.max_score:\n # Initialize track score.\n self.tracks[row, 1] = match[2]\n else:\n # Update track score with running average.\n # NOTE(dd): this running average can contain scores from old matches\n # not contained in last max_length track points.\n track_len = (self.tracks[row, 2:] != -1).sum() - 1.\n frac = 1. / float(track_len)\n self.tracks[row, 1] = (1.-frac)*self.tracks[row, 1] + frac*match[2]\n # Add unmatched tracks.\n new_ids = np.arange(pts.shape[1]) + offsets[-1]\n new_ids = new_ids[~matched]\n new_tracks = -1*np.ones((new_ids.shape[0], self.maxl + 2))\n new_tracks[:, -1] = new_ids\n new_num = new_ids.shape[0]\n new_trackids = self.track_count + np.arange(new_num)\n new_tracks[:, 0] = new_trackids\n new_tracks[:, 1] = self.max_score*np.ones(new_ids.shape[0])\n self.tracks = np.vstack((self.tracks, new_tracks))\n self.track_count += new_num # Update the track count.\n # Remove empty tracks.\n keep_rows = np.any(self.tracks[:, 2:] >= 0, axis=1)\n self.tracks = self.tracks[keep_rows, :]\n # Store the last descriptors.\n self.last_desc = desc.copy()\n return\n\n def get_tracks(self, min_length):\n \"\"\" Retrieve point tracks of a given minimum length.\n Input\n min_length - integer >= 1 with minimum track length\n Output\n returned_tracks - M x (2+L) sized matrix storing track indices, where\n M is the number of tracks and L is the maximum track length.\n \"\"\"\n if min_length < 1:\n raise ValueError('\\'min_length\\' too small.')\n valid = np.ones((self.tracks.shape[0])).astype(bool)\n good_len = np.sum(self.tracks[:, 2:] != -1, axis=1) >= min_length\n # Remove tracks which do not have an observation in most recent frame.\n not_headless = (self.tracks[:, -1] != -1)\n keepers = np.logical_and.reduce((valid, good_len, not_headless))\n returned_tracks = self.tracks[keepers, :].copy()\n return returned_tracks\n\n def draw_tracks(self, out, tracks):\n \"\"\" Visualize tracks all overlayed on a single image.\n Inputs\n out - numpy uint8 image sized HxWx3 upon which tracks are overlayed.\n tracks - M x (2+L) sized matrix storing track info.\n \"\"\"\n # Store the number of points per camera.\n pts_mem = self.all_pts\n N = len(pts_mem) # Number of cameras/images.\n # Get offset ids needed to reference into pts_mem.\n offsets = self.get_offsets()\n # Width of track and point circles to be drawn.\n stroke = 1\n # Iterate through each track and draw it.\n for track in tracks:\n clr = myjet[int(np.clip(np.floor(track[1]*10), 0, 9)), :]*255\n for i in range(N-1):\n if track[i+2] == -1 or track[i+3] == -1:\n continue\n offset1 = offsets[i]\n offset2 = offsets[i+1]\n idx1 = int(track[i+2]-offset1)\n idx2 = int(track[i+3]-offset2)\n pt1 = pts_mem[i][:2, idx1]\n pt2 = pts_mem[i+1][:2, idx2]\n p1 = (int(round(pt1[0])), int(round(pt1[1])))\n p2 = (int(round(pt2[0])), int(round(pt2[1])))\n cv2.line(out, p1, p2, clr, thickness=stroke, lineType=16)\n # Draw end points of each track.\n if i == N-2:\n clr2 = (255, 0, 0)\n cv2.circle(out, p2, stroke, clr2, -1, lineType=16)\n\nclass VideoStreamer(object):\n \"\"\" Class to help process image streams. Three types of possible inputs:\"\n 1.) USB Webcam.\n 2.) A directory of images (files in directory matching 'img_glob').\n 3.) A video file, such as an .mp4 or .avi file.\n \"\"\"\n def __init__(self, basedir, camid, height, width, skip, img_glob):\n self.cap = []\n self.camera = False\n self.video_file = False\n self.listing = []\n self.sizer = [height, width]\n self.i = 0\n self.skip = skip\n self.maxlen = 1000000\n # If the \"basedir\" string is the word camera, then use a webcam.\n if basedir == \"camera/\" or basedir == \"camera\":\n print('==> Processing Webcam Input.')\n self.cap = cv2.VideoCapture(camid)\n self.listing = range(0, self.maxlen)\n self.camera = True\n else:\n # Try to open as a video.\n self.cap = cv2.VideoCapture(basedir)\n lastbit = basedir[-4:len(basedir)]\n if (type(self.cap) == list or not self.cap.isOpened()) and (lastbit == '.mp4'):\n raise IOError('Cannot open movie file')\n elif type(self.cap) != list and self.cap.isOpened() and (lastbit != '.txt'):\n print('==> Processing Video Input.')\n num_frames = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT))\n self.listing = range(0, num_frames)\n self.listing = self.listing[::self.skip]\n self.camera = True\n self.video_file = True\n self.maxlen = len(self.listing)\n else:\n print('==> Processing Image Directory Input.')\n search = os.path.join(basedir, img_glob)\n self.listing = glob.glob(search)\n self.listing.sort()\n self.listing = self.listing[::self.skip]\n self.maxlen = len(self.listing)\n if self.maxlen == 0:\n raise IOError('No images were found (maybe bad \\'--img_glob\\' parameter?)')\n\n def read_image(self, impath, img_size):\n \"\"\" Read image as grayscale and resize to img_size.\n Inputs\n impath: Path to input image.\n img_size: (W, H) tuple specifying resize size.\n Returns\n grayim: float32 numpy array sized H x W with values in range [0, 1].\n \"\"\"\n grayim = cv2.imread(impath, 0)\n if grayim is None:\n raise Exception('Error reading image %s' % impath)\n # Image is resized via opencv.\n interp = cv2.INTER_AREA\n grayim = cv2.resize(grayim, (img_size[1], img_size[0]), interpolation=interp)\n grayim = (grayim.astype('float32') / 255.)\n return grayim\n\n def next_frame(self):\n \"\"\" Return the next frame, and increment internal counter.\n Returns\n image: Next H x W image.\n status: True or False depending whether image was loaded.\n \"\"\"\n if self.i == self.maxlen:\n return (None, False)\n if self.camera:\n ret, input_image = self.cap.read()\n if ret is False:\n print('VideoStreamer: Cannot get image from camera (maybe bad --camid?)')\n return (None, False)\n if self.video_file:\n self.cap.set(cv2.CAP_PROP_POS_FRAMES, self.listing[self.i])\n input_image = cv2.resize(input_image, (self.sizer[1], self.sizer[0]),\n interpolation=cv2.INTER_AREA)\n input_image = cv2.cvtColor(input_image, cv2.COLOR_RGB2GRAY)\n input_image = input_image.astype('float')/255.0\n else:\n image_file = self.listing[self.i]\n input_image = self.read_image(image_file, self.sizer)\n # Increment internal counter.\n self.i = self.i + 1\n input_image = input_image.astype('float32')\n return (input_image, True)\n\n\nif __name__ == '__main__':\n\n # Parse command line arguments.\n parser = argparse.ArgumentParser(description='PyTorch SuperPoint Demo.')\n parser.add_argument('input', type=str, default='',\n help='Image directory or movie file or \"camera\" (for webcam).')\n parser.add_argument('--weights_path', type=str, default='superpoint_v1.pth',\n help='Path to pretrained weights file (default: superpoint_v1.pth).')\n parser.add_argument('--img_glob', type=str, default='*.png',\n help='Glob match if directory of images is specified (default: \\'*.png\\').')\n parser.add_argument('--skip', type=int, default=1,\n help='Images to skip if input is movie or directory (default: 1).')\n parser.add_argument('--show_extra', action='store_true',\n help='Show extra debug outputs (default: False).')\n parser.add_argument('--H', type=int, default=120,\n help='Input image height (default: 120).')\n parser.add_argument('--W', type=int, default=160,\n help='Input image width (default:160).')\n parser.add_argument('--display_scale', type=int, default=2,\n help='Factor to scale output visualization (default: 2).')\n parser.add_argument('--min_length', type=int, default=2,\n help='Minimum length of point tracks (default: 2).')\n parser.add_argument('--max_length', type=int, default=5,\n help='Maximum length of point tracks (default: 5).')\n parser.add_argument('--nms_dist', type=int, default=4,\n help='Non Maximum Suppression (NMS) distance (default: 4).')\n parser.add_argument('--conf_thresh', type=float, default=0.015,\n help='Detector confidence threshold (default: 0.015).')\n parser.add_argument('--nn_thresh', type=float, default=0.7,\n help='Descriptor matching threshold (default: 0.7).')\n parser.add_argument('--camid', type=int, default=0,\n help='OpenCV webcam video capture ID, usually 0 or 1 (default: 0).')\n parser.add_argument('--waitkey', type=int, default=1,\n help='OpenCV waitkey time in ms (default: 1).')\n parser.add_argument('--cuda', action='store_true',\n help='Use cuda GPU to speed up network processing speed (default: False)')\n parser.add_argument('--no_display', action='store_true',\n help='Do not display images to screen. Useful if running remotely (default: False).')\n parser.add_argument('--write', action='store_true',\n help='Save output frames to a directory (default: False)')\n parser.add_argument('--write_dir', type=str, default='tracker_outputs/',\n help='Directory where to write output frames (default: tracker_outputs/).')\n opt = parser.parse_args()\n print(opt)\n # code.interact( local=locals() )\n # This class helps load input images from different sources.\n vs = VideoStreamer(opt.input, opt.camid, opt.H, opt.W, opt.skip, opt.img_glob)\n\n print('==> Loading pre-trained network.')\n # This class runs the SuperPoint network and processes its outputs.\n fe = SuperPointFrontend(weights_path=opt.weights_path,\n nms_dist=opt.nms_dist,\n conf_thresh=opt.conf_thresh,\n nn_thresh=opt.nn_thresh,\n cuda=opt.cuda)\n print('==> Successfully loaded pre-trained network.')\n\n # This class helps merge consecutive point matches into tracks.\n tracker = PointTracker(opt.max_length, nn_thresh=fe.nn_thresh)\n\n # Create a window to display the demo.\n if not opt.no_display:\n win = 'SuperPoint Tracker'\n cv2.namedWindow(win)\n else:\n print('Skipping visualization, will not show a GUI.')\n\n # Font parameters for visualizaton.\n font = cv2.FONT_HERSHEY_DUPLEX\n font_clr = (255, 255, 255)\n font_pt = (4, 12)\n font_sc = 0.4\n\n # Create output directory if desired.\n if opt.write:\n print('==> Will write outputs to %s' % opt.write_dir)\n if not os.path.exists(opt.write_dir):\n os.makedirs(opt.write_dir)\n\n print('==> Running Demo.')\n while True:\n\n start = time.time()\n\n # Get a new image.\n img, status = vs.next_frame()\n if status is False:\n break\n\n # Get points and descriptors.\n start1 = time.time()\n pts, desc, heatmap = fe.run(img)\n # import code\n # code.interact( local=locals() )\n end1 = time.time()\n\n # Add points and descriptors to the tracker.\n tracker.update(pts, desc)\n\n # Get tracks for points which were match successfully across all frames.\n tracks = tracker.get_tracks(opt.min_length)\n\n # Primary output - Show point tracks overlayed on top of input image.\n out1 = (np.dstack((img, img, img)) * 255.).astype('uint8')\n tracks[:, 1] /= float(fe.nn_thresh) # Normalize track scores to [0,1].\n tracker.draw_tracks(out1, tracks)\n if opt.show_extra:\n cv2.putText(out1, 'Point Tracks', font_pt, font, font_sc, font_clr, lineType=16)\n\n # Extra output -- Show current point detections.\n out2 = (np.dstack((img, img, img)) * 255.).astype('uint8')\n for pt in pts.T:\n pt1 = (int(round(pt[0])), int(round(pt[1])))\n cv2.circle(out2, pt1, 1, (0, 255, 0), -1, lineType=16)\n cv2.putText(out2, 'Raw Point Detections', font_pt, font, font_sc, font_clr, lineType=16)\n\n # Extra output -- Show the point confidence heatmap.\n if heatmap is not None:\n min_conf = 0.001\n heatmap[heatmap < min_conf] = min_conf\n heatmap = -np.log(heatmap)\n heatmap = (heatmap - heatmap.min()) / (heatmap.max() - heatmap.min() + .00001)\n out3 = myjet[np.round(np.clip(heatmap*10, 0, 9)).astype('int'), :]\n out3 = (out3*255).astype('uint8')\n else:\n out3 = np.zeros_like(out2)\n cv2.putText(out3, 'Raw Point Confidences', font_pt, font, font_sc, font_clr, lineType=16)\n\n # Resize final output.\n if opt.show_extra:\n out = np.hstack((out1, out2, out3))\n out = cv2.resize(out, (3*opt.display_scale*opt.W, opt.display_scale*opt.H))\n else:\n out = cv2.resize(out1, (opt.display_scale*opt.W, opt.display_scale*opt.H))\n\n # Display visualization image to screen.\n if not opt.no_display:\n cv2.imshow(win, out)\n key = cv2.waitKey(opt.waitkey) & 0xFF\n if key == ord('q'):\n print('Quitting, \\'q\\' pressed.')\n break\n\n # Optionally write images to disk.\n if opt.write:\n out_file = os.path.join(opt.write_dir, 'frame_%05d.png' % vs.i)\n print('Writing image to %s' % out_file)\n cv2.imwrite(out_file, out)\n\n end = time.time()\n net_t = (1./ float(end1 - start))\n total_t = (1./ float(end - start))\n if opt.show_extra:\n print('Processed image %d (net+post_process: %.2f FPS, total: %.2f FPS).'\\\n % (vs.i, net_t, total_t))\n\n # Close any remaining windows.\n cv2.destroyAllWindows()\n\n print('==> Finshed Demo.')\n" }, { "path": "scripts/unittest/rtry_superpoint.py", "content": "#!/usr/bin/env python\n\nimport numpy as np\nimport code\nimport cv2\nimport time\n\nimport json\n\nimport torch\n# Stub to warn about opencv version.\nif int(cv2.__version__[0]) < 3: # pragma: no cover\n print('Warning: OpenCV 3 is not installed')\n\nfrom demo_superpoint import *\n\nclass KSuperPointExp:\n def __init__(self):\n #------------------------------------------------------------------------\n # Setup Neural Net\n weights_path='superpoint_v1.pth'\n nms_dist=4\n conf_thresh=0.015\n nn_thresh=0.7\n cuda=True\n print('==> Loading pre-trained network.')\n # This class runs the SuperPoint network and processes its outputs.\n self.fe = SuperPointFrontend(weights_path=weights_path,\n nms_dist=nms_dist,\n conf_thresh=conf_thresh,\n nn_thresh=nn_thresh,\n cuda=cuda)\n print('==> Successfully loaded pre-trained network.')\n\n\n #-----------------------------------------------------------------------\n # Tracker - NN comparison for desc\n max_length = 5 # default as in the demo file\n self.tracker = PointTracker(max_length, nn_thresh=self.fe.nn_thresh)\n\n\n def plot_pts( self, xcanvas, pts ):\n # pts: [x_i, y_i, confidence_i]. 3xN\n for i in range( pts.shape[1] ):\n cv2.circle( xcanvas, (int(pts[0,i]), int(pts[1,i]) ), 1, (0,0,255), -1 )\n\n return xcanvas\n\n def plot_point_sets( self, im0, pts0, im1, pts1 ):\n # pts: [x_i, y_i, confidence_i]. 3xN\n # try:\n xcanvas = np.concatenate( (im0, im1), axis=1 )\n if pts0.shape[0] == 0:\n return xcanvas\n # except:\n # code.interact( local=locals() )\n assert( pts0.shape[1] == pts1.shape[1] )\n assert( pts0.shape[0] == 3 or pts0.shape[0] == 2 )\n assert( pts1.shape[0] == 3 or pts1.shape[0] == 2 )\n\n\n for i in range( pts0.shape[1] ):\n pt0 = (int(pts0[0,i]), int(pts0[1,i]) )\n pt1 = (int(pts1[0,i] + im0.shape[1] ), int(pts1[1,i]) )\n cv2.circle( xcanvas, pt0, 1, (0,255,0), -1)\n cv2.circle( xcanvas, pt1, 1, (0,255,0), -1)\n cv2.line( xcanvas, pt0, pt1, (0,255,0) )\n\n return xcanvas\n\n\n\n def read_image(self, impath, img_size):\n \"\"\" Read image as grayscale and resize to img_size.\n Inputs\n impath: Path to input image.\n img_size: (W, H) tuple specifying resize size.\n Returns\n grayim: float32 numpy array sized H x W with values in range [0, 1].\n \"\"\"\n grayim = cv2.imread(impath, 0)\n if grayim is None:\n raise Exception('Error reading image %s' % impath)\n # Image is resized via opencv.\n interp = cv2.INTER_AREA\n grayim = cv2.resize(grayim, (img_size[1], img_size[0]), interpolation=interp)\n grayim = (grayim.astype('float32') / 255.)\n return grayim\n\n\n def get_descriptor( self, image ):\n \"\"\" Given an image returns the keypoints and descriptors.\n Input\n img - HxW numpy float32 input image in range [0,1].\n Output\n corners - 3xN numpy array with corners [x_i, y_i, confidence_i]^T.\n desc - 256xN numpy array of corresponding unit normalized descriptors.\n heatmap - HxW numpy heatmap in range [0,1] of point confidences.\n \"\"\"\n start1 = time.time()\n pts, desc, heatmap = self.fe.run(image)\n print 'superpoint computed in %4.2fms' %(1000. * (time.time() - start1))\n return pts, desc, heatmap\n\n\n def match_image_pair( self, fname0, fname1 ):\n \"\"\" Given two image file names, read the two images (as per SuperPoint's requirement).\n Computes descriptors and then does NN comparison.\n \"\"\"\n\n ksp_image0 = ksp.read_image(fname0, [240,320] )\n ksp_image1 = ksp.read_image(fname1, [240,320] )\n image0 = cv2.imread( fname0 )\n image1 = cv2.imread( fname1 )\n\n pts0, desc0, heatmap0 = ksp.get_descriptor( ksp_image0 )\n pts1, desc1, heatmap1 = ksp.get_descriptor( ksp_image1 )\n print 'len(pts0)=%d, len(pts1)=%d, desc0.shape=%s, desc1.shape=%s' %(pts0.shape[1], pts1.shape[1], str(desc0.shape), str(desc1.shape) )\n\n start_matching = time.time()\n matches = self.tracker.nn_match_two_way( desc0, desc1, self.fe.nn_thresh )\n sel_pts0 = np.array([ pts0[:,i] for i in matches[0,:].astype('int32') ]).transpose()\n sel_pts1 = np.array([ pts1[:,i] for i in matches[1,:].astype('int32') ]).transpose()\n print 'matching dones in %4.2fms' %(1000. * (time.time() - start_matching) )\n print 'matches=%d' %( matches.shape[1] )\n if matches.shape[1] == 0:\n print 'NO MATCHES'\n return None, None\n\n # Plotting\n # xcanvas0 = self.plot_pts(image0.copy(), pts0 )\n # xcanvas1 = self.plot_pts(image1.copy(), pts1 )\n # xcanvas_ = self.plot_point_sets( xcanvas0, sel_pts0, xcanvas1, sel_pts1 )\n # xcanvas_ = self.plot_point_sets( image0.copy(), sel_pts0, image1.copy(), sel_pts1 )\n # cv2.imshow( 'pts0', xcanvas0 )\n # cv2.imshow( 'pts1', xcanvas1 )\n # cv2.imshow( 'joint', xcanvas_ )\n\n return sel_pts0[0:2,:].transpose() , sel_pts1[0:2,:].transpose()\n # code.interact( local=locals() )\n\n\nif __name__ == '__main__':\n BASE = '/Bulk_Data/_tmp_cerebro/bb4_multiple_loops_in_lab/'\n\n\n #\n # Open Log File\n #\n LOG_FILE_NAME = BASE+'/log.json'\n print 'Open file: ', LOG_FILE_NAME\n with open(LOG_FILE_NAME) as data_file:\n data = json.load(data_file)\n\n\n #\n # Init Keras Model\n ksp = KSuperPointExp()\n\n if True:\n i = 534\n im = cv2.imread( BASE+'%d.jpg' %(i) )\n\n im_float = cv2.cvtColor( cv2.resize(im, (0,0), fx=0.5, fy=0.5), cv2.COLOR_BGR2GRAY ).astype('float32') / 255.\n print 'im_float.shape=', im_float.shape, '\\tdtype=', im_float.dtype\n\n sup_pts, sup_desc, sup_heatmap = ksp.get_descriptor( im_float )\n cv2.imshow( str(i), ksp.plot_pts( im, sup_pts*2 ) )\n\n if True:\n i = 1638\n im = cv2.imread( BASE+'%d.jpg' %(i) )\n\n im_float = cv2.cvtColor( cv2.resize(im, (0,0), fx=0.5, fy=0.5), cv2.COLOR_BGR2GRAY ).astype('float32') / 255.\n print 'im_float.shape=', im_float.shape, '\\tdtype=', im_float.dtype\n\n sup_pts, sup_desc, sup_heatmap = ksp.get_descriptor( im_float )\n cv2.imshow( str(i), ksp.plot_pts( im, sup_pts*2 ) )\n\n cv2.waitKey(0)\n\n\nif __name__ == \"__m1ain__\":\n BASE = '/Bulk_Data/_tmp_cerebro/bb4_multiple_loops_in_lab/'\n\n\n #\n # Open Log File\n #\n LOG_FILE_NAME = BASE+'/log.json'\n print 'Open file: ', LOG_FILE_NAME\n with open(LOG_FILE_NAME) as data_file:\n data = json.load(data_file)\n\n\n #\n # Init Keras Model\n ksp = KSuperPointExp()\n\n\n\n # Loops over all images and precomputes their netvlad vector\n for i in range( len(data['DataNodes']) ):\n a = data['DataNodes'][i]['isKeyFrame']\n b = data['DataNodes'][i]['isImageAvailable']\n c = data['DataNodes'][i]['isPoseAvailable']\n d = data['DataNodes'][i]['isPtCldAvailable']\n\n\n if not ( a==1 and b==1 and c==1 and d==1 ): #only process keyframes which have pose and ptcld info\n continue\n\n im = cv2.imread( BASE+'%d.jpg' %(i) )\n\n im_float = cv2.cvtColor( cv2.resize(im, (0,0), fx=0.5, fy=0.5), cv2.COLOR_BGR2GRAY ).astype('float32') / 255.\n print 'im_float.shape=', im_float.shape, '\\tdtype=', im_float.dtype\n\n start_time = time.time()\n sup_pts, sup_desc, sup_heatmap = ksp.get_descriptor( im_float )\n print '---', i , '\\n',\n print 'Done in %4.4fms' %( 1000. * (time.time() - start_time ) )\n\n cv2.imshow( 'im_superpoints', ksp.plot_pts( im, sup_pts*2 ) )\n cv2.imshow( 'im', im )\n key = cv2.waitKey(10)\n if key == ord( 'q' ):\n break\n" }, { "path": "scripts/whole_image_desc_compute_client.py", "content": "#!/usr/bin/env python\n\n# Sample Client to call the `whole_image_descriptor_compute_server`\n\nfrom cerebro.srv import *\nimport rospy\nimport numpy as np\nimport cv2\n\nfrom cv_bridge import CvBridge, CvBridgeError\nfrom sensor_msgs.msg import Image\n\nrospy.wait_for_service( 'whole_image_descriptor_compute' )\ntry:\n res = rospy.ServiceProxy( 'whole_image_descriptor_compute', WholeImageDescriptorCompute )\n\n # X = np.zeros( (100, 100), dtype=np.uint8 )\n X = cv2.resize( cv2.imread( '/app/lena.jpg' ), (752,480) )\n i = CvBridge().cv2_to_imgmsg( X )\n u = res( i, 23 )\n print 'received: ', u\nexcept rospy.ServiceException, e:\n print 'failed', e\n" }, { "path": "scripts/whole_image_desc_compute_server.py", "content": "#!/usr/bin/env python\n\nfrom cerebro.srv import *\nimport rospy\n\nfrom cv_bridge import CvBridge, CvBridgeError\nimport cv2\nimport numpy as np\nimport code\nimport time\nimport os\nimport scipy.io\nimport hashlib\n\nimport keras\nfrom keras_helpers import *\nfrom predict_utils import *\nfrom TerminalColors import bcolors\ntcol = bcolors()\n\nimport rospkg\nTHIS_PKG_BASE_PATH = rospkg.RosPack().get_path('cerebro')\n\nfrom numpy.random import seed\nseed(42)\n\nclass SampleGPUComputer:\n def __init__(self):\n self.model = keras.applications.vgg16.VGG16( weights=None)\n self.model._make_predict_function()\n self.model.summary()\n\n\n\n def handle_req( self, req ):\n print 'Handle request '\n\n\n # print req\n cv_image = CvBridge().imgmsg_to_cv2( req.ima )\n print 'cv_image.shape', cv_image.shape\n # cv2.imshow( 'cv_image from server', cv_image )\n # cv2.waitKey(0)\n\n #\n # compute descriptor\n #\n # code.interact( local=locals() )\n im = cv2.resize( cv_image, (224,224) )\n self.model._make_predict_function()\n preds = self.model.predict( np.expand_dims(im,0).astype('float32') )\n print preds\n\n\n #\n # Return the descriptor in the message\n #\n result = WholeImageDescriptorComputeResponse()\n result.desc = [ cv_image.shape[0], cv_image.shape[1] ]\n return result\n\nclass ReljaNetVLAD:\n def __init__(self, im_rows=600, im_cols=960, im_chnls=3):\n ## Build net\n from keras.backend.tensorflow_backend import set_session\n import tensorflow as tf\n tf.set_random_seed(42)\n config = tf.ConfigProto()\n config.gpu_options.per_process_gpu_memory_fraction = 0.1\n # config.gpu_options.visible_device_list = \"0\"\n set_session(tf.Session(config=config))\n\n self.im_rows = int(im_rows)\n self.im_cols = int(im_cols)\n self.im_chnls = int(im_chnls)\n\n\n input_img = keras.layers.Input( batch_shape=(1,self.im_rows, self.im_cols, self.im_chnls ) )\n cnn = make_from_vgg16( input_img, weights=None, layer_name='block5_pool' )\n out, out_amap = NetVLADLayer(num_clusters = 64)( cnn )\n model = keras.models.Model( inputs=input_img, outputs=out )\n\n DATA_DIR = '/app_learning/cartwheel_train/relja_matlab_weight.dump/'\n print 'Relja Data Dir: ', DATA_DIR\n model.load_weights( DATA_DIR+'/matlab_model.keras' )\n WPCA_M = scipy.io.loadmat( DATA_DIR+'/WPCA_1.mat' )['the_mat'] # 1x1x32768x4096\n WPCA_b = scipy.io.loadmat( DATA_DIR+'/WPCA_2.mat' )['the_mat'] # 4096x1\n WPCA_M = WPCA_M[0,0] # 32768x4096\n WPCA_b = np.transpose(WPCA_b) #1x4096\n\n\n self.model = model\n self.WPCA_M = WPCA_M\n self.WPCA_b = WPCA_b\n self.model_type = 'relja_matlab_model'\n\n\n\n # Doing this is a hack to force keras to allocate GPU memory. Don't comment this,\n tmp_zer = np.zeros( (1,self.im_rows,self.im_cols,self.im_chnls), dtype='float32' )\n tmp_zer_out = self.model.predict( tmp_zer )\n tmp_zer_out = np.matmul( tmp_zer_out, self.WPCA_M ) + self.WPCA_b\n tmp_zer_out /= np.linalg.norm( tmp_zer_out )\n print 'model input.shape=', tmp_zer.shape, '\\toutput.shape=', tmp_zer_out.shape\n print 'model_type=', self.model_type\n\n print '-----'\n print '\\tinput_image.shape=', tmp_zer.shape\n print '\\toutput.shape=', tmp_zer_out.shape\n print '\\tminmax=', np.min( tmp_zer_out ), np.max( tmp_zer_out )\n print '\\tnorm=', np.linalg.norm( tmp_zer_out )\n print '\\tdtype=', tmp_zer_out.dtype\n print '-----'\n\n\n def handle_req( self, req ):\n ## Get Image out of req\n cv_image = CvBridge().imgmsg_to_cv2( req.ima )\n print '[Handle Request] cv_image.shape', cv_image.shape, '\\ta=', req.a, '\\tt=', req.ima.header.stamp\n\n\n assert (cv_image.shape[0] == self.im_rows and\n cv_image.shape[1] == self.im_cols and\n cv_image.shape[2] == self.im_chnls),\\\n \"\\n[whole_image_descriptor_compute_server] Input shape of the image \\\n does not match with the allocated GPU memory. Expecting an input image of \\\n size %dx%dx%d, but received : %s\" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )\n\n # cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )\n # cv2.waitKey(10)\n # cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )\n\n ## Compute Descriptor\n start_time = time.time()\n\n # Normalize image\n cv_image_rgb = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)\n avg_image = [122.6778, 116.6522, 103.9997]\n cv_image_rgb[:,:,0]= cv_image_rgb[:,:,0] - avg_image[0]\n cv_image_rgb[:,:,1]= cv_image_rgb[:,:,1] - avg_image[1]\n cv_image_rgb[:,:,2]= cv_image_rgb[:,:,2] - avg_image[2]\n\n\n # predict\n u = self.model.predict( np.expand_dims( cv_image_rgb.astype('float32'), 0 ) )\n\n # WPCA\n u = np.matmul( u, self.WPCA_M ) + self.WPCA_b\n u /= np.linalg.norm( u )\n\n print 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ),\n print '\\tdesc.shape=', u.shape,\n print '\\tinput_image.shape=', cv_image.shape,\n print '\\tminmax=', np.min( u ), np.max( u ),\n print '\\tmodel_type=', self.model_type,\n print '\\tdtype=', cv_image.dtype\n\n\n\n ## Populate output message\n result = WholeImageDescriptorComputeResponse()\n # result.desc = [ cv_image.shape[0], cv_image.shape[1] ]\n result.desc = u[0,:]\n result.model_type = self.model_type\n return result\n\n\n\n\nclass NetVLADImageDescriptor:\n def __init__(self, im_rows=600, im_cols=960, im_chnls=3):\n ## Build net\n from keras.backend.tensorflow_backend import set_session\n import tensorflow as tf\n tf.set_random_seed(42)\n config = tf.ConfigProto()\n config.gpu_options.per_process_gpu_memory_fraction = 0.1\n # config.gpu_options.visible_device_list = \"0\"\n set_session(tf.Session(config=config))\n\n # Blackbox 4\n # self.im_rows = 512\n # self.im_cols = 640\n # self.im_chnls = 3\n\n # point grey\n # self.im_rows = 600\n # self.im_cols = 960\n # self.im_chnls = 3\n\n # EuroC\n # self.im_rows = 480\n # self.im_cols = 752\n # self.im_chnls = 3\n\n self.im_rows = int(im_rows)\n self.im_cols = int(im_cols)\n self.im_chnls = int(im_chnls)\n\n\n #----- @ INPUT LAYER\n input_img = keras.layers.Input( shape=(self.im_rows, self.im_cols, self.im_chnls) )\n\n #----- @ CNN\n # cnn = make_from_vgg16( input_img, weights=None, layer_name='block5_pool' )\n cnn = make_from_mobilenet( input_img, weights=None, layer_name='conv_pw_7_relu' )\n\n #----- @ DOWN-SAMPLE LAYER (OPTINAL)\n if False: #Downsample last layer (Reduce nChannels of the output.)\n cnn_dwn = keras.layers.Conv2D( 256, (1,1), padding='same', activation='relu' )( cnn )\n cnn_dwn = keras.layers.normalization.BatchNormalization()( cnn_dwn )\n cnn_dwn = keras.layers.Conv2D( 64, (1,1), padding='same', activation='relu' )( cnn_dwn )\n cnn_dwn = keras.layers.normalization.BatchNormalization()( cnn_dwn )\n cnn = cnn_dwn\n\n #----- @ NetVLADLayer\n # out, out_amap = NetVLADLayer(num_clusters = 16)( cnn )\n out = NetVLADLayer(num_clusters = 16)( cnn )\n\n model = keras.models.Model( inputs=input_img, outputs=out )\n model.summary()\n # `model_visual_fname` as None will disable writing the image file.\n model_visual_fname = None\n # model_visual_fname = '/app/core.png'\n if model_visual_fname is not None:\n print 'Writing Model Visual to: ', model_visual_fname\n keras.utils.plot_model( model, to_file=model_visual_fname, show_shapes=True )\n\n\n #----- @ Load Model Weights\n #TODO Read from config file the path of keras model\n # model_file = '/app/catkin_ws/src/cerebro/scripts/keras.models/core_model.keras'\n # model_type = 'test_keras_model'\n\n # model_file = '/app_learning/cartwheel_train/models.keras/vgg16/block5_pool_k16_tripletloss2/core_model.keras'\n # model_type = 'block5_pool_k16_tripletloss2'\n\n\n # model_file = '/app_learning/cartwheel_train/models.keras/mobilenet_conv7_quash_chnls_allpairloss/core_model.keras'\n # model_type = 'mobilenet_conv7_quash_chnls_allpairloss'\n\n #model_file = '/app_learning/cartwheel_train/models.keras/mobilenet_conv7_allpairloss/core_model.keras'\n #model_type = 'mobilenet_conv7_allpairloss'\n\n # model_file = '/app_learning/cartwheel_train/models.keras/mobilenet_new/pw13_quash_chnls_k16_allpairloss/core_model.1800.keras'\n # model_type = 'pw13_quash_chnls_k16_allpairloss'\n\n # model_file = '/app_learning/cartwheel_train/models.keras/vgg16_new/block5_pool_k16_tripletloss2/core_model.keras'\n # model_type = 'block5_pool_k16_tripletloss2'\n\n # model_file = '/app_learning/cartwheel_train/models.keras/mobilenet_new/pw13_quash_chnls_k16_allpairloss/core_model.800.keras'\n # model_type = 'pw13_quash_chnls_k16_allpairloss'\n\n model_file = THIS_PKG_BASE_PATH+'/scripts/keras.models/mobilenet_conv7_allpairloss.keras'\n model_type = 'mobilenet_conv7_allpairloss'\n\n print 'model_file: ', model_file\n model.load_weights( model_file )\n\n\n\n # Write model as json to file, for info.\n # if False:\n # model.summary()\n # keras.utils.plot_model( model, to_file='/tmp/xcore.png', show_shapes=True )\n # with open('/tmp/xmodel.json', 'w') as outfile:\n # json.dump(model.to_json(), outfile, indent=4 )\n\n self.model = model\n self.model_type = model_type\n # ! Done...!\n\n # Doing this is a hack to force keras to allocate GPU memory. Don't comment this,\n tmp_zer = np.zeros( (1,self.im_rows,self.im_cols,self.im_chnls), dtype='float32' )\n tmp_zer_out = self.model.predict( tmp_zer )\n print 'model input.shape=', tmp_zer.shape, '\\toutput.shape=', tmp_zer_out.shape\n print 'model_type=', self.model_type\n\n print '-----'\n print '\\tinput_image.shape=', tmp_zer.shape\n print '\\toutput.shape=', tmp_zer_out.shape\n print '\\tminmax=', np.min( tmp_zer_out ), np.max( tmp_zer_out )\n print '\\tdtype=', tmp_zer_out.dtype\n print '-----'\n\n\n\n def handle_req( self, req ):\n ## Get Image out of req\n cv_image = CvBridge().imgmsg_to_cv2( req.ima )\n print '[Handle Request] cv_image.shape', cv_image.shape, '\\ta=', req.a, '\\tt=', req.ima.header.stamp\n\n if len(cv_image.shape) == 2:\n cv_image = np.expand_dims( cv_image, -1 );\n\n assert (cv_image.shape[0] == self.im_rows and\n cv_image.shape[1] == self.im_cols and\n cv_image.shape[2] == self.im_chnls),\\\n \"\\n[whole_image_descriptor_compute_server] Input shape of the image \\\n does not match with the allocated GPU memory. Expecting an input image of \\\n size %dx%dx%d, but received : %s\" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )\n\n # cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )\n # cv2.waitKey(10)\n # cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )\n\n ## Compute Descriptor\n start_time = time.time()\n u = self.model.predict( np.expand_dims( cv_image.astype('float32'), 0 ) )\n print 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ),\n print '\\tdesc.shape=', u.shape,\n print '\\tinput_image.shape=', cv_image.shape,\n print '\\tminmax=', np.min( u ), np.max( u ),\n print '\\tnorm=', np.linalg.norm(u[0]),\n print '\\tmodel_type=', self.model_type,\n print '\\tdtype=', cv_image.dtype\n\n\n\n ## Populate output message\n result = WholeImageDescriptorComputeResponse()\n # result.desc = [ cv_image.shape[0], cv_image.shape[1] ]\n result.desc = u[0,:]\n result.model_type = self.model_type\n return result\n\n\nclass JSONModelImageDescriptor:\n \"\"\"\n This class loads the net structure from the json file, model.json and\n weights from core_model.??.keras. In the argument `kerasmodel_file`\n you need to specify the core_model.??.keras full path (keras model file).\n The model.json need to exist in that folder.\n \"\"\"\n def __init__(self, kerasmodel_file, im_rows=600, im_cols=960, im_chnls=3):\n ## Build net\n from keras.backend.tensorflow_backend import set_session\n import tensorflow as tf\n tf.set_random_seed(42)\n config = tf.ConfigProto()\n config.gpu_options.per_process_gpu_memory_fraction = 0.2\n # config.gpu_options.visible_device_list = \"0\"\n set_session(tf.Session(config=config))\n\n # Blackbox 4\n # self.im_rows = 512\n # self.im_cols = 640\n # self.im_chnls = 3\n\n # point grey\n # self.im_rows = 600\n # self.im_cols = 960\n # self.im_chnls = 3\n\n # EuroC\n # self.im_rows = 480\n # self.im_cols = 752\n # self.im_chnls = 3\n\n self.im_rows = int(im_rows)\n self.im_cols = int(im_cols)\n self.im_chnls = int(im_chnls)\n\n LOG_DIR = '/'.join( kerasmodel_file.split('/')[0:-1] )\n print '+++++++++++++++++++++++++++++++++++++++++++++++++++++++'\n print '++++++++++ (JSONModelImageDescriptor) LOG_DIR=', LOG_DIR\n print '++++++++++ im_rows=', im_rows, ' im_cols=', im_cols, ' im_chnls=', im_chnls\n print '+++++++++++++++++++++++++++++++++++++++++++++++++++++++'\n model_type = LOG_DIR.split('/')[-1]\n # code.interact( local=locals() )\n\n assert os.path.isdir( LOG_DIR ), \"The LOG_DIR doesnot exist, or there is a permission issue. LOG_DIR=\"+LOG_DIR\n assert os.path.isfile( LOG_DIR+'/model.json' ), \"model.json does not exist in LOG_DIR=\"+LOG_DIR+'. This file is needed to load the network architecture from json format. This file should have been created when you learned the model using script in github.com/mpkuse/cartwheel_train'\n\n\n\n #----- @ Load Model Structure from JSON\n # Load JSON formatted model\n json_string = open_json_file( LOG_DIR+'/model.json' )\n # print '======================='\n # pprint.pprint( json_string, indent=4 )\n # print '======================='\n model = keras.models.model_from_json(str(json_string), custom_objects={'NetVLADLayer': NetVLADLayer, 'GhostVLADLayer':GhostVLADLayer} )\n old_input_shape = model._layers[0].input_shape\n\n model._layers[0]\n print 'OLD MODEL: ', 'input_shape=', str(old_input_shape)\n model.summary()\n model_visual_fname = None\n # model_visual_fname = '/app/core.png'\n if model_visual_fname is not None:\n print 'Writing Model Visual to: ', model_visual_fname\n keras.utils.plot_model( model, to_file=model_visual_fname, show_shapes=True )\n\n #----- @ Load Weights\n assert os.path.isfile( kerasmodel_file ), 'The model weights file doesnot exists or there is a permission issue.'+\"kerasmodel_file=\"+kerasmodel_file\n model_fname = kerasmodel_file\n print tcol.OKGREEN, 'Load model: ', model_fname, tcol.ENDC\n model.load_weights( model_fname )\n\n\n # Replace Input Layer\n new_model = change_model_inputshape( model, new_input_shape=(1,self.im_rows,self.im_cols,self.im_chnls) )\n new_input_shape = new_model._layers[0].input_shape\n print 'OLD MODEL: ', 'input_shape=', str(old_input_shape)\n print 'NEW MODEL: input_shape=', str(new_input_shape)\n\n self.model = new_model\n self.model_type = model_type\n\n\n # Doing this is a hack to force keras to allocate GPU memory. Don't comment this,\n tmp_zer = np.zeros( (1,self.im_rows,self.im_cols,self.im_chnls), dtype='float32' )\n tmp_zer_out = self.model.predict( tmp_zer )\n print 'model input.shape=', tmp_zer.shape, '\\toutput.shape=', tmp_zer_out.shape\n print 'model_type=', self.model_type\n\n print '-----'\n print '\\tinput_image.shape=', tmp_zer.shape\n print '\\toutput.shape=', tmp_zer_out.shape\n print '\\tminmax=', np.min( tmp_zer_out ), np.max( tmp_zer_out )\n print '\\tnorm=', np.linalg.norm( tmp_zer_out )\n print '\\tdtype=', tmp_zer_out.dtype\n print '-----'\n\n\n\n def handle_req( self, req ):\n \"\"\" The received image from CV bridge has to be [0,255]. In function makes it to\n intensity range [-0.5 to 0.5]\n \"\"\"\n ## Get Image out of req\n cv_image = CvBridge().imgmsg_to_cv2( req.ima )\n print '[Handle Request] cv_image.shape', cv_image.shape, '\\ta=', req.a, '\\tt=', req.ima.header.stamp\n if len(cv_image.shape)==2:\n # print 'Input dimensions are NxM but I am expecting it to be NxMxC, so np.expand_dims'\n cv_image = np.expand_dims( cv_image, -1 )\n elif len( cv_image.shape )==3:\n pass\n else:\n assert( False )\n\n\n\n\n assert (cv_image.shape[0] == self.im_rows and\n cv_image.shape[1] == self.im_cols and\n cv_image.shape[2] == self.im_chnls),\\\n \"\\n[whole_image_descriptor_compute_server] Input shape of the image \\\n does not match with the allocated GPU memory. Expecting an input image of \\\n size %dx%dx%d, but received : %s\" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )\n\n # cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )\n # cv2.waitKey(10)\n # cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )\n\n ## Compute Descriptor\n start_time = time.time()\n # i__image = np.expand_dims( cv_image.astype('float32'), 0 )\n # i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)/255. [-0.5,0.5]\n i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)*2.0/255. #[-1,1]\n\n u = self.model.predict( i__image )\n\n print tcol.HEADER, 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ), tcol.ENDC\n print '\\tinput_image.shape=', cv_image.shape,\n print '\\tinput_image dtype=', cv_image.dtype\n print tcol.OKBLUE, '\\tinput image (to neuralnet) minmax=', np.min( i__image ), np.max( i__image ), tcol.ENDC\n print '\\tdesc.shape=', u.shape,\n print '\\tdesc minmax=', np.min( u ), np.max( u ),\n print '\\tnorm=', np.linalg.norm(u[0])\n print '\\tmodel_type=', self.model_type\n\n\n\n ## Populate output message\n result = WholeImageDescriptorComputeResponse()\n # result.desc = [ cv_image.shape[0], cv_image.shape[1] ]\n result.desc = u[0,:]\n result.model_type = self.model_type\n return result\n\n\n\nclass HDF5ModelImageDescriptor:\n \"\"\"\n This class loads the net structure from the .h5 file. This file contains\n the model weights as well as architecture details.\n In the argument `kerasmodel_file`\n you need to specify the core_model.??.keras full path (keras model file).\n\n \"\"\"\n def __init__(self, kerasmodel_file, im_rows=600, im_cols=960, im_chnls=3):\n ## Build net\n from keras.backend.tensorflow_backend import set_session\n import tensorflow as tf\n tf.set_random_seed(42)\n config = tf.ConfigProto()\n config.gpu_options.per_process_gpu_memory_fraction = 0.2\n # config.gpu_options.visible_device_list = \"0\"\n set_session(tf.Session(config=config))\n keras.backend.tensorflow_backend.set_learning_phase(0)\n\n self.request_count = 0\n\n # Blackbox 4\n # self.im_rows = 512\n # self.im_cols = 640\n # self.im_chnls = 3\n\n # point grey\n # self.im_rows = 600\n # self.im_cols = 960\n # self.im_chnls = 3\n\n # EuroC\n # self.im_rows = 480\n # self.im_cols = 752\n # self.im_chnls = 3\n\n self.im_rows = int(im_rows)\n self.im_cols = int(im_cols)\n self.im_chnls = int(im_chnls)\n\n LOG_DIR = '/'.join( kerasmodel_file.split('/')[0:-1] )\n print '+++++++++++++++++++++++++++++++++++++++++++++++++++++++'\n print '++++++++++ (HDF5ModelImageDescriptor) LOG_DIR=', LOG_DIR\n print '++++++++++ im_rows=', im_rows, ' im_cols=', im_cols, ' im_chnls=', im_chnls\n print '+++++++++++++++++++++++++++++++++++++++++++++++++++++++'\n model_type = LOG_DIR.split('/')[-1]\n\n\n assert os.path.isdir( LOG_DIR ), \"The LOG_DIR doesnot exist, or there is a permission issue. LOG_DIR=\"+LOG_DIR\n\n # See if cached file exists, if yes load it else do as usual\n digest = '/tmp/'+str(hashlib.sha256(kerasmodel_file).hexdigest())+'.h5'\n print 'Check for exisitance of file', digest\n if os.path.isfile(digest):\n print 'The cached file, ', digest, ' seems to exists, load that file'\n print tcol.OKGREEN, 'Load model (from cache): ', digest, tcol.ENDC\n model = keras.models.load_model( digest, custom_objects={'NetVLADLayer': NetVLADLayer, 'GhostVLADLayer':GhostVLADLayer} )\n model.summary()\n print tcol.OKGREEN, 'Load model (from cache): ', digest, tcol.ENDC\n\n self.model = model\n self.model_type = model_type\n\n else:\n # Load from HDF5\n assert os.path.isfile( kerasmodel_file ), 'The model weights file doesnot exists or there is a permission issue.'+\"kerasmodel_file=\"+kerasmodel_file\n model_fname = kerasmodel_file\n print tcol.OKGREEN, 'Load model: ', model_fname, tcol.ENDC\n model = keras.models.load_model( model_fname, custom_objects={'NetVLADLayer': NetVLADLayer, 'GhostVLADLayer':GhostVLADLayer} )\n old_input_shape = model._layers[0].input_shape\n print 'OLD MODEL: ', 'input_shape=', str(old_input_shape)\n model.summary()\n model_visual_fname = None\n if model_visual_fname is not None:\n print 'Writing Model Visual to: ', model_visual_fname\n keras.utils.plot_model( model, to_file=model_visual_fname, show_shapes=True )\n\n\n\n # Replace Input Layer\n new_model = change_model_inputshape( model, new_input_shape=(1,self.im_rows,self.im_cols,self.im_chnls) )\n new_input_shape = new_model._layers[0].input_shape\n print 'OLD MODEL: ', 'input_shape=', str(old_input_shape)\n print 'NEW MODEL: input_shape=', str(new_input_shape)\n\n self.model = new_model\n self.model_type = model_type\n\n # Write to cache\n if True:\n digest = '/tmp/'+str(hashlib.sha256(kerasmodel_file).hexdigest())+'.h5'\n print tcol.WARNING, '}}}}}}}}\\nWrite out cache in '+digest+'\\n}}}}}}}}', tcol.ENDC\n self.model.save( digest )\n\n\n # Doing this is a hack to force keras to allocate GPU memory. Don't comment this,\n tmp_zer = np.zeros( (1,self.im_rows,self.im_cols,self.im_chnls), dtype='float32' )\n tmp_zer_out = self.model.predict( tmp_zer )\n print 'model input.shape=', tmp_zer.shape, '\\toutput.shape=', tmp_zer_out.shape\n print 'model_type=', self.model_type\n\n print '-----'\n print '\\tinput_image.shape=', tmp_zer.shape\n print '\\toutput.shape=', tmp_zer_out.shape\n print '\\tminmax=', np.min( tmp_zer_out ), np.max( tmp_zer_out )\n print '\\tnorm=', np.linalg.norm( tmp_zer_out )\n print '\\tdtype=', tmp_zer_out.dtype\n print '-----'\n\n\n\n def handle_req( self, req ):\n \"\"\" The received image from CV bridge has to be [0,255]. In function makes it to\n intensity range [-0.5 to 0.5]\n \"\"\"\n ## Get Image out of req\n cv_image = CvBridge().imgmsg_to_cv2( req.ima )\n print '[HDF5ModelImageDescriptor Handle Request#%d] cv_image.shape' %(self.request_count), cv_image.shape, '\\ta=', req.a, '\\tt=', req.ima.header.stamp\n if len(cv_image.shape)==2:\n # print 'Input dimensions are NxM but I am expecting it to be NxMxC, so np.expand_dims'\n cv_image = np.expand_dims( cv_image, -1 )\n elif len( cv_image.shape )==3:\n pass\n else:\n assert( False )\n\n\n\n\n assert (cv_image.shape[0] == self.im_rows and\n cv_image.shape[1] == self.im_cols and\n cv_image.shape[2] == self.im_chnls),\\\n \"\\n[whole_image_descriptor_compute_server] Input shape of the image \\\n does not match with the allocated GPU memory. Expecting an input image of \\\n size %dx%dx%d, but received : %s\" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )\n\n # cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )\n # cv2.waitKey(10)\n # cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )\n\n ## Compute Descriptor\n start_time = time.time()\n # i__image = np.expand_dims( cv_image.astype('float32'), 0 )\n # i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)/255. [-0.5,0.5]\n i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)*2.0/255. #[-1,1]\n\n u = self.model.predict( i__image )\n\n print tcol.HEADER, 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ), tcol.ENDC\n print '\\tinput_image.shape=', cv_image.shape,\n print '\\tinput_image dtype=', cv_image.dtype\n print tcol.OKBLUE, '\\tinput image (to neuralnet) minmax=', np.min( i__image ), np.max( i__image ), tcol.ENDC\n print '\\tdesc.shape=', u.shape,\n print '\\tdesc minmax=', np.min( u ), np.max( u ),\n print '\\tnorm=', np.linalg.norm(u[0])\n print '\\tmodel_type=', self.model_type\n self.request_count += 1\n\n\n\n ## Populate output message\n result = WholeImageDescriptorComputeResponse()\n # result.desc = [ cv_image.shape[0], cv_image.shape[1] ]\n result.desc = u[0,:]\n result.model_type = self.model_type\n return result\n\n\n\nrospy.init_node( 'whole_image_descriptor_compute_server' )\n\n##\n## Load the config file and read image row, col\n##\nfs_image_width = -1\nfs_image_height = -1\nfs_image_chnls = 1\n\nif True: # read from param `config_file`\n if not rospy.has_param( '~config_file'):\n print 'FATAL...cannot find param ~config_file. This is needed to determine size of the input image to allocate GPU memory. If you do not specify the config_file, you need to atleast specify the nrows, ncols, nchnls'\n rospy.logerr( '[whole_image_descriptor_compute_server]FATAL...cannot find param ~config_file. This is needed to determine size of the input image to allocate GPU memory. If you do not specify the config_file, you need to atleast specify the nrows, ncols, nchnls' )\n\n if ( rospy.has_param( '~nrows') and rospy.has_param( '~ncols') ):\n print tcol.OKGREEN, 'However, you seem to have set the parameters nrows and ncols, so will read those.', tcol.ENDC\n else:\n quit()\n # quit only if you cannot see nrows and ncols\n\n else:\n config_file = rospy.get_param('~config_file')\n print '++++++++\\n++++++++ config_file: ', config_file\n if not os.path.isfile(config_file):\n print 'FATAL...cannot find config_file: ', config_file\n rospy.logerr( '[whole_image_descriptor_compute_server]FATAL...cannot find config_file: '+ config_file )\n quit()\n\n\n print '++++++++ READ opencv-yaml file: ', config_file\n fs = cv2.FileStorage(config_file, cv2.FILE_STORAGE_READ)\n fs_image_width = int( fs.getNode( \"image_width\" ).real() )\n fs_image_height = int( fs.getNode( \"image_height\" ).real() )\n print '++++++++ opencv-yaml:: image_width=', fs_image_width, ' image_height=', fs_image_height\n print '++++++++'\n\n\n##\n## Load nrows and ncols directly as config\n##\nif True: # read from param `nrows` and `ncols`\n if fs_image_width < 0 :\n if ( not rospy.has_param( '~nrows') or not rospy.has_param( '~ncols') or not rospy.has_param( '~nchnls') ):\n print 'FATAL...cannot find param either of ~nrows, ~ncols, ~nchnls. This is needed to determine size of the input image to allocate GPU memory'\n rospy.logerr( '[whole_image_descriptor_compute_server] FATAL...cannot find param either of ~nrows, ~ncols, nchnls. This is needed to determine size of the input image to allocate GPU memory' )\n quit()\n else:\n fs_image_height = rospy.get_param('~nrows')\n fs_image_width = rospy.get_param('~ncols')\n fs_image_chnls = rospy.get_param('~nchnls')\n\n print '~~~~~~~~~~~~~~~~'\n print '~nrows = ', fs_image_height, '\\t~ncols = ', fs_image_width, '\\t~nchnls = ', fs_image_chnls\n print '~~~~~~~~~~~~~~~~'\n\n\n##\n## Load Channels\n##\nif True:\n if not rospy.has_param( '~nchnls' ):\n rospy.logerr( \"[whole_image_descriptor_compute_server] FATAL....cannot file cmd param nchnls.\")\n quit()\n else:\n fs_image_chnls = rospy.get_param('~nchnls')\n\n##\n## Start Server\n##\n\n\n# Something wrong with jsonfile loading or with change_model_inputshape().\nif True:\n # kerasmodel_file = '/models.keras/Apr2019/gray_conv6_K16Ghost1__centeredinput/core_model.%d.keras' %(500)\n if rospy.has_param( '~kerasmodel_file'):\n kerasmodel_file = rospy.get_param('~kerasmodel_file')\n else:\n print tcol.ERROR, 'FATAL...missing specification of model file. You need to specify ~kerasmodel_file', tcol.ENDC\n quit()\n # gpu_netvlad = JSONModelImageDescriptor( kerasmodel_file=kerasmodel_file, im_rows=fs_image_height, im_cols=fs_image_width, im_chnls=fs_image_chnls )\n gpu_netvlad = HDF5ModelImageDescriptor( kerasmodel_file=kerasmodel_file, im_rows=fs_image_height, im_cols=fs_image_width, im_chnls=fs_image_chnls )\nelse:\n #gpu_s = SampleGPUComputer()\n gpu_netvlad = NetVLADImageDescriptor( im_rows=fs_image_height, im_cols=fs_image_width )\n # gpu_netvlad = ReljaNetVLAD( im_rows=fs_image_height, im_cols=fs_image_width )\n\n\ns = rospy.Service( 'whole_image_descriptor_compute', WholeImageDescriptorCompute, gpu_netvlad.handle_req )\nprint 'whole_image_descriptor_compute_server is running'\n\n\n# image = cv2.resize( cv2.imread( '/app/lena.jpg'), (224,224) )\n# print gpu_s.model.predict( np.expand_dims(image,0) )\n\n\n\nrospy.spin()\n" }, { "path": "src/Cerebro.cpp", "content": "#include \"Cerebro.h\"\n\n//-------------------------------------------------------------//\n//--- Setup Cerebro\n//-------------------------------------------------------------//\nCerebro::Cerebro( ros::NodeHandle& nh )\n{\n b_run_thread = false;\n b_descriptor_computer_thread = false;\n this->nh = nh;\n\n connected_to_descriptor_server = false;\n descriptor_size_available = false;\n}\n\nvoid Cerebro::setDataManager( DataManager* dataManager )\n{\n this->dataManager = dataManager;\n m_dataManager_available = true;\n}\n\nvoid Cerebro::setPublishers( const string base_topic_name )\n{\n string pub_loopedge_topic = base_topic_name+\"/loopedge\";\n ROS_INFO( \"[Cerebro::setPublishers] Publish %s\", pub_loopedge_topic.c_str() );\n pub_loopedge = nh.advertise( pub_loopedge_topic, 1000 );\n\n m_pub_available = true;\n}\n\n//-------------------------------------------------------------//\n//--- END Setup Cerebro\n//-------------------------------------------------------------//\n\n\n//------------------------------------------------------------------//\n// per Keyframe Descriptor Computation\n// Can have more threads to compute other aspects for keyframes\n// like texts, objects visible etc. TODO\n//------------------------------------------------------------------//\n\n#define __Cerebro__descriptor_computer_thread( msg ) ;\n// #define __Cerebro__descriptor_computer_thread( msg ) msg\n\n#define __Cerebro__descriptor_computer_thread__imp( msg ) ;\n// #define __Cerebro__descriptor_computer_thread__imp( msg ) msg;\nvoid Cerebro::descriptor_computer_thread()\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Cerebro before execution of the run() thread can begin. You can set the dataManager by call to Cerebro::setDataManager()\\n\");\n assert( b_descriptor_computer_thread && \"You need to call descriptor_computer_thread_enable() before spawning the thread\\n\" );\n\n //---------\n // Send a zeros image to the server just to know the descriptor size\n int nrows=-1, ncols=-1, desc_size=-1;\n int nChannels = 1; //< ***This needs to be set correctly depending on the model_type. If you need error messages from server about channels size, you need to change this. ****\n //----------\n\n // Service Call\n // Sample Code : https://github.com/mpkuse/cerebro/blob/master/src/unittest/unittest_rosservice_client.cpp\n connected_to_descriptor_server = false;\n descriptor_size_available = false;\n int n_sec_wait_for_connection = 71;\n cout << TermColor::iYELLOW() << \"[Cerebro::descriptor_computer_thread]Attempt connecting to ros-service for \" << n_sec_wait_for_connection << \" sec (will give up after that)\\n\" << TermColor::RESET();\n ros::ServiceClient client = nh.serviceClient( \"/whole_image_descriptor_compute\" );\n client.waitForExistence( ros::Duration(n_sec_wait_for_connection, 0) ); //wait maximum 10 sec\n if( !client.exists() ) {\n ROS_ERROR( \"[Cerebro::descriptor_computer_thread]Connection to server NOT successful. I tried waiting for %d sec, still couldnt establish connection. Quiting the thread.\", n_sec_wait_for_connection );\n return;\n }\n else std::cout << TermColor::GREEN() << \"Connection to ros-service ``\" << client.getService() << \"`` established\" << TermColor::RESET() << endl;\n connected_to_descriptor_server = true;\n\n\n ElapsedTime _est_desc_compute_time_ms;\n {\n auto _abs_cam = dataManager->getAbstractCameraRef();\n assert( _abs_cam && \"[Cerebro::descriptor_computer_thread] request from cerebro to access camera from dataManager is invalid. This means that camera was not yet set in dataManager\\n\");\n nrows = _abs_cam->imageHeight();\n ncols = _abs_cam->imageWidth();\n cv::Mat zero_image;\n if( nChannels == 3 )\n zero_image = cv::Mat::zeros( nrows, ncols, CV_8UC3 );\n else if( nChannels == 1 )\n zero_image = cv::Mat::zeros( nrows, ncols, CV_8UC1 );\n else {\n assert( false && \"Invalid number of channels specified in Cerebro::descriptor_computer_thread() \");\n cout << TermColor::RED() << \"[ERROR] Ax Invalid number of channels specified in Cerebro::descriptor_computer_thread() \" << endl << TermColor::RESET();\n exit(3);\n }\n\n // create zero image sensor_msgs::Image\n sensor_msgs::ImagePtr image_msg;\n if( nChannels == 3 )\n image_msg = cv_bridge::CvImage(std_msgs::Header(), \"bgr8\", zero_image).toImageMsg();\n else if( nChannels == 1 )\n image_msg = cv_bridge::CvImage(std_msgs::Header(), \"mono8\", zero_image).toImageMsg();\n else {\n assert( false && \"Invalid number of channels specified in Cerebro::descriptor_computer_thread() \");\n cout << TermColor::RED() << \"[ERROR] Bx Invalid number of channels specified in Cerebro::descriptor_computer_thread() \" << endl << TermColor::RESET();\n exit(3);\n }\n image_msg->header.stamp = ros::Time::now();\n cerebro::WholeImageDescriptorCompute srv; //service message\n srv.request.ima = *image_msg;\n srv.request.a = 986;\n // make request to server\n\n _est_desc_compute_time_ms.tic();\n if( client.call( srv ) ) {\n __Cerebro__descriptor_computer_thread(std::cout << \"Received response from server\\t\";)\n __Cerebro__descriptor_computer_thread(std::cout << \"desc.size=\" << srv.response.desc.size() << std::endl;)\n assert( srv.response.desc.size() > 0 && \"The received descriptor appear to be of zero length. This is a fatal error.\\n\" );\n desc_size = int( srv.response.desc.size() );\n }\n }\n assert( nrows > 0 && ncols > 0 && desc_size > 0 );\n int estimated_descriptor_compute_time_ms = _est_desc_compute_time_ms.toc_milli();\n cout << \"[Cerebro::descriptor_computer_thread] nrows=\" << nrows << \" ncols=\" << ncols << \" nChannels=\" << nChannels << \" ===> desc_size=\" << desc_size << \"\\t estimated_descriptor_compute_time_ms=\" << estimated_descriptor_compute_time_ms << endl;\n this->descriptor_size = int(desc_size);\n descriptor_size_available = true;\n\n\n\n ros::Rate rate(20);\n auto data_map = dataManager->getDataMapRef();\n auto img_data_mgr = dataManager->getImageManagerRef();\n\n #if 1\n //*****************\n // If loadStateFromDisk, ie. if data_map already has a lot of items means that the\n // state was preloaded. in this case just setup this thread appropriately.\n //**************************\n if( data_map->begin() == data_map->end() )\n {\n // This means the data_map is empty, which inturn means fresh run\n cout << TermColor::iBLUE() << \"[Cerebro::descriptor_computer_thread] Looks like data_map is empty which means this is a fresh run\" << TermColor::RESET() << endl;;\n }\n else {\n // This means, state was preloaded from file\n cout << TermColor::iGREEN() << \"[Cerebro::descriptor_computer_thread] Looks like data_map is NOT empty which means state was loaded from disk\" << TermColor::RESET() << endl;;\n\n // loop over data_map and make a note of all the timestamps where descriptor is available.\n int n_whlimgdescavai = 0;\n // img_data_mgr->print_status();\n for( auto itd=data_map->begin() ; itd!=data_map->end() ; itd++ )\n {\n if( itd->second->isWholeImageDescriptorAvailable() ) {\n n_whlimgdescavai++;\n wholeImageComputedList_pushback( itd->first );\n\n // Also make sure these images are rechable.\n bool kxxx = img_data_mgr->isImageRetrivable( \"left_image\", itd->first );\n if( kxxx == false ) // this is bad\n {\n cout << \"[Cerebro::descriptor_computer_thread] THIS IS BAD. REPORT IT TO AUTHORS WITH CODE jewbcjsmn\\nEXIT.....\\n\";\n exit(1);\n }\n }\n }\n cout << \"[Cerebro::descriptor_computer_thread]i find \"<< n_whlimgdescavai << \" datanodes where image descrptor is available and images are retrivable through the ImageDataManager\\n\";\n }\n #endif\n\n\n ros::Time last_proc_timestamp =ros::Time();\n int n_computed=0;\n ElapsedTime _time;\n int incoming_diff_ms = 0;\n while( b_descriptor_computer_thread )\n {\n if( data_map->begin() == data_map->end() ) {\n __Cerebro__descriptor_computer_thread( cout << \"nothing to compute descriptor\\n\" );\n std::this_thread::sleep_for( std::chrono::milliseconds( 1000 ) );\n continue;\n }\n ros::Time lb = data_map->rbegin()->first - ros::Duration(10, 0); // look at recent 10sec.\n auto S = data_map->lower_bound( lb );\n auto E = data_map->end();\n __Cerebro__descriptor_computer_thread(cout << \"[Cerebro::descriptor_computer_thread]] S=\" << S->first << \" E=\" << (data_map->rbegin())->first << endl;)\n for( auto it = S; it != E ; it++ ) {\n\n // cout << \"[Cerebro::descriptor_computer_thread]try : \" << it->first << \"\\t\";\n // cout << \"isWholeImageDescriptorAvailable=\" << it->second->isWholeImageDescriptorAvailable() << \"\\t\";\n // cout << \"isKeyFrame=\" << it->second->isKeyFrame() << \"\\t\";\n // cout << endl;\n\n //descriptor does not exisit at this stamp, so compute it.\n // Here I compute the whole image descriptor only at keyframes, you may try something like, if the pose is available compute it.\n if( it->second->isWholeImageDescriptorAvailable() == false && it->second->isKeyFrame() && it->first > last_proc_timestamp )\n {\n __Cerebro__descriptor_computer_thread(cout << TermColor::MAGENTA() << \"[Cerebro::descriptor_computer_thread]--- process t=\" << it->first << \"\\trel_t=\" << it->first - dataManager->getPose0Stamp() << TermColor::RESET() << \" n_computed=\" << n_computed << endl;)\n n_computed++;\n incoming_diff_ms = (it->first - last_proc_timestamp).toSec() * 1000. ;\n float skip_frac = 1.0 - incoming_diff_ms/float(estimated_descriptor_compute_time_ms) ;\n __Cerebro__descriptor_computer_thread( cout << \"incoming_diff_ms = \" << incoming_diff_ms << \"\\testimated_descriptor_compute_time_ms=\" << estimated_descriptor_compute_time_ms << \" so, skip_frac=\"<< skip_frac << endl; )\n last_proc_timestamp = it->first;\n\n // if( n_computed%2 == 0 ) // simple skip\n if( n_computed>4 && ( rand()/ float(RAND_MAX) ) < skip_frac ) // dynamic skip\n {\n __Cerebro__descriptor_computer_thread( cout << \"\\t\\t...SKIP...\\n\"; )\n continue;\n }\n\n\n if( it->second->getNumberOfSuccessfullyTrackedFeatures() < 20 )\n {\n __Cerebro__descriptor_computer_thread( cout << \"[Cerebro::descriptor_computer_thread] skip computing whole-image-descriptor for this image because it appears to be a kidnapped image frame.\\n\" ; )\n continue;\n }\n\n _time.tic();\n\n // use it->second->getImage() to compute descriptor. call the service\n cv::Mat image_curr;\n #if 0 // set this to 1 to get image from data_map (deprecated way), 0 to get image from image_manager.\n // old code in which images were stored in nodes\n image_curr = it->second->getImage();\n #else\n // using image data manager\n { //dont remove these braces, it is used for scoping and automatic deallocation\n cv::Mat tmp_img;\n bool getimstatus = img_data_mgr->getImage( \"left_image\", it->first, tmp_img );\n if( getimstatus == false ) {\n __Cerebro__descriptor_computer_thread( cout << \"since I cannot getImage, goto hurr_here\\n\"; );\n goto huur_here;\n }\n\n if( nChannels == 3 && tmp_img.channels() == 1 )\n cv::cvtColor( tmp_img, image_curr, CV_GRAY2BGR );\n else if( nChannels==1 && tmp_img.channels() == 3)\n cv::cvtColor( tmp_img, image_curr, CV_BGR2GRAY );\n else\n image_curr = tmp_img;\n }\n\n __Cerebro__descriptor_computer_thread(\n cout << \"image from mgr info: \" << MiscUtils::cvmat_info(image_curr) << endl;\n cout << \"nChannels=\" << nChannels << endl;\n )\n #endif\n\n sensor_msgs::ImagePtr image_msg;\n if( nChannels == 3 ) {\n image_msg = cv_bridge::CvImage(std_msgs::Header(), \"bgr8\", image_curr).toImageMsg();\n }\n else if( nChannels == 1 ) {\n // cout << \"cv_bridge mono8 encoding\\n\";\n image_msg = cv_bridge::CvImage(std_msgs::Header(), \"mono8\", image_curr).toImageMsg();\n }\n else {\n assert( false && \"Invalid number of channels specified in Cerebro::descriptor_computer_thread() \");\n cout << TermColor::RED() << \"[ERROR] Cx Invalid number of channels specified in Cerebro::descriptor_computer_thread() \" << endl << TermColor::RESET();\n exit(3);\n }\n image_msg->header.stamp = ros::Time( it->first );\n\n\n\n cerebro::WholeImageDescriptorCompute srv; //service message\n srv.request.ima = *image_msg;\n srv.request.a = 986;\n if( client.call( srv ) ) {\n // __Cerebro__descriptor_computer_thread(std::cout << \"Received response from server\\t\";)\n // __Cerebro__descriptor_computer_thread(std::cout << \"desc.size=\" << srv.response.desc.size() << std::endl;)\n assert( srv.response.desc.size() > 0 && \"The received descriptor appear to be of zero length. This is a fatal error.\\n\" );\n\n VectorXd vec( srv.response.desc.size() ); // allocated a vector\n for( int j=0 ; jsecond->setWholeImageDescriptor( vec );\n wholeImageComputedList_pushback( it->first );\n\n // __Cerebro__descriptor_computer_thread(cout << \"Computed descriptor at t=\" << it->first - dataManager->getPose0Stamp() << \"\\t\" << it->first << endl;)\n // __Cerebro__descriptor_computer_thread(std::cout << \"Computed descriptor in (millisec) = \" << _time.toc_milli() << endl;)\n // __Cerebro__descriptor_computer_thread__imp(cout << TermColor::CYAN() << \"Computed descriptor at t=\" << it->first - dataManager->getPose0Stamp() << \"\\t\" << it->first << TermColor::RESET() << endl);\n\n estimated_descriptor_compute_time_ms = _time.toc_milli();\n __Cerebro__descriptor_computer_thread__imp(\n cout << \"Computed descriptor at t=\" << it->first << \" of dim=\" << srv.response.desc.size() << \" in millisec=\" << estimated_descriptor_compute_time_ms << endl;\n )\n\n\n }\n else {\n ROS_ERROR( \"Failed to call ros service\" );\n }\n\n }\n huur_here:\n int ddddd=0; //this is here for the goto statement.\n }\n\n rate.sleep();\n }\n\n\n cout << \"[Cerebro::descriptor_computer_thread] Finished\\n\";\n\n}\n\n\n\n// these both functions are newly added. There are still some raw access\n// limited only to the function ``descrip_N__dot__descrip_0_N()``.\nconst int Cerebro::wholeImageComputedList_size() const {\n std::lock_guard lk(m_wholeImageComputedList);\n return wholeImageComputedList.size();\n}\n\nconst ros::Time Cerebro::wholeImageComputedList_at(int k) const\n{\n std::lock_guard lk(m_wholeImageComputedList);\n assert( k>=0 && k lk(m_wholeImageComputedList);\n this->wholeImageComputedList.push_back( __tx ); // note down where it was computed.\n\n}\n\n//------------------------------------------------------------------//\n// END per Keyframe Descriptor Computation\n//------------------------------------------------------------------//\n\n\n\n\n//------------------------------------------------------------------//\n//---------------- Populate Loop Candidates --------------------//\n//------------------------------------------------------------------//\n\n\n\n// #define __Cerebro__run__( msg ) msg ;\n#define __Cerebro__run__( msg ) ;\n\n// #define __Cerebro__run__debug( msg ) msg ;\n#define __Cerebro__run__debug( msg ) ;\n\n/// TODO: In the future more intelligent schemes can be experimented with. Besure to run those in new threads and disable this thread.\n/// wholeImageComputedList is a list for which descriptors are computed. Similarly other threads can compute\n/// scene-object labels, text etc etc in addition to currently computed whole-image-descriptor\nvoid Cerebro::run()\n{\n descrip_N__dot__descrip_0_N();\n // faiss__naive_loopcandidate_generator();\n // faiss_clique_loopcandidate_generator();\n // faiss_multihypothesis_tracking();\n\n}\n\n\n#ifdef HAVE_FAISS\n///-----------------------------------------------------------------\n/// Nearest neighbors search using facebook research's faiss library's IndexFlatIP\n/// Roughly follows : https://github.com/mpkuse/vins_mono_debug_pkg/blob/master/src_place_recog/faiss_try1.py\n/// This function is similar in functionality to `descrip_N__dot__descrip_0_N`\n/// but using faiss\nvoid Cerebro::faiss__naive_loopcandidate_generator()\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Cerebro before execution of the run() thread can begin. You can set the dataManager by call to Cerebro::setDataManager()\\n\");\n assert( b_run_thread && \"you need to call run_thread_enable() before run() can start executing\\n\" );\n\n //-----------------//\n //---- Settings ---//\n //-----------------//\n const int start_adding_descriptors_to_index_after = 150;\n const int LOCALITY_THRESH = 12;\n const float DOT_PROD_THRESH = 0.9;\n\n //------ END -----//\n\n // wait until connected_to_descriptor_server=true and descriptor_size_available=true\n if( wait_until__connectedToDescServer_and_descSizeAvailable( 71 ) == false ) {\n cout << TermColor::RED() << \"[Cerebro::faiss__naive_loopcandidate_generator ERROR] wait_until__connectedToDescServer_and_descSizeAvailable returned false implying a timeout.\\n\" << TermColor::RESET();\n return;\n }\n __Cerebro__run__( cout << TermColor::GREEN() <<\"[Cerebro::faiss__naive_loopcandidate_generator] descriptor_size=\" << this->descriptor_size << \" connected_to_descriptor_server && descriptor_size_available\" << TermColor::RESET() << endl; )\n assert( this->descriptor_size> 0 );\n\n\n cout << TermColor::GREEN() << \"init a faiss::IndexFlatIP(\"<< this->descriptor_size << \")\" << TermColor::RESET() << endl;\n faiss::IndexFlatIP index(this->descriptor_size);\n\n\n ros::Rate rate(10);\n int l=0, last_l=0;\n int l_last_added_to_index = 0;\n\n auto data_map = dataManager->getDataMapRef();\n while( b_run_thread )\n {\n l=wholeImageComputedList_size();\n\n if( l - last_l < 3 ) {\n __Cerebro__run__debug( cout << \"[Cerebro::faiss__naive_loopcandidate_generator]nothing new\\n\"; );\n rate.sleep();\n continue;\n }\n\n __Cerebro__run__( cout << TermColor::RED() << \"---\" << TermColor::RESET() << endl; )\n __Cerebro__run__( cout << \"l=\" << l << endl; )\n __Cerebro__run__debug( cout << \"[Cerebro::faiss__naive_loopcandidate_generator] data_map.size() = \" << data_map->size() << \"\\tper_image_descriptor_size=\" << this->descriptor_size << endl; )\n\n\n // add\n __Cerebro__run__(cout << TermColor::YELLOW();)\n if( l> start_adding_descriptors_to_index_after ) {\n for( int j=l_last_added_to_index ; jat( wholeImageComputedList_at( j ) )->getWholeImageDescriptor();\n VectorXf X_float = X.cast();\n float * X_raw = X_float.data();\n #if 0 //see if the type casting was correct.\n for( int g=0;g<5;g++ ) {\n // cout << \"(\" << X(g) << \",\" << X_float(g) << \") \";\n cout << \"(\" << X(g) << \",\" << X_raw[g] << \") \";\n }\n cout << endl;\n #endif\n index.add( 1, X_raw ); //-TODO\n }\n l_last_added_to_index = l-start_adding_descriptors_to_index_after;\n } else {\n __Cerebro__run__(cout << \"not seen enough. so,, add nothing. Will start adding after \"<< start_adding_descriptors_to_index_after<< \" descriptors\\n\";)\n }\n __Cerebro__run__(cout << TermColor::RESET();)\n\n // search\n vector tmp_;\n vector tmp_i;\n for( int l_i=last_l ; l_iat( wholeImageComputedList_at( l_i ) )->getWholeImageDescriptor();\n VectorXf X_float = X.cast();\n float * X_raw = X_float.data();\n float distances[5];\n faiss::Index::idx_t labels[5];\n ElapsedTime time_to_search = ElapsedTime();\n index.search( 1, X_raw, 5, distances, labels ); //TODO\n __Cerebro__run__(cout << \"search done in (ms): \" << time_to_search.toc_milli() << endl;)\n for( int g=0 ; g<5; g++ ) {\n __Cerebro__run__(\n // cout << g << \" labels=\" << labels[g] << \" distances=\" << distances[g] << endl;\n // cout << l_i << \"<--(\"<< distances[g] << \")-->\" << labels[g] << \"\\t\";\n printf( \"%4d<--(%4.2f)-->%d\\t\", l_i, distances[g], labels[g] );\n )\n }\n __Cerebro__run__(cout << endl;)\n __Cerebro__run__(cout << TermColor::RESET();)\n tmp_.push_back( distances[0] );\n tmp_i.push_back( labels[0] );\n }\n\n int _n = tmp_.size();\n if( _n ==3 && tmp_[_n-1] > DOT_PROD_THRESH && abs(tmp_i[0] - tmp_i[1]) < LOCALITY_THRESH && abs(tmp_i[0] - tmp_i[2]) < LOCALITY_THRESH )\n {\n __Cerebro__run__(\n cout << TermColor::RED() << \"loop found\" << l-1 << \"<--->\" << tmp_i[2] << TermColor::RESET();\n )\n\n {\n std::lock_guard lk_foundloops(m_foundLoops);\n foundLoops.push_back( std::make_tuple( wholeImageComputedList_at(l-1), wholeImageComputedList_at(tmp_i[2]), tmp_[2] ) );\n }\n }\n\n last_l = l;\n rate.sleep();\n }\n cout << \"[Cerebro::faiss__naive_loopcandidate_generator()] Finished\\n\";\n}\n\n\n// #define __faiss_clique_loopcandidate_generator__imp(msg) msg;\n#define __faiss_clique_loopcandidate_generator__imp(msg) ;\n\n// #define __faiss_clique_loopcandidate_generator__debug(msg) msg;\n#define __faiss_clique_loopcandidate_generator__debug(msg) ;\n\n// #define __faiss_clique_loopcandidate_generator__addtoindex(msg) msg;\n#define __faiss_clique_loopcandidate_generator__addtoindex(msg) ;\n\n#define __faiss_clique_loopcandidate_generator__search(msg) msg;\n// #define __faiss_clique_loopcandidate_generator__search(msg) ;\nvoid Cerebro::faiss_clique_loopcandidate_generator()\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Cerebro before execution of the run() thread can begin. You can set the dataManager by call to Cerebro::setDataManager()\\n\");\n assert( b_run_thread && \"you need to call run_thread_enable() before run() can start executing\\n\" );\n //-----------------//\n //---- Settings ---//\n //-----------------//\n const int start_adding_descriptors_to_index_after = 150;\n const int K_NEAREST_NEIGHBOURS=5;\n const double DOT_PROD_THRESH = 0.85; //0.85\n const int LOCALITY = 7;//7\n const int reset_accumulation_every_n_frames = 4; //4\n\n // wait until connected_to_descriptor_server=true and descriptor_size_available=true\n if( wait_until__connectedToDescServer_and_descSizeAvailable( 71 ) == false ) {\n cout << TermColor::RED() << \"[Cerebro::faiss__naive_loopcandidate_generator ERROR] wait_until__connectedToDescServer_and_descSizeAvailable returned false implying a timeout.\\n\" << TermColor::RESET();\n return;\n }\n __faiss_clique_loopcandidate_generator__imp( cout << TermColor::GREEN() <<\"[Cerebro::faiss__naive_loopcandidate_generator] descriptor_size=\" << this->descriptor_size << \" connected_to_descriptor_server && descriptor_size_available\" << TermColor::RESET() << endl; )\n assert( this->descriptor_size> 0 );\n\n // init faiss index\n __faiss_clique_loopcandidate_generator__imp(\n cout << TermColor::GREEN() << \"init a faiss::IndexFlatIP(\"<< this->descriptor_size << \")\" << TermColor::RESET() << endl;\n )\n faiss::IndexFlatIP index(this->descriptor_size);\n\n ros::Rate rate(10);\n auto data_map = dataManager->getDataMapRef();\n\n int l=0, last_l=0, l_last_added_to_index=0;\n std::map< int, std::map< int, float> > data_strc;\n\n\n float * distances = new float[K_NEAREST_NEIGHBOURS];\n faiss::Index::idx_t * labels = new faiss::Index::idx_t[K_NEAREST_NEIGHBOURS];\n map retained; //< Accumulation map. reset every say 3 indices. tune this as per amount of computation available\n\n while( b_run_thread ) {\n l = wholeImageComputedList_size();\n if( l<= last_l ) {\n __faiss_clique_loopcandidate_generator__debug( cout << \"[Cerebro::faiss_clique_loopcandidate_generator] nothing new\\n\";)\n rate.sleep();\n continue;\n }\n\n __faiss_clique_loopcandidate_generator__imp(\n cout << TermColor::RED() << \"--- \" << TermColor::RESET() << l << \"\\t\";\n cout << \"new [\" << last_l << \" to \" << l << \")\\n\";\n )\n\n\n //--------- add\n __faiss_clique_loopcandidate_generator__addtoindex(cout << TermColor::YELLOW();)\n if( l>start_adding_descriptors_to_index_after) {\n for( int j=l_last_added_to_index ; jat( wholeImageComputedList_at( j ) )->getWholeImageDescriptor();\n VectorXf X_float = X.cast();\n float * X_raw = X_float.data();\n #if 0 //see if the type casting was correct.\n for( int g=0;g<5;g++ ) {\n // cout << \"(\" << X(g) << \",\" << X_float(g) << \") \";\n cout << \"(\" << X(g) << \",\" << X_raw[g] << \") \";\n }\n cout << endl;\n #endif\n index.add( 1, X_raw );\n }\n l_last_added_to_index = l-start_adding_descriptors_to_index_after;\n } else {\n __faiss_clique_loopcandidate_generator__addtoindex(\n cout << \"not seen enough. so,, add nothing. Will start adding after \"<< start_adding_descriptors_to_index_after<< \" descriptors\\n\";\n )\n }\n __faiss_clique_loopcandidate_generator__addtoindex(cout << TermColor::RESET();)\n\n\n\n //----------- search\n __faiss_clique_loopcandidate_generator__search(cout << TermColor::MAGENTA();)\n for( int l_i = last_l ; l_i < l ; l_i++ )\n {\n __faiss_clique_loopcandidate_generator__search(\n cout << \"\\t\\tindex.search(\" << l_i << \")\\t\";\n cout << \"index.search(\" << wholeImageComputedList_at(l_i) << \")\\t\";\n cout << \"index.ntotal=\" << index.ntotal << \"\\n\";\n )\n if( index.ntotal < K_NEAREST_NEIGHBOURS )\n break;\n\n // l_i's descriptor\n VectorXd X = data_map->at( wholeImageComputedList_at( l_i ) )->getWholeImageDescriptor();\n VectorXf X_float = X.cast();\n float * X_raw = X_float.data();\n\n // ElapsedTime time_to_search = ElapsedTime();\n index.search( 1, X_raw, K_NEAREST_NEIGHBOURS, distances, labels );\n // cout << \"search done in (ms): \" << time_to_search.toc_milli() << endl;\n\n __faiss_clique_loopcandidate_generator__search(\n // Printing\n for( int g=0 ; g DOT_PROD_THRESH )\n cout << TermColor::GREEN();\n else cout << TermColor::MAGENTA();\n printf( \"%4d<--(%4.2f)-->%d\\t\", l_i, distances[g], labels[g] );\n cout << TermColor::RESET();\n }\n cout << endl;\n )\n\n\n // Go thru each nearest neighbours and add to list separated ones. vv same as 168, ignore\n // for example, if the neighbours: `802<--(0.92)-->606\t 802<--(0.89)-->168\t 802<--(0.89)-->172\t 802<--(0.89)-->169\t 802<--(0.88)-->607`\n // ^^ ^^ ^^same as 168, so ignore ^^ same as 606, ignore\n\n // cout << \"loop thru \" << K_NEAREST_NEIGHBOURS << \"\\n\";\n for( int g=0 ; g < K_NEAREST_NEIGHBOURS ; g++ )\n {\n if( distances[g] < DOT_PROD_THRESH ) //since distances are arranged in decending order, if you start seeing below my threshold its time to not process further.\n break;\n\n // cout << \"g=\"<first=\" << ity->first << endl;\n if( (ity->first - labels[g]) < LOCALITY )\n {\n // cout << \"\\tcond satisfied\\n\";\n duplicate = ity->first;\n break;\n }\n }\n if( duplicate != -1 ) {\n // cout << \"\\tthis was not uniq so increment the key=\" << duplicate << \" by 1\\n\";\n retained[ duplicate ]++;\n } else {\n // cout << \"\\tadd new key at \" << labels[g] << endl;\n retained[ labels[g] ] = 1;\n }\n }\n\n\n if( retained.size() > 0 && l_i%reset_accumulation_every_n_frames == 0)\n {\n __faiss_clique_loopcandidate_generator__search(\n cout << TermColor::iYELLOW() << \"l_i=\" << l_i << \" retained (ie. added to `foundLoops`) cout all: \";\n for( auto ity=retained.begin() ; ity!=retained.end() ; ity++ )\n {\n cout << ity->first << \":\" << ity->second << \", \";\n }\n cout << TermColor::RESET() << endl;\n )\n\n //%%%%\n //%%%% NOTE: If you put all the feasible candidates into the foundLoops it causes\n //%%%% The pose computation to queue-up, since the number of candidates are too many.\n //%%%% Usually addition of more than 2 candidates at a time spells trouble.\n //%%%% I use simple heuristics when there are more than 3 items in the retained.\n\n\n if( retained.size() == 1 )\n {\n __faiss_clique_loopcandidate_generator__search(\n cout << \"only 1 item in retained, pushback \" << wholeImageComputedList_at(l-1) << \"<--->\" <<\n wholeImageComputedList_at(retained.begin()->first) << endl;\n )\n std::lock_guard lk_foundloops(m_foundLoops);\n // __faiss_clique_loopcandidate_generator__search( cout << \"pushback: \" << wholeImageComputedList_at(l-1) << \"<--->\" << wholeImageComputedList_at(retained.begin()->first) << endl; )\n foundLoops.push_back( std::make_tuple( wholeImageComputedList_at(l-1),\n wholeImageComputedList_at(retained.begin()->first ),\n 0.9 ) );\n }\n\n if( retained.size() > 1 )\n {\n int percent = 100. / retained.size(); //will retain this many\n __faiss_clique_loopcandidate_generator__search( cout << \"Retain %=\" << percent << endl; )\n for( auto ity=retained.begin() ; ity!=retained.end() ; ity++ )\n {\n if( rand()%100 < percent ) {\n std::lock_guard lk_foundloops(m_foundLoops);\n __faiss_clique_loopcandidate_generator__search( cout << \"pushback: \" << wholeImageComputedList_at(l-1) << \"<--->\" << wholeImageComputedList_at(ity->first) << endl; )\n foundLoops.push_back( std::make_tuple( wholeImageComputedList_at(l-1),\n wholeImageComputedList_at(ity->first ),\n 0.9 ) );\n }\n }\n }\n\n\n retained.clear();\n }\n\n\n }\n\n\n last_l = l;\n rate.sleep();\n }\n\n\n delete [] distances;\n delete [] labels;\n cout << \"[Cerebro::faiss_clique_loopcandidate_generator] Finished Thread\\n\";\n\n\n}\n\n\n\n#define ___faiss_multihypothesis_tracking___add(msg) msg;\n// #define ___faiss_multihypothesis_tracking___add(msg) ;\n\n#define ___faiss_multihypothesis_tracking___search(msg) msg;\n// #define ___faiss_multihypothesis_tracking___search(msg) ;\nvoid Cerebro::faiss_multihypothesis_tracking()\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Cerebro before execution of the run() thread can begin. You can set the dataManager by call to Cerebro::setDataManager()\\n\");\n assert( b_run_thread && \"you need to call run_thread_enable() before run() can start executing\\n\" );\n //-----------------//\n //---- Settings ---//\n //-----------------//\n const int start_adding_descriptors_to_index_after = 150;\n const int K_NEAREST_NEIGHBOURS=5;\n\n //-----------------//\n //---- Wait ----//\n //-----------------//\n // wait until connected_to_descriptor_server=true and descriptor_size_available=true\n if( wait_until__connectedToDescServer_and_descSizeAvailable( 71 ) == false ) {\n cout << TermColor::RED() << \"[Cerebro::faiss__naive_loopcandidate_generator ERROR] wait_until__connectedToDescServer_and_descSizeAvailable returned false implying a timeout.\\n\" << TermColor::RESET();\n return;\n }\n cout << TermColor::GREEN() <<\"[Cerebro::faiss_multihypothesis_tracking] descriptor_size=\" << this->descriptor_size << \" connected_to_descriptor_server && descriptor_size_available\" << TermColor::RESET() << endl;\n assert( this->descriptor_size> 0 );\n\n //---------------------//\n //--- init faiss index //\n //---------------------//\n cout << TermColor::GREEN() << \"[[Cerebro::faiss_multihypothesis_tracking]] init a faiss::IndexFlatIP(\"<< this->descriptor_size << \")\" << TermColor::RESET() << endl;\n faiss::IndexFlatIP index(this->descriptor_size);\n\n\n // init (misc)\n ros::Rate rate(10);\n auto data_map = dataManager->getDataMapRef();\n HypothesisManager * hyp_manager = new HypothesisManager(); // there is a delete at the end of this function\n\n // Start monitoring thread\n hyp_manager->monitoring_thread_enable();\n // hyp_manager->monitoring_thread_disable();\n std::thread hyp_monitoring_th( &HypothesisManager::monitoring_thread, hyp_manager );\n\n\n\n int l=0, last_l=0, l_last_added_to_index=0;\n\n\n float * distances = new float[K_NEAREST_NEIGHBOURS];\n faiss::Index::idx_t * labels = new faiss::Index::idx_t[K_NEAREST_NEIGHBOURS];\n\n //---------------------//\n // While\n // index.add( i-150 )\n // index.search( i )\n //---------------------//\n while( b_run_thread ) {\n l = wholeImageComputedList_size();\n if( l<= last_l ) {\n cout << \"[Cerebro::faiss_multihypothesis_tracking] nothing new\\n\";\n rate.sleep();\n continue;\n }\n\n // looks like new descriptors are available.\n cout << TermColor::RED() << \"--- \" << TermColor::RESET() << l << \"\\t\";\n cout << \"new [\" << last_l << \" to \" << l << \")\\n\";\n\n\n\n //--------- add\n ___faiss_multihypothesis_tracking___add(cout << TermColor::YELLOW();)\n if( l>start_adding_descriptors_to_index_after) {\n for( int j=l_last_added_to_index ; jat( wholeImageComputedList_at( j ) )->getWholeImageDescriptor();\n VectorXf X_float = X.cast();\n float * X_raw = X_float.data();\n #if 0 //see if the type casting was correct.\n for( int g=0;g<5;g++ ) {\n // cout << \"(\" << X(g) << \",\" << X_float(g) << \") \";\n cout << \"(\" << X(g) << \",\" << X_raw[g] << \") \";\n }\n cout << endl;\n #endif\n index.add( 1, X_raw );\n }\n l_last_added_to_index = l-start_adding_descriptors_to_index_after;\n } else {\n ___faiss_multihypothesis_tracking___add(\n cout << \"not seen enough. so,, add nothing to the index. Will start adding after \"<< start_adding_descriptors_to_index_after<< \" descriptors\\n\";\n )\n\n }\n ___faiss_multihypothesis_tracking___add(cout << TermColor::RESET();)\n\n\n\n //----------- search\n cout << TermColor::MAGENTA();\n for( int l_i = last_l ; l_i < l ; l_i++ ) {\n cout << \"\\t\\tindex.search(\" << l_i << \")\\t\";\n cout << \"index.search(\" << wholeImageComputedList_at(l_i) << \")\\t\";\n cout << \"index.ntotal=\" << index.ntotal << \"\\n\";\n if( index.ntotal < K_NEAREST_NEIGHBOURS )\n break;\n\n // l_i's descriptor\n VectorXd X = data_map->at( wholeImageComputedList_at( l_i ) )->getWholeImageDescriptor();\n VectorXf X_float = X.cast();\n float * X_raw = X_float.data();\n\n // ElapsedTime time_to_search = ElapsedTime();\n index.search( 1, X_raw, K_NEAREST_NEIGHBOURS, distances, labels );\n // cout << \"search done in (ms): \" << time_to_search.toc_milli() << endl;\n\n // Loop over all the nearest neighbours\n cout << \"\\t\\t\";\n for( int g=0 ; g 0.85 )\n cout << TermColor::GREEN();\n else cout << TermColor::MAGENTA();\n printf( \"g=%d: %4d<--(%4.2f)-->%d\\t\", g, l_i, distances[g], labels[g] );\n cout << TermColor::RESET();\n\n\n\n if( distances[g] > 0.85 ) {\n // Send (l_i, labels[g], distances[g] ) to HypothesisManager\n hyp_manager->add_node( l_i, labels[g], distances[g] );\n }\n }\n cout << endl ;\n\n // Loop through each hypothesis and decrement the time-to-live\n hyp_manager->digest();\n }\n\n\n last_l = l;\n rate.sleep();\n }\n\n\n hyp_manager->monitoring_thread_disable();\n hyp_monitoring_th.join();\n\n\n delete [] distances;\n delete [] labels;\n delete hyp_manager;\n cout << \"[Cerebro::faiss_multihypothesis_tracking] Finished Thread\\n\";\n\n\n}\n\n\n\n\n#endif //HAVE_FAISS\n\n///-----------------------------------------------------------------\n\n///-----------------------------------------------------------------\n/// This implements a simple loopclosure detection scheme based on dot-product of descriptor-vectors.\n///\n\n// 1 will plot the result of dot product as image. 0 will not plot to image\n// #define __Cerebro__descrip_N__dot__descrip_0_N__implotting 0\n#define __Cerebro__descrip_N__dot__descrip_0_N__implotting 1\n\n\nvoid Cerebro::descrip_N__dot__descrip_0_N()\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Cerebro before execution of the run() thread can begin. You can set the dataManager by call to Cerebro::setDataManager()\\n\");\n assert( b_run_thread && \"you need to call run_thread_enable() before run() can start executing\\n\" );\n\n cout << TermColor::GREEN() << \"[Cerebro::descrip_N__dot__descrip_0_N] Start thread\" << TermColor::RESET() << endl;\n //---\n //--- Main settings for this function\n //---\n int LOCALITY_THRESH = 12;\n float DOT_PROD_THRESH = 0.85;\n const int start_adding_descriptors_to_index_after = 50;\n\n ros::Rate rate(10);\n\n #if 0\n // wait until connected_to_descriptor_server=true and descriptor_size_available=true\n int wait_itr = 0;\n while( true ) {\n if( this->connected_to_descriptor_server && this->descriptor_size_available)\n break;\n __Cerebro__run__(cout << wait_itr << \" [Cerebro::descrip_N__dot__descrip_0_N]waiting for `descriptor_size_available` to be true\\n\";)\n rate.sleep();\n wait_itr++;\n if( wait_itr > 157 ) {\n __Cerebro__run__( cout << TermColor::RED() << \"[Cerebro::descrip_N__dot__descrip_0_N] `this->connected_to_descriptor_server && this->descriptor_size_available` has not become true dispite waiting for about 15sec. So quiting the run thread.\\n\" << TermColor::RESET(); )\n return;\n }\n }\n #endif\n\n if( wait_until__connectedToDescServer_and_descSizeAvailable( 71 ) == false ) {\n cout << TermColor::RED() << \"[ Cerebro::descrip_N__dot__descrip_0_N ERROR] wait_until__connectedToDescServer_and_descSizeAvailable returned false implying a timeout.\\n\" << TermColor::RESET();\n return;\n }\n\n __Cerebro__run__( cout << TermColor::GREEN() <<\"[Cerebro::descrip_N__dot__descrip_0_N] descriptor_size=\" << this->descriptor_size << \" connected_to_descriptor_server && descriptor_size_available\" << TermColor::RESET() << endl; )\n assert( this->descriptor_size> 0 );\n\n\n\n int l=0, last_l=0;\n int last_processed=0;\n MatrixXd M = MatrixXd::Zero( this->descriptor_size, 29000 ); // TODO: Need dynamic allocation here.\n cout << \"[Cerebro::descrip_N__dot__descrip_0_N] M.rows = \" << M.rows() << \" M.cols=\" << M.cols() << endl;\n cout << \"[Cerebro::descrip_N__dot__descrip_0_N] TODO: Need dynamic allocation here.\\n\";\n\n #if __Cerebro__descrip_N__dot__descrip_0_N__implotting > 0\n // Plotting image\n Plot2Mat handle(320,240, cv::Scalar(80,80,80) );\n // Plot2Mat handle;\n handle.setYminmax( -0.1, 1.1);\n #endif\n while( b_run_thread )\n {\n\n auto data_map = dataManager->getDataMapRef(); // this needs to be get every iteration else i dont get the ubdated values which are constantly being updated by other threads.\n l = wholeImageComputedList_size();\n\n if( l - last_l < 3 ) {\n __Cerebro__run__debug( cout << \"[Cerebro::descrip_N__dot__descrip_0_N]nothing new\\n\"; );\n rate.sleep();\n continue;\n }\n\n __Cerebro__run__( cout << TermColor::RED() << \"---\" << TermColor::RESET() << endl; )\n __Cerebro__run__( cout << \"l=\" << l << endl; )\n __Cerebro__run__debug( cout << \"[Cerebro::descrip_N__dot__descrip_0_N] data_map.size() = \" << data_map->size() << \"\\tper_image_descriptor_size=\"<< this->descriptor_size << endl; )\n\n\n ///------------ Extract v, v-1, v-2. The latest 3 descriptors.\n VectorXd v, vm, vmm;\n ros::Time i_v, i_vm, i_vmm;\n {\n // std::lock_guard lk(m_wholeImageComputedList);\n assert( data_map.count( wholeImageComputedList_at(l-1) ) > 0 &&\n data_map.count( wholeImageComputedList_at(l-2) ) > 0 &&\n data_map.count( wholeImageComputedList_at(l-3) ) > 0 &&\n \"either of l, l-1, l-2 is not available in the datamap\"\n );\n\n\n\n\n v = data_map->at( wholeImageComputedList_at(l-1) )->getWholeImageDescriptor();\n vm = data_map->at( wholeImageComputedList_at(l-2) )->getWholeImageDescriptor();\n vmm = data_map->at( wholeImageComputedList_at(l-3) )->getWholeImageDescriptor();\n i_v = wholeImageComputedList_at(l-1);\n i_vm = wholeImageComputedList_at(l-2);\n i_vmm = wholeImageComputedList_at(l-3);\n }\n\n\n\n // This is very inefficient. Better to have a matrix-vector product and not getWholeImageDescriptor() all the time.\n assert( M.rows() == v.size() );\n assert( l < M.cols() );\n\n\n ////-------------- Fill descriptors [last_l, l) into M.\n {\n // std::lock_guard lk(m_wholeImageComputedList); //no need for this as we switched to _at functions which are threadsafe\n for( int _s=last_l ; _sat( wholeImageComputedList_at(_s) )->getWholeImageDescriptor();\n\n __Cerebro__run__debug(\n cout << \"M.col(\" << _s << \") = data_map[ \" << wholeImageComputedList_at(_s) << \" ]. \\t\";\n cout << \" isWholeImageDescriptorAvailable = \" << data_map->at( wholeImageComputedList_at(_s) )->isWholeImageDescriptorAvailable() << endl;\n )\n }\n }\n\n\n //////////////////////////////////////\n ////----------- DOT PRODUCT----------\n /////////////////////////////////////\n int k = l - start_adding_descriptors_to_index_after; // given a stamp, l, get another stamp k. better make this to 200.\n\n //usable size of M is 8192xl, let k (k 5 ) {\n ElapsedTime timer;\n timer.tic();\n // dot\n VectorXd u = v.transpose() * M.leftCols( k );\n VectorXd um = vm.transpose() * M.leftCols( k );\n VectorXd umm = vmm.transpose() * M.leftCols( k );\n __Cerebro__run__( cout << \" \";)\n __Cerebro__run__( cout << \" \";)\n __Cerebro__run__( cout << \" \";)\n __Cerebro__run__( cout << \"Done in (ms): \" << timer.toc_milli() << endl; )\n\n // max coefficient and the index of maxcoeff.\n double u_max = u.maxCoeff();\n double um_max = um.maxCoeff();\n double umm_max = umm.maxCoeff();\n int u_argmax=-1, um_argmax=-1, umm_argmax=-1;\n for( int ii=0 ; ii 0 && um_argmax > 0 && umm_argmax > 0 );\n\n\n #if __Cerebro__descrip_N__dot__descrip_0_N__implotting > 0\n // plot\n handle.resetCanvas();\n handle.plot( u );\n\n #endif\n\n\n // Criteria for a recognized place\n if( abs(u_argmax - um_argmax) < LOCALITY_THRESH && abs(u_argmax-umm_argmax) < LOCALITY_THRESH && u_max > DOT_PROD_THRESH )\n {\n // std::lock_guard lk(m_wholeImageComputedList); //no need of lock here as we switched to _at functions for whlist\n\n\n __Cerebro__run__(\n cout << TermColor::RED() << \"Loop FOUND!! a_idx=\" << l-1 << \"<-----> (\" << u_argmax << \",\" << um_argmax << \",\" << umm_argmax << \")\"<< TermColor::RESET() << endl;\n cout << TermColor::RED() << \"loop FOUND!! \"\n << \"t_l=\" << wholeImageComputedList_at(l-1)\n << \" \"\n << \"t_argmax=\" << wholeImageComputedList_at(u_argmax)\n << TermColor::RESET() << endl;\n )\n\n #if __Cerebro__descrip_N__dot__descrip_0_N__implotting > 0\n handle.mark( u_argmax );\n #endif\n\n //TODO :\n // publish the image pair based on a config_file_flag\n // read flags for publish image, threshold(0.92), locality threshold (8) from file.\n\n {\n std::lock_guard lk_foundloops(m_foundLoops);\n foundLoops.push_back( std::make_tuple( wholeImageComputedList_at(l-1), wholeImageComputedList_at(u_argmax), u_max ) );\n }\n }\n\n\n #if __Cerebro__descrip_N__dot__descrip_0_N__implotting > 0\n cv::imshow(\"canvas\", handle.getCanvasConstPtr() );\n cv::waitKey(20);\n #endif\n\n }\n else {\n __Cerebro__run__(\n cout << \"[Cerebro::descrip_N__dot__descrip_0_N] do nothing. not seen enough yet.\\n\";\n )\n }\n\n\n last_l = l;\n rate.sleep();\n }\n\n cout << TermColor::RED() << \"[Cerebro::descrip_N__dot__descrip_0_N] Finished thread\" << TermColor::RESET() << endl;\n}\n\n\n\n//------------------------------------------------------------------//\n//---------------- END Populate Loop Candidates --------------------//\n//------------------------------------------------------------------//\n\n\n\nconst int Cerebro::foundLoops_count() const\n{\n std::lock_guard lk(m_foundLoops);\n return foundLoops.size();\n}\n\nconst std::tuple Cerebro::foundLoops_i( int i) const\n{\n std::lock_guard lk(m_foundLoops);\n assert( i >= 0 && i lk(m_foundLoops);\n\n json jsonout_obj;\n auto data_map = dataManager->getDataMapRef();\n\n int n_foundloops = foundLoops.size();\n for( int i=0 ; i(u);\n ros::Time t_prev = std::get<1>(u);\n double score = std::get<2>(u);\n\n\n assert( data_map.count( t_curr ) > 0 && data_map.count( t_prev ) > 0 && \"One or both of the timestamps in foundloops where not in the data_map. This cannot be happening...fatal...\\n\" );\n int idx_1 = std::distance( data_map->begin(), data_map->find( t_curr ) );\n int idx_2 = std::distance( data_map->begin(), data_map->find( t_prev ) );\n // cout << \"loop#\" << i << \" of \" << n_foundloops << \": \";\n // cout << t_curr << \"(\" << score << \")\" << t_prev << endl;\n // cout << idx_1 << \"<--->\" << idx_2 << endl;\n\n json _cur_json_obj;\n _cur_json_obj[\"time_sec_a\"] = t_curr.sec;\n _cur_json_obj[\"time_nsec_a\"] = t_curr.nsec;\n _cur_json_obj[\"time_sec_b\"] = t_prev.sec;\n _cur_json_obj[\"time_nsec_b\"] = t_prev.nsec;\n _cur_json_obj[\"time_double_a\"] = t_curr.toSec();\n _cur_json_obj[\"time_double_b\"] = t_prev.toSec();\n _cur_json_obj[\"global_a\"] = idx_1;\n _cur_json_obj[\"global_b\"] = idx_2;\n _cur_json_obj[\"score\"] = score;\n jsonout_obj.push_back( _cur_json_obj );\n }\n\n return jsonout_obj;\n\n}\n\n//--------------------------------------------------------------//\n//------------------ Geometry Thread ---------------------------//\n//--------------------------------------------------------------//\n\n// #define __Cerebro__loopcandi_consumer__(msg) msg;\n#define __Cerebro__loopcandi_consumer__(msg) ;\n// ^This will also imshow image-pairs with gms-matches marked.\n\n// #define __Cerebro__loopcandi_consumer__IMP( msg ) msg;\n#define __Cerebro__loopcandi_consumer__IMP( msg ) ;\n// ^Important Text only printing\n\n\n#define __Cerebro__loopcandi_consumer__IMSHOW 0 // will not produce the images (ofcourse will not show as well)\n// #define __Cerebro__loopcandi_consumer__IMSHOW 1 // produce the images and log them, will not imshow\n// #define __Cerebro__loopcandi_consumer__IMSHOW 2 // produce the images and imshow them, don't log\n\n// Just uncomment it to disable consistency check.\n// #define __Cerebro__loopcandi_consumer__no_pose_consistency_check\nvoid Cerebro::loopcandiate_consumer_thread()\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Cerebro before execution of the run() thread can begin. You can set the dataManager by call to Cerebro::setDataManager()\\n\");\n assert( b_loopcandidate_consumer && \"you need to call loopcandidate_consumer_enable() before loopcandiate_consumer_thread() can start executing\\n\" );\n cout << TermColor::GREEN() << \"[Cerebro::loopcandiate_consumer_thread] Start thread\" << TermColor::RESET() << endl;\n\n // init StereoGeometry\n bool stereogeom_status = init_stereogeom();\n if( !stereogeom_status ) {\n assert( false && \"[Cerebro::loopcandiate_consumer_thread()] cannot init_stereogeom\\n\");\n return;\n }\n // stereogeom->set_K( 375.0, 375.0, 376.0, 240.0 ); // set this to something sensible, best is to use left camera's K\n\n\n // init pt-feature-matcher\n\n\n ros::Rate rate(1);\n int prev_count = 0;\n int new_count = 0;\n ElapsedTime timer;\n\n while( b_loopcandidate_consumer )\n {\n new_count = foundLoops_count();\n\n if( new_count == prev_count ) {\n rate.sleep();\n continue;\n }\n\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::iGREEN() ;\n cout << \"I see \"<< new_count - prev_count << \" new candidates. from i=[\"<< prev_count << \",\" << new_count-1 << \"]\\n\";\n // cout << \"Cerebro::loopcandiate_consumer_thread(); #loops=\" << new_count << TermColor::RESET() << endl;\n cout << TermColor::RESET() ;\n )\n\n for( int j= prev_count ; j<=new_count-1 ; j++ )\n {\n ProcessedLoopCandidate proc_candi;\n\n timer.tic() ;\n #ifdef __Cerebro__loopcandi_consumer__no_pose_consistency_check\n bool proc___status = process_loop_candidate_imagepair( j, proc_candi );\n #else\n bool proc___status = process_loop_candidate_imagepair_consistent_pose_compute( j, proc_candi );\n #endif\n __Cerebro__loopcandi_consumer__IMP(\n cout << \"\\t\" << timer.toc_milli() << \"(ms) !! process_loop_candidate_imagepair()\\n\";\n )\n\n // the data is in the `proc_candi` which is put into a vector which is a class variable.\n if( proc___status )\n {\n std::lock_guard lk(m_processedLoops);\n\n // publish proc_candi\n cerebro::LoopEdge loopedge_msg;\n #ifdef __Cerebro__loopcandi_consumer__no_pose_consistency_check\n bool __makemsg_status = proc_candi.makeLoopEdgeMsg( loopedge_msg );\n #else\n bool __makemsg_status = proc_candi.makeLoopEdgeMsgWithConsistencyCheck( loopedge_msg );\n #endif\n\n\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::iBLUE() << \"__makemsg_status: \" << __makemsg_status << \" publish loopedge_msg\" << TermColor::RESET() << endl;\n )\n\n // publish only if msg was successfully made ==> all the candidate relative poses are consistent.\n if( __makemsg_status )\n {\n __Cerebro__loopcandi_consumer__IMP( cout << TermColor::GREEN() << \"publish loopedge_msg\" << TermColor::RESET() << endl; )\n pub_loopedge.publish( loopedge_msg );\n } else {\n __Cerebro__loopcandi_consumer__IMP( cout << TermColor::RED() << \"not publishing because __makemsg_status was false\" << TermColor::RESET() << endl; )\n }\n\n // irrespective of where the poses are consistent or not add it to the list.\n processedloopcandi_list.push_back( proc_candi );\n __Cerebro__loopcandi_consumer__IMP(cout << \"\\tAdded to `processedloopcandi_list`\" << endl;)\n }\n else {\n __Cerebro__loopcandi_consumer__IMP( cout << \"\\tNot added to `processedloopcandi_list`, status was false\\n\" << endl; )\n }\n }\n\n prev_count = new_count;\n rate.sleep();\n\n }\n\n cout << TermColor::RED() << \"[Cerebro::loopcandiate_consumer_thread] disable called, quitting loopcandiate_consumer_thread\" << TermColor::RESET() << endl;\n\n}\n\n\nconst int Cerebro::processedLoops_count() const\n{\n std::lock_guard lk(m_processedLoops);\n return processedloopcandi_list.size();\n\n}\n\nconst ProcessedLoopCandidate& Cerebro::processedLoops_i( int i ) const\n{\n std::lock_guard lk(m_processedLoops);\n\n assert( i>=0 && i< processedloopcandi_list.size() && \"[processedLoops_i] input i is not in the correct range\" );\n if( i>=0 && i< processedloopcandi_list.size() ) {\n return processedloopcandi_list[ i ];\n }\n else {\n cout << TermColor::RED() << \"[Cerebro::processedLoops_i] error you requested for processedloopcandidate idx=\" << i << \" but the length of the vector was \" << processedloopcandi_list.size() << endl;\n exit(1);\n }\n}\n\n\n\nbool Cerebro::init_stereogeom()\n{\n auto left_camera = dataManager->getAbstractCameraRef(0);\n if( !dataManager->isAbstractCameraSet(1) )\n {\n ROS_ERROR( \"Stereo-camera right doesn't appear to be set. to use this thread, you need to have StereoGeometry. Edit the config file to set the 2nd camera and its baseline\\n\");\n return false;\n }\n auto right_camera = dataManager->getAbstractCameraRef(1);\n __Cerebro__loopcandi_consumer__(\n cout << TermColor::iGREEN() ;\n cout << \"[Cerebro::loopcandiate_consumer_thread] left_camera\\n\" << left_camera->parametersToString() << endl;\n cout << \"[Cerebro::loopcandiate_consumer_thread] right_camera\\n\" << right_camera->parametersToString() << endl;\n cout << TermColor::RESET();\n )\n\n Matrix4d right_T_left;\n if( !dataManager->isCameraRelPoseSet( std::make_pair(1,0)) )\n {\n ROS_ERROR( \"stereo camera appears to be set but the baseline (stereo-extrinsic) is not specified in the config_file. In config file you need to set `extrinsic_1_T_0`\");\n return false;\n }\n right_T_left = dataManager->getCameraRelPose( std::make_pair(1,0) );\n __Cerebro__loopcandi_consumer__(\n cout << TermColor::iGREEN() ;\n cout << \"[Cerebro::loopcandiate_consumer_thread] right_T_left: \" << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;\n cout << TermColor::RESET();\n )\n stereogeom = std::make_shared( left_camera,right_camera, right_T_left );\n return true;\n}\n\n\nbool Cerebro::retrive_stereo_pair( DataNode* node, cv::Mat& left_image, cv::Mat& right_image, bool bgr2gray )\n{\n #if 0\n // raw stereo-images in gray\n if( !(node->isImageAvailable()) || !(node->isImageAvailable(1)) ) {\n cout << TermColor::RED() << \"[Cerebro::retrive_stereo_pair] Either of the node images (stereo-pair was not available). This is probably not fatal, this loopcandidate will be skipped.\" << TermColor::RESET() << endl;\n return false;\n }\n\n const cv::Mat& bgr_left_image = node->getImage();\n const cv::Mat& bgr_right_image = node->getImage(1);\n #else\n auto img_data_mgr = dataManager->getImageManagerRef();\n\n if( img_data_mgr->isImageRetrivable( \"left_image\", node->getT() )==false )\n {\n cout << TermColor::RED() << \"[Cerebro::retrive_stereo_pair] img_data_mgr->isImageRetrivable( \\\"left_image\\\",\" << node->getT() << \") returned false\" << TermColor::RESET() << endl;\n cout << TermColor::RED() << \"[Cerebro::retrive_stereo_pair] Either of the node images (stereo-pair was not available). This is probably not fatal, this loopcandidate will be skipped.\" << TermColor::RESET() << endl;\n return false;\n }\n\n if ( img_data_mgr->isImageRetrivable( \"right_image\", node->getT() ) == false )\n {\n cout << TermColor::RED() << \"[Cerebro::retrive_stereo_pair] img_data_mgr->isImageRetrivable( \\\"right_image\\\",\" << node->getT() << \") returned false\" << TermColor::RESET() << endl;\n cout << TermColor::RED() << \"[Cerebro::retrive_stereo_pair] Either of the node images (stereo-pair was not available). This is probably not fatal, this loopcandidate will be skipped.\" << TermColor::RESET() << endl;\n // img_data_mgr->print_status();\n // cout << TermColor::RED() << \"[Cerebro::retrive_stereo_pair] img_data_mgr->isImageRetrivable( \\\"right_image\\\",\" << node->getT() << \") returned false\" << TermColor::RESET() << endl;\n return false;\n }\n\n\n cv::Mat bgr_left_image, bgr_right_image;\n img_data_mgr->getImage( \"left_image\", node->getT(), bgr_left_image );\n img_data_mgr->getImage( \"right_image\", node->getT(), bgr_right_image );\n\n #if 0\n cout << \"bgr_left_image: \" << MiscUtils::cvmat_info( bgr_left_image ) << \"\\n\";\n cout << \"bgr_right_image: \" << MiscUtils::cvmat_info( bgr_right_image ) << \"\\n\";\n #endif\n if( !(bgr_left_image.data) || !(bgr_right_image.data) )\n {\n cout << \"[Cerebro::retrive_stereo_pair] Invalid images bfhjreturn false.\\n\";\n return false;\n }\n #endif\n\n if( bgr2gray ) {\n\n if( bgr_left_image.channels() > 1 )\n cv::cvtColor( bgr_left_image, left_image, CV_BGR2GRAY );\n else\n left_image = bgr_left_image;\n\n if( bgr_right_image.channels() > 1 )\n cv::cvtColor( bgr_right_image, right_image, CV_BGR2GRAY );\n else\n right_image = bgr_right_image;\n }\n else {\n left_image = bgr_left_image;\n right_image = bgr_right_image;\n }\n\n #if 0\n cout << \"[Cerebro::retrive_stereo_pair]left_image: \" << MiscUtils::cvmat_info( left_image ) << \"\\n\";\n cout << \"[Cerebro::retrive_stereo_pair]right_image: \" << MiscUtils::cvmat_info( right_image ) << \"\\n\";\n cout << endl;\n #endif\n return true;\n\n}\n\n\n\nbool Cerebro::process_loop_candidate_imagepair_consistent_pose_compute( int ii, ProcessedLoopCandidate& proc_candi )\n{\n auto u = foundLoops_i( ii );\n ros::Time t_curr = std::get<0>(u);\n ros::Time t_prev = std::get<1>(u);\n double score = std::get<2>(u);\n\n auto data_map = dataManager->getDataMapRef();\n assert( data_map->count( t_curr ) > 0 && data_map->count( t_prev ) > 0 && \"One or both of the timestamps in foundloops where not in the data_map. This cannot be happening...fatal...\\n\" );\n int idx_1 = std::distance( data_map->begin(), data_map->find( t_curr ) );\n int idx_2 = std::distance( data_map->begin(), data_map->find( t_prev ) );\n\n DataNode * node_1 = data_map->find( t_curr )->second;\n DataNode * node_2 = data_map->find( t_prev )->second;\n\n\n\n\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::BLUE() << \"{\"<\" << idx_2 << TermColor::RESET() << \"\\tt_curr=\" << t_curr << \" <--> t_prev=\" << t_prev << endl;\n )\n\n // if( node_1->getNumberOfSuccessfullyTrackedFeatures() < 20 || node_2->getNumberOfSuccessfullyTrackedFeatures() < 20 ) {\n // __Cerebro__loopcandi_consumer__IMP( \"[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute] skip processing this pair because it is a kidnaped node\\n\" );\n // return false;\n // }\n\n cv::Mat a_imleft_raw, a_imright_raw;\n bool ret_status_a = retrive_stereo_pair( node_1, a_imleft_raw, a_imright_raw );\n if( !ret_status_a )\n return false;\n\n cv::Mat b_imleft_raw, b_imright_raw;\n bool ret_status_b = retrive_stereo_pair( node_2, b_imleft_raw, b_imright_raw );\n if( !ret_status_b )\n return false;\n\n //------------------------------------------------\n //------ 3d points from frame_a\n //------------------------------------------------\n __Cerebro__loopcandi_consumer__( cout << \"[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute] 3d points from frame_a\\n\"; )\n cv::Mat a_imleft_srectified, a_imright_srectified;\n cv::Mat a_3dImage;\n MatrixXd a_3dpts;\n cv::Mat a_disp_viz;\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images(\n a_imleft_raw, a_imright_raw,\n a_imleft_srectified,a_imright_srectified, a_3dpts, a_3dImage, a_disp_viz );\n\n\n //-----------------------------------------------------\n //--------------- 3d points from frame_b\n //-----------------------------------------------------\n __Cerebro__loopcandi_consumer__( cout << \"[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute] 3d points from frame_b\\n\"; )\n cv::Mat b_imleft_srectified, b_imright_srectified;\n cv::Mat b_3dImage;\n MatrixXd b_3dpts;\n cv::Mat b_disp_viz;\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images(\n b_imleft_raw, b_imright_raw,\n b_imleft_srectified, b_imright_srectified, b_3dpts, b_3dImage, b_disp_viz );\n\n\n //---------------------------------------------------------------------\n //------------ point matches between a_left, b_left\n //---------------------------------------------------------------------\n __Cerebro__loopcandi_consumer__( cout << \"[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute]point matches between a_left, b_left\\n\"; )\n MatrixXd uv, uv_d; // u is from frame_a; ud is from frame_b\n ElapsedTime timer;\n timer.tic();\n StaticPointFeatureMatching::gms_point_feature_matches(a_imleft_srectified, b_imleft_srectified, uv, uv_d );\n string msg_pf_matches = to_string( timer.toc_milli() )+\" (ms) elapsed time for point_feature_matches computation and resulted in \" + std::to_string(uv.cols()) + \" number of point correspondences\" ;\n __Cerebro__loopcandi_consumer__( cout << msg_pf_matches << endl; )\n if( uv.cols() < 150 ) {\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::RED() << \"[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute]too few gms matches between node#\" << idx_1 << \" and node#\" << idx_2;\n cout << \" contains pf_matches=\" << uv.cols() << \", thresh=150, so I am rejecting this loopcandidate.\" << TermColor::RESET() << endl;\n )\n return false;\n }\n\n\n\n\n //----------------------------------------------------------------------\n //-------------- PNP and P3P\n //----------------------------------------------------------------------\n Matrix4d odom_b_T_a = node_2->getPose().inverse() * node_1->getPose();\n proc_candi = ProcessedLoopCandidate( ii, node_1, node_2 );\n proc_candi.idx_from_datamanager_1 = idx_1;\n proc_candi.idx_from_datamanager_2 = idx_2;\n proc_candi.pf_matches = uv.cols();\n\n //----- Option-A:\n __Cerebro__loopcandi_consumer__IMP( cout << TermColor::BLUE() << \"Option-A\" << TermColor::RESET() << endl; )\n std::vector feature_position_uv;\n std::vector feature_position_uv_d;\n std::vector world_point_uv;\n StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity(\n stereogeom, uv, a_3dImage, uv_d,\n feature_position_uv, feature_position_uv_d, world_point_uv);\n Matrix4d op1__b_T_a; string pnp__msg;\n #if 1\n //--\n float pnp_goodness = StaticTheiaPoseCompute::PNP( world_point_uv, feature_position_uv_d, op1__b_T_a, pnp__msg );\n //--END\n #else\n //--\n op1__b_T_a = odom_b_T_a; // setting initial guess as odometry rel pose with translation as zero\n // op1__b_T_a(0,3) = 0.0; op1__b_T_a(1,3) = 0.0; op1__b_T_a(2,3) = 0.0;\n float pnp_goodness = StaticCeresPoseCompute::PNP( world_point_uv, feature_position_uv_d, op1__b_T_a, pnp__msg );\n //--END\n #endif\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::YELLOW() << \"pnp_goodness=\" << pnp_goodness << \" op1__b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a ) << TermColor::RESET() << endl;\n )\n __Cerebro__loopcandi_consumer__(\n cout << pnp__msg << endl;\n )\n\n // reprojection debug image for op-1\n // plot( PI( op1__b_T_a * world_point_uv ) ) on imB\n #if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)\n {\n MatrixXd PI_world_point_uv, PI_world_point_uv_odom;\n GeometryUtils::idealProjection( stereogeom->get_K(), op1__b_T_a, world_point_uv, PI_world_point_uv );\n GeometryUtils::idealProjection( stereogeom->get_K(), odom_b_T_a, world_point_uv, PI_world_point_uv_odom );\n cv::Mat pi_dst_img;\n MiscUtils::plot_point_sets( b_imleft_srectified, PI_world_point_uv, pi_dst_img, cv::Scalar(0,255,0), false );\n MiscUtils::plot_point_sets( pi_dst_img, PI_world_point_uv_odom, cv::Scalar(255,0,0), false );\n MiscUtils::plot_point_sets( pi_dst_img, uv, cv::Scalar(0,0,255), false );\n MiscUtils::plot_point_sets( pi_dst_img, uv_d, cv::Scalar(255,0,255), false );\n cv::resize(pi_dst_img,pi_dst_img, cv::Size(), 0.6, 0.6 );\n MiscUtils::append_status_image( pi_dst_img, string( \"^this is image b=\")+to_string(idx_2)+\";plot( PI( op1__b_T_a * world_point_uv ) ) in green on imB;plot( PI( odom__b_T_a * world_point_uv ) ) in blue on imB;plot( uv ) in red on imB;plot( uv_d) in pink on imB;>> green and pink show alignment quality; >> ignore blue and red\");\n\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 1\n proc_candi.debug_images.push_back( pi_dst_img );\n proc_candi.debug_images_titles.push_back( \"reprojection_debug_image_1\" );\n #endif\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 2\n cv::imshow( \"plot( PI( op1__b_T_a * world_point_uv ) ) on imB\", pi_dst_img );\n #endif\n }\n\n #endif\n\n\n //----- Option-B:\n __Cerebro__loopcandi_consumer__IMP( cout << TermColor::BLUE() << \"Option-B\" << TermColor::RESET() << endl; )\n std::vector world_point_uv_d;\n StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity(\n stereogeom, uv_d, b_3dImage, uv,\n feature_position_uv_d, feature_position_uv, world_point_uv_d);\n Matrix4d op2__a_T_b, op2__b_T_a; string pnp__msg_option_B;\n #if 1\n //--\n float pnp_goodness_optioN_B = StaticTheiaPoseCompute::PNP( world_point_uv_d, feature_position_uv, op2__a_T_b, pnp__msg_option_B );\n //--END\n #else\n //--\n op2__a_T_b = odom_b_T_a.inverse(); //initial guess same as odometry\n // op2__a_T_b(0,3)=0.0;op2__a_T_b(1,3)=0.0;op2__a_T_b(2,3)=0.0;\n float pnp_goodness_optioN_B = StaticCeresPoseCompute::PNP( world_point_uv_d, feature_position_uv, op2__a_T_b, pnp__msg_option_B );\n //--END\n #endif\n\n op2__b_T_a = op2__a_T_b.inverse();\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::YELLOW() << pnp_goodness_optioN_B << \" op2__a_T_b = \" << PoseManipUtils::prettyprintMatrix4d( op2__a_T_b ) << TermColor::RESET() << endl;\n cout << TermColor::YELLOW() << pnp_goodness_optioN_B << \" op2__b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( op2__b_T_a ) << TermColor::RESET() << endl;\n )\n __Cerebro__loopcandi_consumer__(\n cout << pnp__msg_option_B << endl;\n )\n\n\n // reprojection debug image for op-2\n // plot( PI( op2__a_T_b * world_point_uv_d ) ) on imA\n #if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)\n {\n MatrixXd PI_world_point_uvd, PI_world_point_uvd_odom;\n GeometryUtils::idealProjection( stereogeom->get_K(), op2__a_T_b, world_point_uv_d, PI_world_point_uvd );\n GeometryUtils::idealProjection( stereogeom->get_K(), odom_b_T_a.inverse(), world_point_uv_d, PI_world_point_uvd_odom );\n cv::Mat pi_dst_img;\n MiscUtils::plot_point_sets( a_imleft_srectified, PI_world_point_uvd, pi_dst_img, cv::Scalar(0,255,0), false );\n MiscUtils::plot_point_sets( pi_dst_img, PI_world_point_uvd_odom, cv::Scalar(255,0,0), false );\n MiscUtils::plot_point_sets( pi_dst_img, uv, cv::Scalar(0,0,255), false );\n MiscUtils::plot_point_sets( pi_dst_img, uv_d, cv::Scalar(255,0,255), false );\n cv::resize(pi_dst_img,pi_dst_img, cv::Size(), 0.6, 0.6 );\n MiscUtils::append_status_image( pi_dst_img, string( \"^this is image a=\")+to_string(idx_1)+\";plot( PI( op2__a_T_b * world_point_uv_d ) ) in green on imA;plot( PI( odom__b_T_a * world_point_uv ) ) in blue on imA ;plot( uv ) in red on imA;plot( uv_d) in pink on imA;>> green and red show alignment quality;>> ignore blue and pink\");\n\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 1\n proc_candi.debug_images.push_back( pi_dst_img );\n proc_candi.debug_images_titles.push_back( \"reprojection_debug_image_2\" );\n #endif\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 2\n cv::imshow( \"plot( PI( op2__a_T_b * world_point_uv_d ) ) on imA\", pi_dst_img );\n #endif\n }\n\n #endif\n\n //----- Option-C\n __Cerebro__loopcandi_consumer__IMP( cout << TermColor::BLUE() << \"Option-C\" << TermColor::RESET() << endl; )\n vector< Vector3d> uv_X;\n vector< Vector3d> uvd_Y;\n StaticPointFeatureMatching::make_3d_3d_collection__using__pfmatches_and_disparity( uv, a_3dImage, uv_d, b_3dImage,\n uv_X, uvd_Y );\n Matrix4d icp_b_T_a; string p3p__msg;\n #if 1\n //--\n float p3p_goodness = StaticTheiaPoseCompute::P3P_ICP( uv_X, uvd_Y, icp_b_T_a, p3p__msg );\n //--END\n #else\n //--\n icp_b_T_a = odom_b_T_a;\n // icp_b_T_a(0,3) = 0.0; icp_b_T_a(1,3) = 0.0; icp_b_T_a(2,3) = 0.0;\n float p3p_goodness = StaticCeresPoseCompute::P3P_ICP( uv_X, uvd_Y, icp_b_T_a, p3p__msg );\n\n //--END\n #endif\n\n\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::YELLOW() << p3p_goodness << \" icp_b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( icp_b_T_a ) << TermColor::RESET() << endl;\n )\n __Cerebro__loopcandi_consumer__(\n cout << p3p__msg << endl;\n )\n\n\n // reprojection debug image for op-3\n // plot( PI( icp_b_T_a * world_point_uv ) ) on imB\n #if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)\n {\n MatrixXd PI_world_point_uv, PI_world_point_uv_odom;\n GeometryUtils::idealProjection( stereogeom->get_K(), icp_b_T_a, world_point_uv, PI_world_point_uv );\n GeometryUtils::idealProjection( stereogeom->get_K(), odom_b_T_a, world_point_uv, PI_world_point_uv_odom );\n cv::Mat pi_dst_img;\n MiscUtils::plot_point_sets( b_imleft_srectified, PI_world_point_uv, pi_dst_img, cv::Scalar(0,255,0), false );\n MiscUtils::plot_point_sets( pi_dst_img, PI_world_point_uv_odom, cv::Scalar(255,0,0), false );\n MiscUtils::plot_point_sets( pi_dst_img, uv, cv::Scalar(0,0,255), false );\n MiscUtils::plot_point_sets( pi_dst_img, uv_d, cv::Scalar(255,0,255), false );\n cv::resize(pi_dst_img,pi_dst_img, cv::Size(), 0.6, 0.6 );\n MiscUtils::append_status_image( pi_dst_img, string( \"^this is image b=\")+to_string(idx_2)+\";plot( PI( icp_b_T_a * world_point_uv ) ) in green on imB ;plot( PI( odom__b_T_a * world_point_uv ) ) in blue on imB;plot( uv ) in red on imB;plot( uv_d) in pink on imB\");\n\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 1\n proc_candi.debug_images.push_back( pi_dst_img );\n proc_candi.debug_images_titles.push_back( \"reprojection_debug_image_3\" );\n #endif\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 2\n cv::imshow( \"plot( PI( icp_b_T_a * world_point_uv ) ) on imB\", pi_dst_img );\n #endif\n }\n\n #endif\n\n // if( isnan( op1__b_T_a ) ) {\n if( op1__b_T_a != op1__b_T_a || op2__b_T_a != op2__b_T_a || icp_b_T_a != icp_b_T_a ) {\n __Cerebro__loopcandi_consumer__IMP( cout << \"=======++++ one of the 3 ways had inf or nan in them. This is bad, so skip this loop candidate.\\n\"; );\n return false;\n }\n\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::YELLOW() << \"odom_b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) << TermColor::RESET() << endl;\n cout << \"|op1 - op2|\" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a.inverse() * op2__b_T_a ) << endl;\n cout << \"|op1 - icp|\" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a.inverse() * icp_b_T_a ) << endl;\n cout << \"|op2 - icp|\" << PoseManipUtils::prettyprintMatrix4d( op2__b_T_a.inverse() * icp_b_T_a ) << endl;\n )\n\n\n\n\n\n //-----------------------------------------------------------------\n // Fill the output\n //-----------------------------------------------------------------\n\n // final pose\n // proc_candi._3d2d__2T1 = op1__b_T_a;\n // proc_candi.isSet_3d2d__2T1 = true;\n // proc_candi._3d2d__2T1__ransac_confidence = pnp_goodness;\n\n\n // Fill up all the poses which were computed\n // option-A\n proc_candi.opX_b_T_a.push_back( op1__b_T_a );\n proc_candi.opX_goodness.push_back( pnp_goodness );\n proc_candi.opX_b_T_a_name.push_back( \"op1__b_T_a\" );\n proc_candi.opX_b_T_a_debugmsg.push_back( pnp__msg );\n // Option-B\n proc_candi.opX_b_T_a.push_back( op2__b_T_a );\n proc_candi.opX_goodness.push_back( pnp_goodness_optioN_B );\n proc_candi.opX_b_T_a_name.push_back( \"op2__b_T_a\" );\n proc_candi.opX_b_T_a_debugmsg.push_back( pnp__msg_option_B );\n // Option-C\n proc_candi.opX_b_T_a.push_back( icp_b_T_a );\n proc_candi.opX_goodness.push_back( p3p_goodness );\n proc_candi.opX_b_T_a_name.push_back( \"icp_b_T_a\" );\n proc_candi.opX_b_T_a_debugmsg.push_back( p3p__msg );\n\n\n\n\n\n\n //-----------------------------------------------------------------\n // Build Viz Images\n //-----------------------------------------------------------------\n #if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)\n\n ///------A : pf matching\n cv::Mat dst_feat_matches;\n MiscUtils::plot_point_pair( a_imleft_srectified, uv, idx_1,\n b_imleft_srectified, uv_d, idx_2,\n dst_feat_matches, 3, msg_pf_matches+\";#pf-matches: \"+to_string( uv.cols() ) );\n cv::resize(dst_feat_matches, dst_feat_matches, cv::Size(), 0.5, 0.5 );\n\n\n ///----------B : Disparities\n cv::Mat dst_disp;\n MiscUtils::side_by_side( a_disp_viz, b_disp_viz, dst_disp );\n MiscUtils::append_status_image( dst_disp, \"a=\"+ to_string(idx_1)+\" b=\"+to_string(idx_2), .8 );\n cv::resize(dst_disp, dst_disp, cv::Size(), 0.5, 0.5 );\n // cv::imshow( \"dst_disp\", dst_disp );\n\n MiscUtils::append_status_image( dst_disp, \"[b_T_a <-- PNP( 3d(a), uv(b))];\"+pnp__msg+\";[a_T_b <-- PNP( 3d(b), uv(a))];\"+pnp__msg_option_B+\";[icp_b_T_a <-- ICP(3d(a), 3d(b))];\"+p3p__msg );\n\n MiscUtils::append_status_image( dst_disp, \"odom_b_T_a:\"+PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) );\n\n ///---------C: Reprojections\n //TODO\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 1\n proc_candi.debug_images.push_back( dst_feat_matches );\n proc_candi.debug_images_titles.push_back( \"apf_matches\" );\n proc_candi.debug_images.push_back( dst_disp );\n proc_candi.debug_images_titles.push_back( \"dsparity_and_poses\" );\n #endif\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 2\n cv::imshow( \"dst_feat_matches\", dst_feat_matches );\n cv::imshow( \"dst_disp\", dst_disp );\n cv::waitKey(30);\n #endif\n\n #endif\n\n\n return true;\n\n}\n\n#if 0\nbool Cerebro::process_loop_candidate_imagepair( int ii, ProcessedLoopCandidate& proc_candi )\n{\n auto u = foundLoops_i( ii );\n ros::Time t_curr = std::get<0>(u);\n ros::Time t_prev = std::get<1>(u);\n double score = std::get<2>(u);\n\n auto data_map = dataManager->getDataMapRef();\n assert( data_map->count( t_curr ) > 0 && data_map->count( t_prev ) > 0 && \"One or both of the timestamps in foundloops where not in the data_map. This cannot be happening...fatal...\\n\" );\n int idx_1 = std::distance( data_map->begin(), data_map->find( t_curr ) );\n int idx_2 = std::distance( data_map->begin(), data_map->find( t_prev ) );\n\n DataNode * node_1 = data_map->find( t_curr )->second;\n DataNode * node_2 = data_map->find( t_prev )->second;\n\n\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::BLUE() << \"{\"<\" << idx_2 << TermColor::RESET() << endl;\n )\n // cv::imshow( \"im_1\", im_1);\n // cv::imshow( \"im_2\", im_2);\n\n\n\n //----------------------------------------\n //---------Disparity of `idx_1`\n //----------------------------------------\n cv::Mat grey_im_1_left, grey_im_1_right;\n bool ret_status = retrive_stereo_pair( node_1, grey_im_1_left, grey_im_1_right );\n if( !ret_status )\n return false;\n\n\n // will get 3d points, stereo-rectified image, and disparity false colormap\n MatrixXd _3dpts__im1; //4xN. 3d points of im1\n cv::Mat _3dImage__im1;\n cv::Mat imleft_srectified, imright_srectified;\n cv::Mat disparity_for_visualization;\n ElapsedTime timer;\n timer.tic();\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images( grey_im_1_left, grey_im_1_right,\n imleft_srectified,imright_srectified,\n _3dpts__im1, _3dImage__im1, disparity_for_visualization );\n __Cerebro__loopcandi_consumer__(\n cout << timer.toc_milli() << \" (ms)!! get_srectifiedim_and_3dpoints_and_disparity_from_raw_images\\n\";\n )\n\n\n\n\n\n //---------------- END Disparity of `idx_1`\n\n\n\n //--------------------------------------------------------------\n //------------ srectified of idx_2 image pair needed\n //--------------------------------------------------------------\n cv::Mat grey_im_2_left, grey_im_2_right;\n bool ret_status_2 = retrive_stereo_pair( node_2, grey_im_2_left, grey_im_2_right );\n if( !ret_status_2 )\n return false;\n\n cv::Mat im2_left_srectified, im2_right_srectified;\n timer.tic();\n // TODO: If feasible can also compute depth of im2, so that the pose computed can be verified for consistency.\n // TODO: accept the pose if pnp( 3d of idx_1, 2d of idx_2 ) === pnp( 2d of idx_1, 3d of idx_2 )\n stereogeom->do_stereo_rectification_of_raw_images( grey_im_2_left, grey_im_2_right,\n im2_left_srectified, im2_right_srectified );\n __Cerebro__loopcandi_consumer__(\n cout << timer.toc_milli() << \" (ms)!! do_stereo_rectification_of_raw_images\\n\";\n )\n // cv::imshow( \"im2_left_srectified\", im2_left_srectified );\n\n //------------- END srectified of idx_2 image pair needed\n\n\n\n //-----------------------------------------------------------------------\n //----------point_feature_matches for `idx_1` <--> `idx_2`\n // gms matcher\n //-----------------------------------------------------------------------\n MatrixXd uv, uv_d; // u is from frame_a; ud is from frame_b\n timer.tic();\n StaticPointFeatureMatching::gms_point_feature_matches( imleft_srectified, im2_left_srectified, uv, uv_d );\n auto elapsed_time_gms = timer.toc_milli();\n __Cerebro__loopcandi_consumer__(\n cout << elapsed_time_gms << \" (ms)!! gms_point_feature_matches\\n\";\n )\n\n\n //--------------------- END Matcher ----------------------------------\n\n\n\n //---------------- make collection of 3d 2d points\n // 3d of `idx_1` <---> 2d of `idx_2`\n\n\n std::vector feature_position_uv;\n std::vector feature_position_uv_d;\n std::vector world_point;\n\n\n StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity( stereogeom, uv, _3dImage__im1, uv_d,\n feature_position_uv, feature_position_uv_d, world_point );\n // make_3d_2d_collection__using__pfmatches_and_disparity( uv, _3dImage__im1, uv_d,\n // feature_position_uv, feature_position_uv_d, world_point );\n int n_valid_depths = world_point.size();\n\n\n\n\n // TODO 2d of idx_1 <---> 3d of idx_2\n\n\n\n\n\n // See if the matching can be rejected due to insufficient # of matches\n if( uv.cols() < 150 ) {\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::YELLOW() << \"of the total \"+std::to_string(uv.cols())+\" point feature correspondences \" +std::to_string(n_valid_depths)+ \" had valid depths. \" << \" too few pf-matches from gms. Skip this. TH=80\" << TermColor::RESET() << endl;\n )\n return false;\n }\n\n if( n_valid_depths < 150 ) {\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::YELLOW() << \"of the total \"+std::to_string(uv.cols())+\" point feature correspondences \" +std::to_string(n_valid_depths)+ \" had valid depths. \" << \"too few pf-matches depths from gms. Skip this. TH=80\" << TermColor::RESET() << endl;\n )\n return false;\n }\n\n // TODO assess u, ud, world_point for histogram spreads. a more spread in measurements will give more precise more. Also will be helpful to weedout bad poses\n\n //------------------------------------------\n //-------------- theia::pnp\n //------------------------------------------\n Matrix4d b_T_a; //< RESULT\n string pnp__msg = string(\"\"); //< msg about pnp\n #if 1\n //--- DlsPnp\n std::vector solution_rotations;\n std::vector solution_translations;\n timer.tic();\n theia::DlsPnp( feature_position_uv_d, world_point, &solution_rotations, &solution_translations );\n auto elapsed_dls_pnp = timer.toc_milli() ;\n __Cerebro__loopcandi_consumer__(\n cout << elapsed_dls_pnp << \" : (ms) : theia::DlsPnp done in\\n\";\n cout << \"solutions count = \" << solution_rotations.size() << \" \" << solution_translations.size() << endl;\n )\n\n if( solution_rotations.size() == 0 ) {\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::RED() << \" theia::DlsPnp returns no solution\" << TermColor::RESET() << endl;\n )\n return false;\n }\n\n if( solution_rotations.size() > 1 ) {\n __Cerebro__loopcandi_consumer__IMP(\n cout << TermColor::RED() << \" theia::DlsPnp returns multiple solution\" << TermColor::RESET() << endl;\n )\n return false;\n }\n\n // retrive solution\n b_T_a = Matrix4d::Identity();\n b_T_a.topLeftCorner(3,3) = solution_rotations[0].toRotationMatrix();\n b_T_a.col(3).topRows(3) = solution_translations[0];\n\n __Cerebro__loopcandi_consumer__IMP(\n // cout << \"solution_T \" << b_T_a << endl;\n cout << TermColor::GREEN() << \"DlsPnp (b_T_a): \" << PoseManipUtils::prettyprintMatrix4d( b_T_a ) << TermColor::RESET() << endl;\n // out_b_T_a = b_T_a;\n pnp__msg += \"DlsPnp (b_T_a): \" + PoseManipUtils::prettyprintMatrix4d( b_T_a ) + \";\";\n pnp__msg += \" elapsed_dls_pnp=\"+to_string(elapsed_dls_pnp)+\";\";\n )\n #endif\n\n\n #if 1\n //--- DlsPnpWithRansac\n __Cerebro__loopcandi_consumer__( timer.tic() );\n // prep data\n vector data_r;\n for( int i=0 ; i ransac_estimator(params, dlspnp_estimator);\n // Initialize must always be called!\n ransac_estimator.Initialize();\n\n theia::RansacSummary summary;\n ransac_estimator.Estimate(data_r, &best_rel_pose, &summary);\n\n auto elapsed_dls_pnp_ransac=timer.toc_milli();\n __Cerebro__loopcandi_consumer__IMP(\n cout << elapsed_dls_pnp_ransac << \"(ms)!! DlsPnpWithRansac ElapsedTime includes data prep\\n\";\n cout << \"Ransac:\";\n // for( int i=0; iget_K() * _X_i; //< scaling with camera-intrinsic matrix\n\n\n Vector3d _tmp;\n _tmp << feature_position_uv_d[i], 1.0 ;\n _detectedpts_of_B.col(i) = stereogeom->get_K() * _tmp;\n\n _tmp << feature_position_uv[i], 1.0 ;\n _detectedpts_of_A.col(i) = stereogeom->get_K() * _tmp;\n\n\n Vector3d delta = _3dpts_of_A_projectedonB.col(i) - _detectedpts_of_B.col(i);\n if( abs(delta(0)) < 2. && abs(delta(1)) < 2. ) {\n // cout << \" delta=\" << delta.transpose();\n n_good++;\n }\n else {\n // cout << TermColor::RED() << \"delta=\" << delta.transpose() << TermColor::RESET();\n }\n // cout << endl;\n }\n // cout << \"n_good=\" <isPoseAvailable() && node_2->isPoseAvailable() ) {\n\n Matrix4d odom_wTA = node_1->getPose();\n // cout << \"wTa(odom)\" << PoseManipUtils::prettyprintMatrix4d( wTa ) << endl;\n Matrix4d odom_wTB = node_2->getPose();\n // cout << \"wTb(odom)\" << PoseManipUtils::prettyprintMatrix4d( wTb ) << endl;\n Matrix4d odom_b_T_a = odom_wTB.inverse() * odom_wTA;\n\n\n __Cerebro__loopcandi_consumer__(\n cout << \"odom_b_T_a\" << PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) << endl;\n )\n pnp__msg += \"odom_b_T_a \"+PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) +\";\";\n\n auto a_timestamp = node_1->getT();\n auto b_timestamp = node_2->getT();\n pnp__msg += \"a.timestamp: \" + to_string(a_timestamp.toSec());\n pnp__msg += \" b.timestamp: \" + to_string(b_timestamp.toSec()) + \";\";\n\n // PI( odom_b_T_a * a_X )\n for( int i=0 ; iget_K() * _X_i; //< scaling with camera-intrinsic matrix\n }\n\n }\n else {\n cout << TermColor::RED() << \"[Cerebro::process_loop_candidate_imagepair] In plotting part needs poses from DataManager which is not available.\" << TermColor::RESET() << endl;\n }\n\n #endif\n\n\n\n\n // plot( B, ud )\n cv::Mat __dst;\n MiscUtils::plot_point_sets( im2_left_srectified, _detectedpts_of_B, __dst, cv::Scalar( 0,0,255), false, \"_detectedpts_of_B (red) npts=\"+to_string(_detectedpts_of_B.cols()) );\n // cv::imshow( \"plot( B, ud )\", __dst );\n\n // plot( B, _3dpts_of_A_projectedonB )\n MiscUtils::plot_point_sets( __dst, _3dpts_of_A_projectedonB, cv::Scalar( 0,255,255), false, \";_3dpts_of_A_projectedonB, PI( b_T_a * X),(yellow)\" );\n // MiscUtils::plot_point_sets( b_imleft_srectified, _3dpts_of_A_projectedonB, __dst, cv::Scalar( 0,255,255), false, \"_3dpts_of_A_projectedonB\" );\n\n MiscUtils::plot_point_sets( __dst, _detectedpts_of_A, cv::Scalar( 255,255,255), false, \";;_detectedpts_of_A(white)\" );\n\n MiscUtils::plot_point_sets( __dst, _3dpts_of_A_projectedonB_with_odom_rel_pose, cv::Scalar( 255,0,0), false, \";;;_3dpts_of_A_projectedonB_with_odom_rel_pose, PI( odom_b_T_a * X),(blue)\" );\n\n\n // status image for __dst.\n string status_msg = \"im2_left_srectified;\";\n status_msg += \"_detectedpts_of_B (red);\";\n status_msg += \"_3dpts_of_A_projectedonB, PI( b_T_a * X),(yellow);\";\n status_msg += \"_detectedpts_of_A(white);\";\n status_msg += \"_3dpts_of_A_projectedonB_with_odom_rel_pose, PI( odom_b_T_a * X),(blue);\";\n MiscUtils::append_status_image( __dst, status_msg+\";\"+pnp__msg );\n\n // proc_candi.pnp_verification_image = __dst;\n proc_candi.debug_images.push_back( __dst );\n proc_candi.debug_images_titles.push_back( \"pnp_verification_image\" );\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 2\n cv::imshow( \"im2_left_srectified\", __dst );\n #endif\n\n #endif\n\n\n\n // disparity and other images\n #if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)\n // proc_candi.node_1_disparity_viz = disparity_for_visualization; // this is disparity of im1. see code way above\n proc_candi.debug_images.push_back( disparity_for_visualization );\n proc_candi.debug_images_titles.push_back( \"node_1_disparity_viz\" );\n\n #if __Cerebro__loopcandi_consumer__IMSHOW == 2\n // cv::imshow( \"imleft_srectified\", imleft_srectified );\n // cv::imshow( \"imright_srectified\", imright_srectified );\n cv::imshow( \"im1_disparity_for_visualization\", disparity_for_visualization );\n #endif\n #endif\n\n\n\n // Done\n #if __Cerebro__loopcandi_consumer__IMSHOW == 2\n cv::waitKey(50);\n #endif\n\n return true;\n\n\n\n\n\n}\n#endif\n//////////// END pose computation //////////////////////////////\n\n\n//--------------------------------------------------------------//\n//------------------ END Geometry Thread -----------------------//\n//--------------------------------------------------------------//\n\n\n\n///////////////////////////////////////////////////////////\n//---------------------- KIDNAP -------------------------//\n///////////////////////////////////////////////////////////\n\n// Kidnap identification thread. This thread monitors dataManager->getDataMapRef().size\n// for every new node added if there are zero tracked features means that, I have\n// been kidnaped. It however declares kidnap only after 2 sec of kidnaped\n// #define __Cerebro__kidnaped_thread__( msg ) msg;\n#define __Cerebro__kidnaped_thread__( msg ) ;\n\n// #define __Cerebro__kidnaped_thread__debug( msg ) msg;\n#define __Cerebro__kidnaped_thread__debug( msg ) ;\nvoid Cerebro::kidnaped_thread( int loop_rate_hz )\n{\n if( loop_rate_hz <= 0 || loop_rate_hz >= 30 ) {\n cout << TermColor::RED() << \"[Cerebro::kidnaped_thead] Invalid loop_rate_hz. Expected to be between [1,30]\\n\" << TermColor::RESET() << endl;\n return;\n }\n\n cout << TermColor::GREEN() << \"Start Cerebro::kipnaped_thead\\n\" << TermColor::RESET() << endl;\n\n\n if( dataManager->isKidnapIndicatorPubSet() == false ) {\n cout << TermColor::RED() << \"FATAL ERROR in [Cerebro::kidnaped_thread] kidnap indicator ros::Publishers were not set\\n\";\n exit(2);\n }\n\n\n //---\n //--- Main Settings for this thread\n //---\n const int THRESH_N_FEATS = 15; //declare kidnap if number of tracked features fall below 15\n const ros::Duration WAIT_BEFORE_DECLARING_AS_KIDNAP = ros::Duration(3.0); // wait this many seconds, of low feature tracking count before declaring kidnap\n\n\n ros::Rate loop_rate( loop_rate_hz );\n int prev_count = 0, new_count = 0;\n ros::Time last_known_keyframe;\n\n // TODO: Move both of these to class variables and make them atomic. is_kidnapped_start is valid only when is_kidnapped==true\n bool is_kidnapped = false;\n bool is_kidnapped_more_than_n_sec = false;\n ros::Time is_kidnapped_start;\n\n\n // Related to handling to playing multiple bags one-after-another.\n bool first_data_received = false;\n bool waiting_for_next_bag_to_start = false;\n\n while( b_kidnaped_thread_enable ) {\n // Book Keeping\n prev_count = new_count;\n loop_rate.sleep();\n\n\n\n auto data_map = dataManager->getDataMapRef(); //< map< ros::Time, DataNode*>\n new_count = data_map->size();\n\n if( new_count <= prev_count ) {\n __Cerebro__kidnaped_thread__(cout << \"[Cerebro::kidnaped_thread]Nothing new\\n\";)\n continue;\n }\n\n ros::Time lb = data_map->rbegin()->first - ros::Duration(5, 0); // look at recent 5sec.\n // auto S = data_map.begin();\n auto S = data_map->lower_bound( lb );\n auto E = data_map->end();\n __Cerebro__kidnaped_thread__debug( cout << \"[Cerebro::kidnaped_thread] S=\" << S->first << \" E=\" << E->first << endl; )\n\n for( auto it = S ; it != E ; it++ )\n {\n\n #if 0\n if( it->second->isImageAvailable() ) {\n cout << TermColor::GREEN();\n } else { cout << TermColor::BLUE() ; }\n\n if( it->second->isPoseAvailable() && it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) {\n cout << \"A\";\n }\n if( !it->second->isPoseAvailable() && it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) {\n cout << \"B\";\n }\n if( it->second->isPoseAvailable() && ! (it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) ) {\n cout << \"C\";\n }\n if( !it->second->isPoseAvailable() && ! (it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) ){\n cout << \"D\";\n }\n cout << TermColor::RESET() ;\n #endif\n\n\n int n_feats = it->second->getNumberOfSuccessfullyTrackedFeatures();\n if( it->first <= last_known_keyframe || n_feats < 0 )\n continue;\n\n __Cerebro__kidnaped_thread__debug(\n if( n_feats > 50 )\n cout <first << \": n_feats=\" << TermColor::iGREEN() << n_feats << TermColor::RESET() << endl;\n if( n_feats > 30 && n_feats<=50 )\n cout <first << \": n_feats=\" << TermColor::GREEN() << n_feats << TermColor::RESET() << endl;\n if( n_feats > 20 && n_feats<=30 )\n cout <first << \": n_feats=\" << TermColor::YELLOW() << n_feats << TermColor::RESET() << endl;\n if( n_feats > 10 && n_feats<=20 )\n cout <first << \": n_feats=\" << TermColor::RED() << n_feats << TermColor::RESET() << endl;\n if( n_feats<=10 )\n cout <first << \": n_feats=\" << TermColor::iRED() << n_feats << TermColor::RESET() << endl;\n )\n\n last_known_keyframe = it->first;\n\n\n if( is_kidnapped==false && n_feats < THRESH_N_FEATS ) {\n is_kidnapped = true;\n is_kidnapped_start = it->first;\n\n __Cerebro__kidnaped_thread__(\n cout << TermColor::RED() << \"I am kidnapped t=\" << it->first << TermColor::RESET() << endl;\n cout << \"I think so because the number of tracked features (from feature tracker) have fallen to only \"\n << n_feats << \", the threshold was \" << THRESH_N_FEATS\n << \". However, I will wait for \" << WAIT_BEFORE_DECLARING_AS_KIDNAP << \" sec to declare kidnapped to vins_estimator.\" << endl;\n )\n }\n\n __Cerebro__kidnaped_thread__(\n if( is_kidnapped ) {\n cout << \"is_kidnapped=true t=\" << it->first << \" n_feats=\" << n_feats << endl;\n }\n )\n\n if( is_kidnapped && !is_kidnapped_more_than_n_sec && (it->first - is_kidnapped_start) > WAIT_BEFORE_DECLARING_AS_KIDNAP )\n {\n __Cerebro__kidnaped_thread__(\n cout << \"Kidnapped for more than \" << WAIT_BEFORE_DECLARING_AS_KIDNAP << \"sec. I am quite confident that I have been kidnapped\\n\";\n cout << \"PUBLISH FALSE\\n\";\n )\n is_kidnapped_more_than_n_sec = true;\n\n dataManager->PUBLISH__FALSE( is_kidnapped_start );\n\n }\n\n if( is_kidnapped && n_feats > THRESH_N_FEATS ) {\n __Cerebro__kidnaped_thread__(\n cout << TermColor::GREEN() << \"Looks like i have been unkidnapped t=\" << it->first << TermColor::RESET() << endl;\n )\n\n if( is_kidnapped_more_than_n_sec )\n {\n // publish true to vins_estimator to indicate that it may resume the estimation with a new co-ordinate system.\n dataManager->PUBLISH__TRUE( it->first );\n\n }\n is_kidnapped = false;\n is_kidnapped_more_than_n_sec = false;\n\n }\n }\n // cout << endl;\n\n\n }\n\n\n cout << TermColor::RED() << \"Cerebro::kipnaped_thead Ends\\n\" << TermColor::RESET() << endl;\n}\n\n\nbool Cerebro::kidnap_info( int i, ros::Time& start_, ros::Time& end_ )\n{\n std::lock_guard lk(mutex_kidnap);\n if( i>=0 && i lk(mutex_kidnap);\n json json_obj;\n json_obj[\"meta_info\"][\"state_is_kidnapped\"] = (bool) state_is_kidnapped;\n json_obj[\"meta_info\"][\"len_start_of_kidnap\"] = start_of_kidnap.size();\n json_obj[\"meta_info\"][\"len_end_of_kidnap\"] = end_of_kidnap.size();\n for( int i=0 ; i lk(mutex_kidnap);\n return start_of_kidnap.size();\n}\n\nbool Cerebro::is_kidnapped(){ state_is_kidnapped; }\n\n\n#define __CEREBRO_kidnap_callbacks(msg) msg;\n// #define __CEREBRO_kidnap_callbacks(msg) ;\n\nvoid Cerebro::kidnap_bool_callback( const std_msgs::BoolConstPtr& rcvd_bool )\n{\n return; // ignore this.\n __CEREBRO_kidnap_callbacks( cout << TermColor::iCYAN() << \"[Cerebro::kidnap_bool_callback] msg=\" << (bool)rcvd_bool->data << TermColor::RESET() << endl; )\n}\n\nvoid Cerebro::kidnap_header_callback( const std_msgs::HeaderConstPtr& rcvd_header )\n{\n __CEREBRO_kidnap_callbacks(\n cout << TermColor::iCYAN() << \"[Cerebro::kidnap_header_callback]\" ;\n cout << rcvd_header->stamp << \":\" << rcvd_header->frame_id ;\n cout << TermColor::RESET() << endl;\n )\n\n if( rcvd_header->frame_id == \"kidnapped\" ) {\n std::lock_guard lk(mutex_kidnap);\n this->state_is_kidnapped = true;\n start_of_kidnap.push_back( rcvd_header->stamp );\n __CEREBRO_kidnap_callbacks(\n cout << TermColor::iCYAN() << \"start_of_kidnap.push_back(\"<< rcvd_header->stamp << \")\" << TermColor::RESET() << endl;\n )\n return;\n }\n\n if( rcvd_header->frame_id == \"unkidnapped\" ) {\n std::lock_guard lk(mutex_kidnap);\n this->state_is_kidnapped = false;\n end_of_kidnap.push_back( rcvd_header->stamp );\n __CEREBRO_kidnap_callbacks(\n cout << TermColor::iCYAN() << \"end_of_kidnap.push_back(\"<< rcvd_header->stamp << \")\" << TermColor::RESET() << endl;\n )\n return;\n }\n\n assert( false && \"in Cerebro::kidnap_header_callback. rcvd_header->frame_id is something other than `kidnapped` or `unkidnapped`.\");\n\n}\n\n\n\n\n\n\n///////////////////////////////////////////////////////////\n//---------------------- KIDNAP -------------------------//\n///////////////////////////////////////////////////////////\n\n\n\n//-----------------------------------------------------------------\n// Cerebro Private Utils\n//-----------------------------------------------------------------\n// private function for descriptor_dot_product thread\nbool Cerebro::wait_until__connectedToDescServer_and_descSizeAvailable( int timeout_in_sec ) //blocking call\n{\n assert( timeout_in_sec > 2 );\n ros::Rate rate(10);\n // wait until connected_to_descriptor_server=true and descriptor_size_available=true\n int wait_itr = 0;\n while( true ) {\n if( this->connected_to_descriptor_server && this->descriptor_size_available)\n break;\n __Cerebro__run__(cout << wait_itr << \" [Cerebro::wait_until__connectedToDescServer_and_descSizeAvailable] waiting for `descriptor_size_available` to be true. timeout_in_sec=\" << timeout_in_sec << \"\\n\";)\n rate.sleep();\n wait_itr++;\n if( wait_itr > timeout_in_sec*10 ) {\n __Cerebro__run__( cout << TermColor::RED() << \"[Cerebro::wait_until__connectedToDescServer_and_descSizeAvailable] `this->connected_to_descriptor_server && this->descriptor_size_available` has not become true dispite waiting for about \"<< timeout_in_sec << \" sec. So quiting the run thread.\\n\" << TermColor::RESET(); )\n return false;\n }\n }\n return true;\n}\n\n\n//-----------------------------------------------------------------\n// END Cerebro Private Utils\n//-----------------------------------------------------------------\n" }, { "path": "src/Cerebro.h", "content": "#pragma once\n\n/** Cerebro Class\n This class is suppose to be the main-brain of this package.\n It has to run its own threads (should not block)\n\n It can access DataManager::camera, DataManager::imu_T_cam, DataManager::data_map.\n\n Author : Manohar Kuse \n Created : 29th Oct, 2018\n*/\n\n#include \n#include \n#include \n#include \n#include \n\n#include \"PinholeCamera.h\"\n#include \"DataManager.h\"\n#include \"ProcessedLoopCandidate.h\"\n#include \"DlsPnpWithRansac.h\"\n#include \"HypothesisManager.h\"\n\n#include \"utils/TermColor.h\"\n#include \"utils/ElapsedTime.h\"\n#include \"utils/Plot2Mat.h\"\n\n#include \"utils/CameraGeometry.h\"\n#include \"utils/PointFeatureMatching.h\"\n\n\n#include \"utils/nlohmann/json.hpp\"\nusing json = nlohmann::json;\n\n// ROS-Service Defination\n#include \n\n// #include \n\n#include \n#include \nusing namespace Eigen;\nusing namespace std;\n\n\n//comment this out to remove dependence on faiss.\n// If using faiss, also remember to link to libfaiss.so. See my CMakeList file to know how to do it.\n// #define HAVE_FAISS\n\n#ifdef HAVE_FAISS\n// faiss is only used for generating loopcandidates.\n// only function that uses faiss is `Cerebro::faiss__naive_loopcandidate_generator()`\n// Although faiss is strictly not needed, it is a much // faster way for nearest neighbour search. If you don't want it\n// just use the naive implementation: descrip_N__dot__descrip_0_N().\n// Note: Cerebro::descrip_N__dot__descrip_0_N() and Cerebro::faiss__naive_loopcandidate_generator()\n// are exactly same in functionality.\n#include \n#endif //HAVE_FAISS\n\nclass Cerebro\n{\n\n //-------------- Constructor --------------------//\npublic:\n Cerebro( ros::NodeHandle& nh ); // TODO removal of nh argument.\n void setDataManager( DataManager* dataManager );\n void setPublishers( const string base_topic_name );\n\nprivate:\n // global private variables\n bool m_dataManager_available=false;\n DataManager * dataManager;\n ros::NodeHandle nh; ///< This is here so that I can set new useful publishers\n\n bool m_pub_available = false;\n ros::Publisher pub_loopedge;\n\n //-------------- END Constructor --------------------//\n\n\n\n\n //--------------- Descriptor Computation Thread ------------------//\npublic:\n // This monitors the dataManager->data_map and makes sure the descriptor are uptodate.\n // descriptors are computed by an external ros-service. in the future can have\n // more similar threads to compute object bounding boxes, text and other perception related services.\n void descriptor_computer_thread_enable() { b_descriptor_computer_thread = true; }\n void descriptor_computer_thread_disable() { b_descriptor_computer_thread = false; }\n void descriptor_computer_thread();\n\nprivate:\n atomic b_descriptor_computer_thread;\n atomic connected_to_descriptor_server;\n atomic descriptor_size_available;\n atomic descriptor_size;\n\n\n // Storage for Intelligence\n mutable std::mutex m_wholeImageComputedList;\n vector wholeImageComputedList; ///< A list of stamps where descriptors are computed and available.\n void wholeImageComputedList_pushback( const ros::Time __tx ); //this is kept private on purpose so that others from outside cannot pushback here.\npublic:\n const int wholeImageComputedList_size() const; //size of the list. threadsafe\n const ros::Time wholeImageComputedList_at(int k) const; //< returns kth element of the list. threadsafe\n //--------------- END Descriptor Computation Thread ------------------//\n\n\n\n\n //---------------- Populate Loop Candidates --------------------//\npublic:\n // This is supposed to be run in a separate thread.\n void run_thread_enable() { b_run_thread = true; }\n void run_thread_disable() { b_run_thread = false; }\n void run(); //< The loopcandidate (geometrically unverified) producer.\n\n //### Method-A : Naive method of dot product DIY\n void descrip_N__dot__descrip_0_N(); //< Naive method of dot product DIY\n\n #ifdef HAVE_FAISS\n //### Method-B : same functionality to descrip_N__dot__descrip_0_N() but uses facebook's faiss library\n void faiss__naive_loopcandidate_generator();\n\n\n //### Method-C:\n //### In this we get say 5 nearest neighbour for every l_i.\n //### Collect nearest neigbours for say 4 consecutive l_i's.\n //### Nearby predictions are merged.\n void faiss_clique_loopcandidate_generator();\n\n\n //### Method-D:\n // Uses a separate hypothesis manager. My multihyp framework.\n // elaborate scheme, still under development.\n void faiss_multihypothesis_tracking();\n #endif //HAVE_FAISS\n\n\n\nprivate:\n // private things to run thread\n atomic b_run_thread;\n bool wait_until__connectedToDescServer_and_descSizeAvailable( int timeout_in_sec ); //blocking call\n\n\npublic:\n // These are loop candidates. These calls are thread-safe\n // producer: `run()`\n // user: `Visualization::publish_loopcandidates`\n const int foundLoops_count() const ;\n const std::tuple foundLoops_i( int i) const;\n json foundLoops_as_JSON();\n\nprivate:\n mutable std::mutex m_foundLoops;\n vector< std::tuple > foundLoops; // a list containing loop pairs. this is populated by `run()`\n\n\n //---------------- END Populate Loop Candidates --------------------//\n\n\n\n //------------------ Geometry Thread ---------------------------//\n // calls this->foundLoops_count() and this->foundLoops_i() and uses dataManager\n // to geometric verify and to compute the poses of loop-pairs.\npublic:\n void loopcandidate_consumer_enable() { b_loopcandidate_consumer=true; }\n void loopcandidate_consumer_disable() { b_loopcandidate_consumer=false; }\n void loopcandiate_consumer_thread();\n\n const int processedLoops_count() const;\n const ProcessedLoopCandidate& processedLoops_i( int i ) const;\n // TODO: have a function which returns a json of the info in processedloopcandi_list.\n\n\n\n\n\nprivate:\n atomic b_loopcandidate_consumer;\n\n // helpers\n\n // Processed foundLoops_i[ j ] and writes the info in the object `proc_candi`\n // bool process_loop_candidate_imagepair( int j, ProcessedLoopCandidate& proc_candi );\n\n // This function processes the jth loopcandidate and fills in the ProcessedLoopCandidate.\n // The return status means that some poses were computed. It doesn't mean the poses were consistent.\n // Infact, nothing about consistency is performed here. It just computes relative poses using 3 indipendent way.\n bool process_loop_candidate_imagepair_consistent_pose_compute( int j, ProcessedLoopCandidate& proc_candi ); //< enhanced version of the above\n\n bool init_stereogeom(); // expected to be called in loopcandiate_consumer_thread. this sets the variable `stereogeom`\n bool retrive_stereo_pair( DataNode* node, cv::Mat& left_image, cv::Mat& right_image, bool bgr2gray=true );\n std::shared_ptr stereogeom;\n\n\n mutable std::mutex m_processedLoops;\n vector< ProcessedLoopCandidate > processedloopcandi_list;\n\n\n\n //------------------ END Geometry Thread ---------------------------//\n\n\n //--------------- kidnaped identification thread ------------------//\npublic:\n void kidnaped_thread( int loop_rate_hz=5);\n void kidnaped_thread_enable() {b_kidnaped_thread_enable=true;};\n void kidnaped_thread_disable() {b_kidnaped_thread_enable=false;};\n\n bool is_kidnapped(); // gives the current (kidnap) status. threadsafe\n bool kidnap_info( int i, ros::Time& start_, ros::Time& end_ ); //< returns true if i was a valid kidnap index. return false if it was an invalid index, ie. such kidnap didnt exist\n json kidnap_info_as_json();\n int n_kidnaps(); //< will with return length of `start_of_kidnap` current state has to be inferred by call to `is_kidnapped()`\n\n // kidnap callbacks\n void kidnap_bool_callback( const std_msgs::BoolConstPtr& rcvd_header ) ;\n void kidnap_header_callback( const std_msgs::HeaderConstPtr& rcvd_header ) ;\n\nprivate:\n atomic b_kidnaped_thread_enable;\n\n std::mutex mutex_kidnap;\n atomic< bool > state_is_kidnapped;\n vector< ros::Time > start_of_kidnap;\n vector< ros::Time > end_of_kidnap;\n\n ros::Publisher rcvd_flag_pub;\n ros::Publisher kidnap_indicator_header_pub;\n bool is_kidnapn_indicator_set = false;\n};\n" }, { "path": "src/DataManager.cpp", "content": "#include \"DataManager.h\"\n\nDataManager::DataManager(ros::NodeHandle &nh )\n//: out_stream(ofstream(\"/dev/pts/0\",ios::out) )\n{\n this->nh = nh;\n}\n\n\n\nDataManager::DataManager(const DataManager &obj)\n//: out_stream(ofstream(\"/dev/pts/0\",ios::out) )\n{\n cout << \"Copy constructor allocating ptr.\" << endl;\n}\n\n\n\n//////////////////////////////////////////////////////////////////////////////\n//////////////////// Setters and Getters for global info /////////////////////\n//////////////////////////////////////////////////////////////////////////////\n\n// void DataManager::setCamera( const PinholeCamera& camera )\n// {\n// this->camera = camera;\n//\n// cout << \"--- Camera Params from DataManager ---\\n\";\n// this->camera.printCameraInfo();\n// // cout << \"K\\n\" << this->camera.e_K << endl;\n// // cout << \"D\\n\" << this->camera.e_D << endl;\n// cout << \"--- END\\n\";\n// }\n\n\n\n\nvoid DataManager::setAbstractCamera( camodocal::CameraPtr abs_camera, short cam_id )\n{\n assert( cam_id >= 0 && \"DataManager, you requested a camera with negative id which is an error. cam_id=0 for default camera and 1,2,.. for additional cameras.\\n\" );\n assert(abs_camera && \"[DataManager::setCamera] in datamanager you are trying to set an invalid abstract camera. You need to loadFromYAML before you can set this camera\\n\");\n // this->abstract_camera = abs_camera;\n this->all_abstract_cameras[cam_id] = abs_camera;\n\n cout << \"--- Abstract CameraParams(cam_id=\" << cam_id << \") from DataManager ---\\n\";\n // cout << this->abstract_camera->parametersToString() << endl;\n cout << this->all_abstract_cameras[cam_id]->parametersToString() << endl;\n cout << \"--- END\\n\";\n}\n\ncamodocal::CameraPtr DataManager::getAbstractCameraRef(short cam_id) const\n{\n assert( cam_id >= 0 && \"DataManager, you requested a camera with negative id which is an error. cam_id=0 for default camera and 1,2,.. for additional cameras.\\n\" );\n // assert( abstract_camera && \"[DataManager::getAbstractCameraRef] you are requesting a camera reference before setting.\\n\" );\n // return abstract_camera;\n assert( isAbstractCameraSet(cam_id) && \"[DataManager::getAbstractCameraRef] you are requesting a camera reference before setting.\\n\");\n // return this->all_abstract_cameras[cam_id]; //old non-const access\n return this->all_abstract_cameras.at(cam_id);\n}\n\nbool DataManager::isAbstractCameraSet(short cam_id) const\n{\n assert( cam_id >= 0 && \"DataManager, you requested a camera with negative id which is an error. cam_id=0 for default camera and 1,2,.. for additional cameras.\\n\" );\n if( this->all_abstract_cameras.count( cam_id ) > 0 ) {\n return true;\n // if( this->all_abstract_cameras[cam_id] )\n // return true;\n // else\n // return false;\n }\n else {\n return false;\n }\n}\n\nvector DataManager::getAbstractCameraKeys() const {\n vector keys;\n for( auto it=this->all_abstract_cameras.begin(); it!=all_abstract_cameras.end() ; it++ ) {\n keys.push_back( it->first );\n }\n return keys;\n}\n\n\nvoid DataManager::setCameraRelPose( Matrix4d a_T_b, std::pair pair_a_b )\n{\n // assert a and b abstract cameras exisits\n assert( isAbstractCameraSet(pair_a_b.first) && isAbstractCameraSet(pair_a_b.second) && \"in [DataManager::setCameraRelPose] one of the abstract-cameras were not set, even though you asked me to set their relative pose. You need to set the cameras first before you can set the relative poses.\\n\" );\n\n cout << \"---DataManager::setCameraRelPose---\\n\";\n cout << \"setting \"<< pair_a_b.first << \"_T_\" << pair_a_b.second << \" :::> \" << PoseManipUtils::prettyprintMatrix4d( a_T_b ) << endl;\n\n cam_relative_poses[ pair_a_b ] = a_T_b;\n cout << \"---DONE---\\n\";\n\n}\n\nbool DataManager::isCameraRelPoseSet( std::pair pair_a_b ) const\n{\n if( this->cam_relative_poses.count( pair_a_b ) > 0 )\n return true;\n else\n return false;\n}\n\nconst Matrix4d& DataManager::getCameraRelPose( std::pair pair_a_b ) const\n{\n assert( isCameraRelPoseSet( pair_a_b ) && \"[DataManager::getCameraRelPose] make sure the rel cam pose you are requesting is available\\n\" );\n if( !isCameraRelPoseSet( pair_a_b) ) {\n ROS_ERROR( \"[DataManager::getCameraRelPose] make sure the rel cam pose you are requesting is available\\nYou requested (%d,%d) which is not available\", pair_a_b.first, pair_a_b.second );\n exit(2);\n }\n\n // return this->cam_relative_poses[ pair_a_b ]; //old non const\n return this->cam_relative_poses.at( pair_a_b );\n}\n\n\nvector< std::pair > DataManager::getCameraRelPoseKeys()\n{\n vector< pair > keys;\n for( auto it=this->cam_relative_poses.begin(); it!=cam_relative_poses.end() ; it++ ) {\n keys.push_back( it->first );\n }\n return keys;\n}\n\nconst Matrix4d& DataManager::getIMUCamExtrinsic() const\n{\n std::lock_guard lk(global_vars_mutex);\n assert( imu_T_cam_available && \"[DataManager::getIMUCamExtrinsic] you request the value before setting it\\n\");\n return imu_T_cam;\n}\n\nbool DataManager::isIMUCamExtrinsicAvailable() const\n{\n std::lock_guard lk(global_vars_mutex);\n return imu_T_cam_available;\n}\n\nconst ros::Time DataManager::getIMUCamExtrinsicLastUpdated() const\n{\n std::lock_guard lk(global_vars_mutex);\n return imu_T_cam_stamp;\n}\n\n\n\nvoid DataManager::print_datamap_status( string fname ) const\n{\n ofstream myfile;\n myfile.open (fname);\n\n\n myfile << \"#Nodes=\" << data_map->size();\n if( data_map->size() > 0 ) {\n myfile << \"\\tBEGIN=\"<< data_map->begin()->first;\n myfile << \"\\tEND=\"<< data_map->rbegin()->first;\n }\n myfile << endl;\n\n // TODO other global info like imu_T_cam. etc.\n for( auto it = data_map->begin() ; it!= data_map->end() ; it++ )\n {\n string s = \"|\";\n s+= (it->second->isKeyFrame())?TermColor::iGREEN():\"\";\n s+= (it->second->isPoseAvailable())?\"P\":\"~\";\n s+= (it->second->isWholeImageDescriptorAvailable())?\"D\":\"~\";\n\n myfile << s << TermColor::RESET();\n }\n myfile << endl;\n\n myfile.close();\n}\n\n//////////////////////////////////////////////////////////////////////////////\n/////////////////// Kidnap Indicator Publisher ///////////////////////////////\n//////////////////////////////////////////////////////////////////////////////\nbool DataManager::isKidnapIndicatorPubSet() const\n{\n return (bool)is_kidnapn_indicator_set ;\n}\n\nvoid DataManager::setKidnapIndicatorPublishers( ros::Publisher& pub_bool, ros::Publisher& pub_header )\n{\n rcvd_flag_pub = pub_bool;\n kidnap_indicator_header_pub = pub_header;\n is_kidnapn_indicator_set = true;\n}\n\n\nvoid DataManager::PUBLISH__TRUE( const ros::Time _t ) const\n{\n assert( is_kidnapn_indicator_set );\n cout << TermColor::RED() << \"[Cerebro::PUBLISH__TRUE] PUBLISH TRUE (t= \" << _t << \" to indicate the vins_estimator to start again.\\n\" << TermColor::RESET();\n\n std_msgs::Bool bool_msg; bool_msg.data = true;\n rcvd_flag_pub.publish( bool_msg );\n\n // publish header msg\n std_msgs::Header header_msg;\n header_msg.stamp = _t;\n header_msg.frame_id = \"unkidnapped\";\n kidnap_indicator_header_pub.publish( header_msg );\n}\n\nvoid DataManager::PUBLISH__FALSE( const ros::Time _t ) const\n{\n assert( is_kidnapn_indicator_set );\n cout << TermColor::RED() << \"[Cerebro::PUBLISH__FALSE] PUBLISH FALSE (t= \" << _t << \" to indicate the vins_estimator to stop.\\n\" << TermColor::RESET();\n\n // publish False (bool msg)\n std_msgs::Bool bool_msg; bool_msg.data = false;\n rcvd_flag_pub.publish( bool_msg );\n\n // publish header message\n std_msgs::Header header_msg;\n header_msg.stamp = _t;\n header_msg.frame_id = \"kidnapped\";\n kidnap_indicator_header_pub.publish( header_msg );\n}\n\n\n//////////////////////////////////////////////////////////////////////////////\n//////////////////////////////// call backs //////////////////////////////////\n//////////////////////////////////////////////////////////////////////////////\n\n// #define __DATAMANAGER_CALLBACK_PRINT( u ) u;\n#define __DATAMANAGER_CALLBACK_PRINT( u ) ;\n\nvoid DataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr msg )\n{\n if( pose_0_available == false ) { //record the 1st pose\n pose_0 = msg->header.stamp;\n pose_0_available = true;\n }\n\n // __DATAMANAGER_CALLBACK_PRINT( cout << TermColor::BLUE() << \"[cerebro/camera_pose_callback del]\" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )\n __DATAMANAGER_CALLBACK_PRINT( cout << TermColor::BLUE() << \"[cerebro/camera_pose_callback]\" << msg->header.stamp << \"\\t\" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )\n // push this to queue. Another thread will associate the data\n pose_buf.push( msg );\n return;\n\n\n}\n\n// currently not in use.\nvoid DataManager::keyframe_pose_callback( const nav_msgs::Odometry::ConstPtr msg )\n{\n //__DATAMANAGER_CALLBACK_PRINT( cout << \"[cerebro/camera_pose_callback del]\" << msg->header.stamp - pose_0 << endl; )\n __DATAMANAGER_CALLBACK_PRINT( cout << TermColor::iBLUE() << \"[cerebro/keyframe_pose_callback]\" << msg->header.stamp << \"\\t\" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )\n // push this to queue. Another thread will associate the data\n kf_pose_buf.push( msg );\n}\n\n\nvoid DataManager::raw_image_callback( const sensor_msgs::ImageConstPtr& msg )\n{\n // __DATAMANAGER_CALLBACK_PRINT( cout << TermColor::GREEN() << \"[cerebro/raw_image_callback del]\" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )\n __DATAMANAGER_CALLBACK_PRINT( cout << TermColor::GREEN() << \"[cerebro/raw_image_callback]\" << msg->header.stamp << \"\\t\" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )\n\n\n if( this->last_image_time != ros::Time() &&\n (msg->header.stamp.toSec() - this->last_image_time.toSec() > 1.0\n ||\n msg->header.stamp.toSec() < this->last_image_time.toSec())\n )\n {\n cout << TermColor::iBLUE()\n << \"+++++++++++++[DataManager::raw_image_callback] \"\n << \" curr_image.stamp - prev_image.stamp > 1.0.\\n\"\n << \"This means an unstable stream or more usually means a new bag has started with --skip-empty in rosbagplay.\\n\"\n << \"I will publish a FALSE t=\" << 10000 << \" then wait for 500ms and publish TRUE t=\" << 10000\n << \" TODO Complete this implementation and verify correctness.\"\n << TermColor::RESET() << endl;\n\n\n\n cout << TermColor::BLUE() << \"PUBLISH__FALSE\\n\" << TermColor::RESET();\n PUBLISH__FALSE( this->last_image_time );\n\n cout << TermColor::BLUE() << \"Sleep 500ms\\n\" << TermColor::RESET();\n // sleep( 500 ms );\n std::this_thread::sleep_for (std::chrono::milliseconds(500));\n\n cout << TermColor::BLUE() << \"PUBLISH__TRUE\\n\" << TermColor::RESET();\n PUBLISH__TRUE( msg->header.stamp );\n\n\n\n }\n\n\n img_buf.push( msg );\n this->last_image_time = msg->header.stamp;\n return;\n\n}\n\nvoid DataManager::raw_image_callback_1( const sensor_msgs::ImageConstPtr& msg )\n{\n // __DATAMANAGER_CALLBACK_PRINT( cout << \"[cerebro/raw_image_callback_1 del]\" << msg->header.stamp-pose_0 << endl; )\n __DATAMANAGER_CALLBACK_PRINT( cout << \"[cerebro/raw_image_callback_1]\" << msg->header.stamp << \"\\t\" << msg->header.stamp-pose_0 << endl; )\n img_1_buf.push( msg );\n return;\n}\n\n\nvoid DataManager::depth_image_callback( const sensor_msgs::ImageConstPtr& msg )\n{\n __DATAMANAGER_CALLBACK_PRINT(\n cout << \"[cerebro/depth_image_callback]\" << msg->header.stamp << \"\\t\" << msg->header.stamp-pose_0 << \"\\t\";\n cout << \"depth image encoding: \" << msg->encoding << endl;\n )\n depth_im_buf.push( msg );\n return ;\n}\n\n\nvoid DataManager::extrinsic_cam_imu_callback( const nav_msgs::Odometry::ConstPtr msg )\n{\n // __DATAMANAGER_CALLBACK_PRINT( cout << \"[cerebro/extrinsic_cam_imu_callback del]\" << msg->header.stamp-pose_0 << endl; )\n __DATAMANAGER_CALLBACK_PRINT( cout << \"[cerebro/extrinsic_cam_imu_callback]\" << msg->header.stamp << \"\\t\" << msg->header.stamp-pose_0 << endl; )\n extrinsic_cam_imu_buf.push( msg );\n}\n\n\n\n// there will be 5 channels. ch[0]: un, ch[1]: vn, ch[2]: u, ch[3]: v. ch[4]: globalid of the feature.\n // cout << \"\\tpoints.size() : \"<< msg->points.size(); // this will be N (say 92)\n // cout << \"\\tchannels.size() : \"<< msg->channels.size(); //this will be N (say 92)\n // cout << \"\\tchannels[0].size() : \"<< msg->channels[0].values.size(); //this will be 5.\n // cout << \"\\n\";\n // An Example Keypoint msg\n // ---\n // header:\n // seq: 40\n // stamp:\n // secs: 1523613562\n // nsecs: 530859947\n // frame_id: world\n // points:\n // -\n // x: -7.59081602097\n // y: 7.11367511749\n // z: 2.85602664948\n // .\n // .\n // .\n // -\n // x: -2.64935922623\n // y: 0.853760659695\n // z: 0.796766400337\n // channels:\n // -\n // name: ''\n // values: [-0.06108921766281128, 0.02294199913740158, 310.8721618652344, 260.105712890625, 2.0]\n // .\n // .\n // .\n // -\n // name: ''\n // values: [-0.47983112931251526, 0.8081198334693909, 218.95481872558594, 435.47357177734375, 654.0]\n // -\n // name: ''\n // values: [0.07728647440671921, 1.0073764324188232, 344.2176208496094, 473.7791442871094, 660.0]\n // -\n // name: ''\n // values: [-0.6801641583442688, 0.10506453365087509, 159.75746154785156, 279.6077575683594, 663.0]\nvoid DataManager::ptcld_callback( const sensor_msgs::PointCloud::ConstPtr msg )\n{\n // __DATAMANAGER_CALLBACK_PRINT( cout << \"[cerebro/ptcld_callback]\" << msg->header.stamp << endl; )\n __DATAMANAGER_CALLBACK_PRINT( cout << TermColor::YELLOW() << \"[cerebro/ptcld_callback]\" << msg->header.stamp << \"\\t\" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )\n ptcld_buf.push( msg );\n return;\n\n}\n\n\n// currently not in use\nvoid DataManager::tracked_feat_callback( const sensor_msgs::PointCloud::ConstPtr msg )\n{\n __DATAMANAGER_CALLBACK_PRINT( cout << \"[cerebro/tracked_feat_callback]\" << msg->header.stamp << \"\\t\" << msg->header.stamp-pose_0 << endl; )\n trackedfeat_buf.push( msg );\n}\n\n\n\n// #define __json_debuggin(msg) msg;\n#define __json_debuggin(msg) ;\njson DataManager::asJson()\n{\n json obj;\n auto __data_map = this->getDataMapRef();\n IOFormat CSVFormat(StreamPrecision, DontAlignCols, \", \", \",\\t\\n\");\n\n __json_debuggin( cout << \"[DataManager::asJson] Start loop through data_map\\n\"; )\n\n #if 1\n // int max_dist = std::distance( __data_map.begin() , __data_map.end() ); //berks__old\n int max_dist = std::distance( __data_map->begin() , __data_map->end() );\n\n // for( auto it = __data_map.begin() ; it!= __data_map.end() ; it++ ) //berks__old\n for( auto it = __data_map->begin() ; it!= __data_map->end() ; it++ )\n {\n // int seq_id = std::distance( __data_map.begin() , it ); // berks__old\n int seq_id = std::distance( __data_map->begin() , it );\n __json_debuggin(\n cout << \"[DataManager::asJson] \" << seq_id << \" max dist=\" << max_dist << endl;)\n\n json curr_obj;\n curr_obj[\"stamp\"] = it->first.toSec();\n curr_obj[\"stamp_relative\"] = ( it->first - this->getPose0Stamp() ).toSec() ;\n curr_obj[\"seq\"] = seq_id;\n __json_debuggin( cout << \"A\\n\"; )\n\n DataNode * __u = it->second;\n curr_obj[\"getT\"] = __u->getT().toSec();\n #if 0\n curr_obj[\"getT_image\"] = __u->getT_image().toSec();\n #endif\n // curr_obj[\"getT_image_1\"] = __u->getT_image(1).toSec();\n curr_obj[\"getT_pose\"] = __u->getT_pose().toSec();\n curr_obj[\"getT_uv\"] = __u->getT_uv().toSec();\n __json_debuggin( cout << \"B\\n\"; )\n\n curr_obj[\"isKeyFrame\"] = __u->isKeyFrame();\n #if 0\n curr_obj[\"isImageAvailable\"] = __u->isImageAvailable();\n curr_obj[\"isImageAvailable_1\"] = __u->isImageAvailable(1);\n #endif\n curr_obj[\"isPoseAvailable\"] = __u->isPoseAvailable();\n curr_obj[\"isPtCldAvailable\"] = __u->isPtCldAvailable();\n curr_obj[\"isUnVnAvailable\"] = __u->isUnVnAvailable();\n curr_obj[\"isUVAvailable\"] = __u->isUVAvailable();\n curr_obj[\"isFeatIdsAvailable\"] = __u->isFeatIdsAvailable();\n curr_obj[\"getNumberOfSuccessfullyTrackedFeatures\"] = __u->getNumberOfSuccessfullyTrackedFeatures();\n curr_obj[\"isWholeImageDescriptorAvailable\"] = __u->isWholeImageDescriptorAvailable();\n __json_debuggin( cout << \"C\\n\"; )\n\n if( __u->isPoseAvailable() )\n {\n const Matrix4d wTc = __u->getPose();\n json pose_ifo;\n pose_ifo[\"rows\"] = wTc.rows();\n pose_ifo[\"cols\"] = wTc.cols();\n pose_ifo[\"stamp\"] = __u->getT_pose().toSec();\n std::stringstream ss;\n ss << wTc.format(CSVFormat);\n pose_ifo[\"data\"] = ss.str();\n pose_ifo[\"data_pretty\"] = PoseManipUtils::prettyprintMatrix4d(wTc);\n curr_obj[\"w_T_c\"] = pose_ifo;\n }\n __json_debuggin( cout << \"D\\n\"; )\n\n\n\n obj[\"DataNodes\"].push_back( curr_obj );\n }\n #endif\n\n __json_debuggin( cout << \"[DataManager::asJson] End loop through data_map\\n\"; )\n\n\n // Global Data\n __json_debuggin( cout << \"[DataManager::asJson] Logging Global data\\n\"; )\n obj[\"global\"][\"isPose0Available\"] = this->isPose0Available();\n obj[\"global\"][\"Pose0Stamp\"] = this->getPose0Stamp().toSec();\n\n obj[\"global\"][\"isIMUCamExtrinsicAvailable\"] = this->isIMUCamExtrinsicAvailable();\n if( this->isIMUCamExtrinsicAvailable() )\n {\n const Matrix4d __imu_T_cam = this->getIMUCamExtrinsic();\n json pose_ifo;\n pose_ifo[\"rows\"] = __imu_T_cam.rows();\n pose_ifo[\"cols\"] = __imu_T_cam.cols();\n pose_ifo[\"stamp\"] = this->getIMUCamExtrinsicLastUpdated().toSec();\n std::stringstream ss;\n ss << __imu_T_cam.format(CSVFormat);\n pose_ifo[\"data\"] = ss.str();\n pose_ifo[\"data_pretty\"] = PoseManipUtils::prettyprintMatrix4d(__imu_T_cam);\n obj[\"global\"][\"Pose0Stamp\"] = pose_ifo;\n }\n\n\n __json_debuggin( cout << \"[DataManager::asJson] Done asJson()\\n\"; )\n return obj;\n\n}\n\n\n\nstring DataManager::print_queue_size( int verbose=1 ) const\n{\n std::stringstream buffer;\n\n if( verbose == 1)\n {\n buffer << \"img_buf=\" << img_buf.size() << \"\\t\";\n buffer << \"img_1_buf=\" << img_1_buf.size() << \"\\t\";\n buffer << \"depth_im_buf=\" << depth_im_buf.size() << \"\\t\";\n buffer << \"pose_buf=\" << pose_buf.size() << \"\\t\";\n buffer << \"kf_pose_buf=\" << kf_pose_buf.size() << \"\\t\";\n buffer << \"ptcld_buf=\" << ptcld_buf.size() << \"\\t\";\n buffer << \"trackedfeat_buf=\" << trackedfeat_buf.size() << \"\\t\";\n buffer << \"extrinsic_cam_imu_buf=\" << extrinsic_cam_imu_buf.size() << \"\\t\";\n buffer << endl;\n }\n\n if( verbose == 2 )\n {\n buffer << \"img_buf=\" << img_buf.size() << \" (\";\n if( img_buf.size() > 0 ) {\n buffer << std::fixed << std::setprecision(4) << img_buf.front()->header.stamp-pose_0 << \"-->\";\n buffer << std::fixed << std::setprecision(4) << img_buf.back()->header.stamp-pose_0 << \";\";\n }\n buffer << \")\\t\";\n\n buffer << \"img_1_buf=\" << img_1_buf.size() << \" (\";\n if( img_1_buf.size() > 0 ) {\n buffer << std::fixed << std::setprecision(4) << img_1_buf.front()->header.stamp-pose_0 << \"-->\";\n buffer << std::fixed << std::setprecision(4) << img_1_buf.back()->header.stamp-pose_0 << \";\";\n }\n buffer << \")\\t\";\n\n buffer << \"depth_im_buf=\" << depth_im_buf.size() << \" (\";\n if( depth_im_buf.size() > 0 ) {\n buffer << std::fixed << std::setprecision(4) << depth_im_buf.front()->header.stamp-pose_0 << \"-->\";\n buffer << std::fixed << std::setprecision(4) << depth_im_buf.back()->header.stamp-pose_0 << \";\";\n }\n buffer << \")\\t\";\n\n buffer << \"pose_buf=\" << pose_buf.size() << \" (\";\n if( pose_buf.size() > 0 ) {\n buffer << std::fixed << std::setprecision(4) << pose_buf.front()->header.stamp-pose_0 << \"-->\";\n buffer << std::fixed << std::setprecision(4) << pose_buf.back()->header.stamp-pose_0 << \";\";\n }\n buffer << \")\\t\";\n\n buffer << \"kf_pose_buf=\" << kf_pose_buf.size() << \" (\";\n if( kf_pose_buf.size() > 0 ) {\n buffer << std::fixed << std::setprecision(4) << kf_pose_buf.front()->header.stamp-pose_0 << \"-->\";\n buffer << std::fixed << std::setprecision(4) << kf_pose_buf.back()->header.stamp-pose_0 << \";\";\n }\n buffer << \")\\t\";\n\n buffer << \"ptcld_buf=\" << ptcld_buf.size() << \" (\";\n if( ptcld_buf.size() > 0 ) {\n buffer << std::fixed << std::setprecision(4) << ptcld_buf.front()->header.stamp-pose_0 << \"-->\";\n buffer << std::fixed << std::setprecision(4) << ptcld_buf.back()->header.stamp-pose_0 << \";\";\n }\n buffer << \")\\t\";\n\n buffer << \"trackedfeat_buf=\" << trackedfeat_buf.size() << \" (\";\n if( trackedfeat_buf.size() > 0 ) {\n buffer << std::fixed << std::setprecision(4) << trackedfeat_buf.front()->header.stamp-pose_0 << \"-->\";\n buffer << std::fixed << std::setprecision(4) << trackedfeat_buf.back()->header.stamp-pose_0 << \";\";\n }\n buffer << \")\\t\";\n\n buffer << \"extrinsic_cam_imu_buf=\" << extrinsic_cam_imu_buf.size() << \" (\";\n if( extrinsic_cam_imu_buf.size() > 0 ) {\n buffer << std::fixed << std::setprecision(4) << extrinsic_cam_imu_buf.front()->header.stamp-pose_0 << \"-->\";\n buffer << std::fixed << std::setprecision(4) << extrinsic_cam_imu_buf.back()->header.stamp-pose_0 << \";\";\n }\n buffer << \")\\t\";\n buffer << \"\\n\";\n }\n\n if( verbose == 3 )\n {\n buffer << \"img_buf.len=\" << img_buf.size() << \"\\t\";\n buffer << \"pose_buf.len=\" << pose_buf.size() << \"\\t\";\n buffer << \"kf_pose_buf.len=\" << kf_pose_buf.size() << \"\\t\";\n buffer << \"ptcld_buf.len=\" << ptcld_buf.size() << \"\\t\";\n buffer << \"trackedfeat_buf.len=\" << trackedfeat_buf.size() << \"\\t\";\n buffer << \"extrinsic_cam_imu_buf.len=\" << extrinsic_cam_imu_buf.size() << \"\\t\";\n buffer << endl;\n\n if( img_buf.size() > 0 )\n buffer << \"img_buf.back.t=\" << img_buf.back()->header.stamp-pose_0 << \"\\t\";\n if( pose_buf.size() > 0 )\n buffer << \"pose_buf.back.t=\" << pose_buf.back()->header.stamp-pose_0 << \"\\t\";\n if( kf_pose_buf.size() > 0 )\n buffer << \"kf_pose_buf.back.t=\" << kf_pose_buf.back()->header.stamp-pose_0 << \"\\t\";\n if( ptcld_buf.size() > 0 )\n buffer << \"ptcld_buf.back.t=\" << ptcld_buf.back()->header.stamp-pose_0 << \"\\t\";\n if( trackedfeat_buf.size() > 0 )\n buffer << \"trackedfeat_buf.back.t=\" << trackedfeat_buf.back()->header.stamp-pose_0 << \"\\t\";\n if( extrinsic_cam_imu_buf.size() > 0 )\n buffer << \"extrinsic_cam_imu_buf.back.t=\" << extrinsic_cam_imu_buf.back()->header.stamp-pose_0 << \"\\t\";\n buffer << endl;\n\n if( img_buf.size() > 0 )\n buffer << \"img_buf.front.t=\" << img_buf.front()->header.stamp-pose_0 << \"\\t\";\n if( pose_buf.size() > 0 )\n buffer << \"pose_buf.front.t=\" << pose_buf.front()->header.stamp-pose_0 << \"\\t\";\n if( kf_pose_buf.size() > 0 )\n buffer << \"kf_pose_buf.front.t=\" << kf_pose_buf.front()->header.stamp-pose_0 << \"\\t\";\n if( ptcld_buf.size() > 0 )\n buffer << \"ptcld_buf.front.t=\" << ptcld_buf.front()->header.stamp-pose_0 << \"\\t\";\n if( trackedfeat_buf.size() > 0 )\n buffer << \"trackedfeat_buf.front.t=\" << trackedfeat_buf.front()->header.stamp-pose_0 << \"\\t\";\n if( extrinsic_cam_imu_buf.size() > 0 )\n buffer << \"extrinsic_cam_imu_buf.front.t=\" << extrinsic_cam_imu_buf.front()->header.stamp-pose_0 << \"\\t\";\n buffer << endl;\n }\n\n return buffer.str();\n}\n\n\n//////////////////////////// Callback ends /////////////////////////////////////\n\n\n\n\n//////////////////////////////////////////////////////////////////////////////\n/////////////////////////// Thread mains /////////////////////////////////////\n//////////////////////////////////////////////////////////////////////////////\n#define _XOUT_ myfile\nvoid DataManager::trial_thread( )\n{\n cout << TermColor::GREEN() << \"Start DataManager::trial_thread \"<< TermColor::RESET() << endl;\n ros::Rate looprate(10);\n\n\n #if 0\n ofstream myfile;\n myfile.open (fname);\n cout << \"[DataManager::trial_thread] Dump output to file: \" << fname << endl;\n\n while( b_trial_thread )\n {\n _XOUT_ << \"trial_thread\\n\";\n\n // berks__old\n // auto S = data_map.begin(); //.lower_bound( lb );\n // auto E = data_map.end();\n\n auto S = data_map->begin();\n auto E = data_map->end();\n _XOUT_ << \"S=\" << S->first << \" E=\" << E->first << endl;\n int n_green = 0, n_cyan = 0 , n_blue = 0;\n for( auto it = S ; it != E ; it++ )\n {\n #if 1\n if( it->second->isImageAvailable() && it->second->isImageAvailable(1) ) {\n _XOUT_ << TermColor::GREEN();n_green++;\n } else {\n if( it->second->isImageAvailable() ) {\n _XOUT_ << TermColor::CYAN() ;n_cyan++;\n }\n else {\n _XOUT_ << TermColor::BLUE() ;n_blue++;\n }\n }\n\n if( it->second->isPoseAvailable() && it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) {\n _XOUT_ << \"A\" ;\n // cout << it->second->getNumberOfSuccessfullyTrackedFeatures() << \",\";\n }\n if( !it->second->isPoseAvailable() && it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) {\n _XOUT_ << \"B\" ;\n // cout << it->second->getNumberOfSuccessfullyTrackedFeatures() << \",\"; // this means there thise node has no pose and no tracked features data\n }\n if( it->second->isPoseAvailable() && ! (it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) ) {\n _XOUT_ << \"C\" ;\n // cout << it->second->getNumberOfSuccessfullyTrackedFeatures() << \",\";\n }\n if( !it->second->isPoseAvailable() && ! (it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) ){\n _XOUT_ << \"D\"; // has no pose data but has tracked features. Pose actually takes some time to arrive. tracked features will be avaiaable first,\n }\n _XOUT_ << TermColor::RESET() ;\n #endif\n }\n _XOUT_ << endl;\n _XOUT_ << \"n_green=\" << n_green << \" \";\n _XOUT_ << \"n_cyan=\" << n_cyan << \" \";\n _XOUT_ << \"n_blue=\" << n_blue << \" \";\n _XOUT_ << endl;\n\n\n\n looprate.sleep();\n }\n myfile.close();\n #endif\n\n\n while( b_trial_thread )\n {\n img_data_mgr->print_status( \"/dev/pts/23\");\n this->print_datamap_status( \"/dev/pts/24\" );\n looprate.sleep();\n }\n\n cout << TermColor::RED() << \"END DataManager::trial_thread \"<< TermColor::RESET() << endl;\n}\n\n\n// #define ___clean_up_cout(msg) msg;\n#define ___clean_up_cout(msg) ;\nvoid DataManager::clean_up_useless_images_thread()\n{\n //---\n // Settings\n //---\n int keep_last_n_sec_in_ram = 5; //to be safe keep 10. 3-5 usually is fine.\n\n cout << TermColor::GREEN() << \"[DataManager::clean_up_useless_images_thread] Start thread \"<< TermColor::RESET() << endl;\n ros::Rate looprate(0.3);\n // data_association_thread_disable();\n while( b_clean_up_useless_images_thread )\n {\n looprate.sleep();\n\n // if( data_map.begin() == data_map.end() ) { //berks__old\n if( data_map->begin() == data_map->end() ) {\n cout << TermColor::CYAN() << \"[clean_up_useless_images_thread] no nodes\" << TermColor::RESET() << endl;\n continue;\n }\n\n // berks__old\n // auto S = data_map.begin();\n // auto E = data_map.upper_bound( data_map.rbegin()->first - ros::Duration( 3.0 ) );\n\n auto S = data_map->upper_bound( data_map->rbegin()->first - ros::Duration( keep_last_n_sec_in_ram+5 ) );\n // auto S = data_map->begin();\n auto E = data_map->upper_bound( data_map->rbegin()->first - ros::Duration( keep_last_n_sec_in_ram ) );\n int q=0;\n ___clean_up_cout( cout << \"[DataManager::clean_up_useless_images_thread] \" << S->first << \" to \" << E->first << \"\\t\"; )\n ___clean_up_cout( cout << S->first-getPose0Stamp() << \" to \" << E->first - getPose0Stamp() << endl; )\n // for( auto it = data_map.begin() ; it->first < E->first ; it++ ) { //berks__old\n for( auto it = S ; it->first < E->first ; it++ ) {\n #if 0 //old, where images where stored inside datanode.\n if( it->second->isImageAvailable() && !it->second->isKeyFrame() ) {\n // if( it->second->isImageAvailable() ) {\n ___clean_up_cout(\n cout << TermColor::CYAN() << \"deallocate_all_images with t=\" << it->first << \" \" << q++ << TermColor::RESET() << endl;\n it->second->print_image_cvinfo();\n )\n it->second->deallocate_all_images();\n }\n else {\n ___clean_up_cout( cout << TermColor::YELLOW() << \"not deallocate coz t=\" << it->first << \" aka t=\" << it->first - getPose0Stamp() << \"is a keyframe (has pose info)\" << q++ << TermColor::RESET() << endl; )\n }\n #endif\n\n if( it->second->isKeyFrame() ) {\n img_data_mgr->stashImage( \"left_image\", it->first );\n img_data_mgr->stashImage( \"right_image\", it->first );\n img_data_mgr->stashImage( \"depth_image\", it->first );\n } else {\n img_data_mgr->rmImage( \"left_image\", it->first );\n img_data_mgr->rmImage( \"right_image\", it->first );\n img_data_mgr->rmImage( \"depth_image\", it->first );\n }\n }\n }\n cout << TermColor::RED() << \"[DataManager::clean_up_useless_images_thread] Finished Thread\"<< TermColor::RESET() << endl;\n\n}\n\n\n// #define __DataManager__data_association_thread__( msg ) msg;\n#define __DataManager__data_association_thread__( msg ) ;\n\nvoid DataManager::data_association_thread( int max_loop_rate_in_hz )\n{\n assert( max_loop_rate_in_hz > 0 && max_loop_rate_in_hz < 200 && \"[DataManager::data_association_thread] I am expecting the loop rate to be between 1-50\" );\n cout << TermColor::GREEN() << \"[DataManager::data_association_thread] Start thread\" << TermColor::RESET() << endl;\n assert( b_data_association_thread && \"You have not enabled thread execution. Call function DataManager::data_association_thread_enable() before you spawn this thread\\n\");\n float requested_loop_time_ms = 1000. / max_loop_rate_in_hz;\n\n while( b_data_association_thread )\n {\n auto loop_start_at = std::chrono::high_resolution_clock::now();\n\n //------------------------ Process here--------------------------------\n // std::this_thread::sleep_for( std::chrono::milliseconds(2000) );\n __DataManager__data_association_thread__(\n cout << \"---\\n\" ;\n cout << print_queue_size(2 ) ; //< sizes of buffer queues\n cout << \"\\t\\tSize_of_data_map = \"+ to_string( data_map->size() ) + \"\\n\" ;\n )\n\n\n // deqeue all raw images and make DataNodes of each of them, s\n while( img_buf.size() > 0 ) {\n sensor_msgs::ImageConstPtr img_msg = img_buf.front();\n img_buf.pop();\n __DataManager__data_association_thread__(\n cout << TermColor::GREEN() << \">>>>>>>>> Added a new DataNode in data_map with poped() rawimage t=\" << img_msg->header.stamp << \" #####> ie.\" << img_msg->header.stamp - pose_0 << TermColor::RESET() << endl;\n )\n DataNode * n = new DataNode( img_msg->header.stamp );\n #if 0\n n->setImageFromMsg( img_msg );\n #else\n img_data_mgr->setNewImageFromMsg( \"left_image\", img_msg );\n #endif\n\n // data_map.insert( std::make_pair(img_msg->header.stamp, n) ); //berks__old\n data_map->insert( std::make_pair(img_msg->header.stamp, n) );\n }\n\n // dequeue additional raw images\n while( img_1_buf.size() > 0 ) {\n sensor_msgs::ImageConstPtr img_1_msg = img_1_buf.front();\n ros::Time t = img_1_msg->header.stamp;\n\n __DataManager__data_association_thread__(\n cout << \">> Attempt adding poped() img_1_msg in data_map with t=\" << img_1_msg->header.stamp << \" ie. #####> \" << img_1_msg->header.stamp-pose_0 << endl;\n )\n // if( data_map.count(t) > 0 ) { //berks__old\n // data_map.at( t )->setImageFromMsg( img_1_msg, 1 );\n // }\n if( data_map->count(t) > 0 ) {\n #if 0\n data_map->at( t )->setImageFromMsg( img_1_msg, 1 );\n #else\n img_data_mgr->setNewImageFromMsg( \"right_image\", img_1_msg );\n #endif\n\n img_1_buf.pop();\n }\n else {\n ros::Time newest_T = data_map->rbegin()->first;\n __DataManager__data_association_thread__( cout << \"newest_T=\" << newest_T << endl; ) ;\n if( newest_T.toNSec() > t.toNSec() )\n {\n cout << \"newest_T is head of the t, so just drop this image\";\n img_1_buf.pop();\n } else {\n // assert( false && \"[DataManager::data_association_thread] attempting to set additional image into datanode. However that datanode is not found in the map. This cannot be happening\\n\");\n __DataManager__data_association_thread__(\n cout << TermColor::RED() << \"[DataManager::data_association_thread]\";\n cout << \"attempting to set additional image_1 with t=\" << t << \" aka \" << t-pose_0 << \" into datanode. \";\n cout << \"However that datanode is not found in the map. This cannot be happening, but might happen when `image_1` preceeds (even a few milis) than `image`.\";\n cout << TermColor::RESET() << endl ;\n\n cout << TermColor::CYAN() << \"so dont pop from queue.\\n\" << TermColor::RESET();\n )\n break;\n }\n }\n }\n\n\n // depth image\n while( depth_im_buf.size() > 0 ) {\n sensor_msgs::ImageConstPtr depth_image_msg = depth_im_buf.front();\n ros::Time t = depth_image_msg->header.stamp;\n\n __DataManager__data_association_thread__(\n cout << \">> Attempt adding poped() depth_image_msg in data_map with t=\" << depth_image_msg->header.stamp << \" ie. #####> \" << depth_image_msg->header.stamp-pose_0 << endl;\n )\n // if( data_map.count(t) > 0 ) { //berks__old\n // data_map.at( t )->setImageFromMsg( img_1_msg, 1 );\n // }\n if( data_map->count(t) > 0 ) {\n img_data_mgr->setNewImageFromMsg( \"depth_image\", depth_image_msg );\n\n depth_im_buf.pop();\n }\n else {\n ros::Time newest_T = data_map->rbegin()->first;\n __DataManager__data_association_thread__( cout << \"newest_T=\" << newest_T << endl; ) ;\n if( newest_T.toNSec() > t.toNSec() )\n {\n cout << \"newest_T is ahead of the t, so just drop this depth-image\";\n depth_im_buf.pop();\n } else {\n // assert( false && \"[DataManager::data_association_thread] attempting to set additional image into datanode. However that datanode is not found in the map. This cannot be happening\\n\");\n __DataManager__data_association_thread__(\n cout << TermColor::RED() << \"[DataManager::data_association_thread]\";\n cout << \"attempting to set additional depth_image with t=\" << t << \" aka \" << t-pose_0 << \" into datanode. \";\n cout << \"However that datanode is not found in the map. This cannot be happening, but might happen when `depth_image` preceeds (even a few milis) than `image`.\";\n cout << TermColor::RESET() << endl ;\n\n cout << TermColor::CYAN() << \"so dont pop from queue.\\n\" << TermColor::RESET();\n )\n break;\n }\n }\n }\n\n\n // dequeue all poses and set them to data_map\n while( pose_buf.size() > 0 ) {\n nav_msgs::Odometry::ConstPtr pose_msg = pose_buf.front();\n\n ros::Time t = pose_msg->header.stamp;\n __DataManager__data_association_thread__(\n cout << \">> Attempt adding poped() pose in data_map with t=\" << pose_msg->header.stamp << \" ie. #####> \" << pose_msg->header.stamp -pose_0 << endl;\n )\n\n // find the DataNode with this timestamp\n //berks__old\n // if( data_map.count( t ) > 0 ) {\n // // a Node seem to exist with this t.\n // data_map.at( t )->setPoseFromMsg( pose_msg );\n // }\n if( data_map->count( t ) > 0 ) {\n // a Node seem to exist with this t.\n data_map->at( t )->setPoseFromMsg( pose_msg );\n pose_buf.pop();\n }\n else {\n if( t > data_map->rbegin()->first ) {\n __DataManager__data_association_thread__(\n cout << \"\\tpose's t was not yet found in datamap. data_map->rbegin()->first=\" << data_map->rbegin()->first << \" \";\n cout << \"this means a node doesnt exists yet for this pose. Usually this does not happen, but it occurs when Image data manager took too long to insert (thread blocking) and in the meantime more poses got available.\\nI will not pop the queue in this.\\n\";\n );\n break;\n }\n\n // try range search\n // t-delta <= x <= t+delta. x is the map key.\n\n __DataManager__data_association_thread__( cout << \"\\tsince the key (for associating pose with data_map) was not found in data_map do range_search\\n\"; )\n //berks__old\n auto __it = data_map->begin();\n for( __it = data_map->begin() ; __it != data_map->end() ; __it++ ) {\n ros::Duration diff = __it->first-t;\n if( (diff.sec == 0 && abs(diff.nsec) < 1000000) || (diff.sec == -1 && diff.nsec > (1000000000-1000000) ) )\n break;\n }\n\n // berks__old\n if( __it == data_map->end() ) {\n cout << TermColor::RED() << \"\\t`data_association_thread`:pose:(not fouind) range search failed AAA FATAL \\n\";\n assert( false && \"\\tnot fouind\\n\");\n exit(2);\n }\n\n __DataManager__data_association_thread__(\n cout << \"\\tfind pose->t=\" << t << \" in data_map\\n\";\n std::cout << \"\\tinsert at position \" << (__it->first) << '\\n';\n )\n\n if( true )\n {\n // berks__old\n data_map->at( __it->first )->setPoseFromMsg( pose_msg );\n pose_buf.pop();\n }\n else {\n\n cout << TermColor::RED() << \"[DataManager::data_association_thread] data_map does not seem to contain the t of pose_msg. This cannot be happening...fatal quit\" << TermColor::RESET() << endl;\n assert( false && \"[DataManager::data_association_thread] data_map does not seem to contain the t of pose_msg. This cannot be happening\\n\");\n exit(2);\n }\n\n } // else of if (data_map->count(t) > 0\n }\n\n\n // dequeue all point clouds (these are at keyframes)\n while( ptcld_buf.size() > 0 ) {\n sensor_msgs::PointCloudConstPtr ptcld_msg = ptcld_buf.front();\n\n\n ros::Time t = ptcld_msg->header.stamp;\n __DataManager__data_association_thread__(\n cout << \">> Attempt adding poped() pointcloud in data_map at t=\" << t << \" ie. #####> \" << t - pose_0 << endl;\n )\n\n // find the DataNode with this timestamp\n // berks__old\n if( data_map->count( t ) > 0 ) {\n // a Node seem to exist with this t.\n\n //NOte: the ``/vins_estimator/keyframe_point`` and ``/feature_tracker/feature``\n // are in different formats so be careful. vins_estimator/keyframe_point has n channels. while feature_tracker/feature has just 5 channels.\n // but they both convey the same info, so be careful what exact you want.\n #if 0\n data_map.at( t )->setPointCloudFromMsg( ptcld_msg );\n data_map.at( t )->setUnVnFromMsg( ptcld_msg );\n data_map.at( t )->setUVFromMsg( ptcld_msg );\n data_map.at( t )->setTrackedFeatIdsFromMsg( ptcld_msg );\n data_map.at( t )->setAsKeyFrame();\n ptcld_buf.pop();\n #else\n __DataManager__data_association_thread__(\n cout << \"setNumberOfSuccessfullyTrackedFeatures \" << t << \" \" << ptcld_msg->points.size() << endl;\n )\n //berks__old\n data_map->at( t )->setNumberOfSuccessfullyTrackedFeatures( ptcld_msg->points.size() );\n data_map->at( t )->setAsKeyFrame();\n ptcld_buf.pop();\n #endif\n }\n else {\n if( t > data_map->rbegin()->first ) {\n __DataManager__data_association_thread__(\n cout << \"ptcld's t was not yet found in datamap. data_map->rbegin()->first=\" << data_map->rbegin()->first << \" \";\n cout << \"this means a node doesnt exists yet for this ptcld. Usually this does not happen, but it occurs when Image data manager took too long to insert (thread blocking) and in the meantime more poses got available.\\n\\tI will not pop the queue in this.\\n\";\n );\n break;\n }\n\n // try range search\n // t-delta <= x <= t+delta. x is the map key.\n __DataManager__data_association_thread__( cout << \"\\tsince the key was not found (for ptcld) in data_map do range_search\\n\"; )\n\n // berks__old\n auto __it = data_map->begin();\n for( __it = data_map->begin() ; __it != data_map->end() ; __it++ ) {\n ros::Duration diff = __it->first-t;\n if( (diff.sec == 0 && abs(diff.nsec) < 1000000) || (diff.sec == -1 && diff.nsec > (1000000000-1000000) ) )\n break;\n }\n\n // berks__old\n if( __it == data_map->end() ) {\n cout << TermColor::RED() << \"\\trange search failed BBB. if this occurs please report to authors (Manohar Kuse)\\n\";\n assert( false && \"\\tnot fouind\\n\");\n exit(2);\n }\n\n __DataManager__data_association_thread__(\n cout << \"\\tfind ptcld->t=\" << t << \" in data_map\\n\";\n std::cout << \"\\tinsert ptcld at position \" << (__it->first) << '\\n';\n )\n\n\n if( true ) {\n #if 0\n data_map.at( __it->first )->setPointCloudFromMsg( ptcld_msg );\n data_map.at( __it->first )->setUnVnFromMsg( ptcld_msg );\n data_map.at( __it->first )->setUVFromMsg( ptcld_msg );\n data_map.at( __it->first )->setTrackedFeatIdsFromMsg( ptcld_msg );\n data_map.at( __it->first )->setAsKeyFrame();\n ptcld_buf.pop();\n #else\n //berks__old\n data_map->at( __it->first )->setNumberOfSuccessfullyTrackedFeatures( ptcld_msg->points.size() );\n data_map->at( __it->first )->setAsKeyFrame();\n ptcld_buf.pop();\n #endif\n }\n else {\n __DataManager__data_association_thread__(cout << TermColor::RED() << \"[DataManager::data_association_thread] data_map does not seem to contain the t of ptcld_msg. This cannot be happening.\" << TermColor::RESET() << endl ;)\n assert( false && \"[DataManager::data_association_thread] data_map does not seem to contain the t of ptcld_msg. This cannot be happening\\n\");\n exit(2);\n }\n }\n }\n\n\n // Deal with cam_imu_extrinsic. Store the last in global variable of this class.\n bool flag = false;\n nav_msgs::OdometryConstPtr __msg;\n while( extrinsic_cam_imu_buf.size() > 0 ) {\n // dump all\n __msg = extrinsic_cam_imu_buf.front();\n extrinsic_cam_imu_buf.pop();\n flag = true;\n }\n if( flag ) {\n std::lock_guard lk(global_vars_mutex);\n PoseManipUtils::geometry_msgs_Pose_to_eigenmat( __msg->pose.pose, imu_T_cam );\n imu_T_cam_available = true;\n imu_T_cam_stamp = __msg->header.stamp;\n }\n\n\n\n\n\n //--------------------------- Done processing--------------------------\n auto loop_end_at = std::chrono::high_resolution_clock::now();\n std::chrono::duration ellapsed = loop_end_at-loop_start_at;\n\n // sleep_for( requested_loop_time - ellapsed )\n int sleep_for = int(requested_loop_time_ms - (float) ellapsed.count()) ;\n __DataManager__data_association_thread__( cout << \"Loop iteration done in \"<< ellapsed.count() << \" ms; sleep_for=\" << sleep_for << \" ms\" << endl; )\n\n if( sleep_for > 0 )\n std::this_thread::sleep_for( std::chrono::milliseconds( sleep_for ) );\n else {\n __DataManager__data_association_thread__( cout << \"Queueing in thread `data_association_thread`\\n\" ; )\n ROS_WARN( \"Queueing in thread `data_association_thread`. requested_loop_time_ms=%f; elapsed=%f. If this occurs occasionally, it might be because of image_manager thread getting blocked by cleanup thread. It is perfectly normal.\", requested_loop_time_ms, ellapsed.count() );\n }\n\n }\n\n cout << TermColor::RED() << \"[DataManager::data_association_thread] Finished thread\" << TermColor::RESET() << endl;\n\n}\n\n\n\n\n// will have DataNodes and ImageDataManager to disk for reloading later.\n// Camera related info is not to be stored. Just assume next time the camera relatives are the same as the map\nbool DataManager::saveStateToDisk( const string save_folder_name )\n{\n cout << TermColor::GREEN();\n cout << \"\\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\\n\";\n cout << \"^^^^^^^^^^ DataManager::saveStateToDisk ^^^^^^^^^^\\n\";\n cout << \"^^^^^^^^^^ DIR=\" << save_folder_name ;\n cout << \"\\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\\n\";\n cout << TermColor::RESET();\n\n // TODO: rm -rf save_folder_name ; mkdir save_folder_name\n string system_cmd0 = string( \"rm -rf \") + save_folder_name + \" && mkdir \"+ save_folder_name;\n const int rm_dir_err0 = RawFileIO::exec_cmd( system_cmd0 );\n if ( rm_dir_err0 == -1 )\n {\n cout << TermColor::RED() << \"[DataManager::saveStateToDiskr] Cannot mkdir folder: \" << save_folder_name << \"!\\n\" << TermColor::RESET() << endl;\n cout << \"So not saveing state to disk...return false\\n\";\n return false;\n }\n\n //\n // ---- Save std::map\n json x_datamap;\n auto __data_map = this->getDataMapRef();\n IOFormat CSVFormat(FullPrecision, DontAlignCols, \", \", \"\\n\");\n\n cout << \"data_map to json\\n\";\n int n_isPoseAvailable = 0;\n int n_isWholeImageDescriptorAvailable = 0;\n int n_keyframes = 0;\n for( auto it = __data_map->begin() ; it!= __data_map->end() ; it++ )\n {\n int seq_id = std::distance( __data_map->begin() , it );\n // cout << \"seq_id=\" << seq_id << endl;\n\n json curr_obj;\n // curr_obj[\"stamp\"] = it->first.toSec();\n curr_obj[\"stampNSec\"] = it->first.toNSec();\n curr_obj[\"stamp_relative\"] = ( it->first - this->getPose0Stamp() ).toSec() ;\n curr_obj[\"seq\"] = seq_id;\n\n\n DataNode * __u = it->second;\n if( __u->isPoseAvailable() )\n {\n const Matrix4d wTc = __u->getPose();\n json pose_ifo;\n pose_ifo[\"rows\"] = wTc.rows();\n pose_ifo[\"cols\"] = wTc.cols();\n // pose_ifo[\"stamp\"] = __u->getT_pose().toSec();\n pose_ifo[\"stampNSec\"] = __u->getT_pose().toNSec();\n std::stringstream ss;\n ss << wTc.format(CSVFormat);\n pose_ifo[\"data\"] = ss.str();\n pose_ifo[\"data_pretty\"] = PoseManipUtils::prettyprintMatrix4d(wTc);\n curr_obj[\"w_T_c\"] = pose_ifo;\n n_isPoseAvailable++;\n }\n\n\n if( __u->isWholeImageDescriptorAvailable() )\n {\n const VectorXd wh_img_desc = __u->getWholeImageDescriptor();\n json desc_ifo;\n desc_ifo[\"rows\"] = wh_img_desc.rows();\n desc_ifo[\"cols\"] = wh_img_desc.cols();\n std::stringstream ss;\n ss << wh_img_desc.format(CSVFormat);\n desc_ifo[\"data\"] = ss.str();\n curr_obj[\"wholeImageDescriptor\"] = desc_ifo;\n n_isWholeImageDescriptorAvailable++;\n }\n\n if( __u->isKeyFrame() )\n n_keyframes++;\n\n curr_obj[\"isKeyFrame\"] = __u->isKeyFrame();\n curr_obj[\"getNumberOfSuccessfullyTrackedFeatures\"] = __u->getNumberOfSuccessfullyTrackedFeatures();\n curr_obj[\"isWholeImageDescriptorAvailable\"] = __u->isWholeImageDescriptorAvailable();\n curr_obj[\"isPoseAvailable\"] = __u->isPoseAvailable();\n\n x_datamap[\"DataNodes\"].push_back( curr_obj );\n\n }\n cout << \"\\t n_isPoseAvailable=\" << n_isPoseAvailable << endl;\n cout << \"\\t n_isWholeImageDescriptorAvailable=\" << n_isWholeImageDescriptorAvailable << endl;\n cout << \"\\t n_keyframes=\" << n_keyframes << endl;\n cout << \"DONE, data_map to json\\n\";\n\n //\n // --- Misc Variables in class DataManager\n\n //\n // --- save img_data_mgr\n // - go over all status and stash all the images that remain on RAM.\n // - save ImageDataManager::status to json\n cout << \"img_data_mgr->stashAll()\\n\";\n json img_mgr_json = img_data_mgr->stashAll();\n cout << \"DONE, img_data_mgr->stashAll()\\n\";\n x_datamap[\"ImageDataManager\"] = img_mgr_json;\n\n // mv stash dir to `save_folder_name`\n string system_cmd = string(\"mv \") + img_data_mgr->getStashDir() + \" \" + save_folder_name;\n const int rm_dir_err = RawFileIO::exec_cmd( system_cmd );\n if ( rm_dir_err == -1 )\n {\n cout << TermColor::RED() << \"[DataManager::saveStateToDiskr] Error moving stash directory!\\n\" << TermColor::RESET() << endl;\n }\n img_data_mgr->set_rm_stash_dir_in_destructor_as_false();\n\n\n //\n // --- Save JSON\n cout << \"x_datamap[\\\"DataNodes\\\"].size()=\" << x_datamap[\"DataNodes\"].size() << endl;\n cout << \"x_datamap[\\\"ImageDataManager\\\"].size()=\" << x_datamap[\"ImageDataManager\"].size() << endl;\n RawFileIO::write_string( save_folder_name+\"/state.json\", x_datamap.dump(4) );\n cout << \"DONE........ DataManager::saveStateToDisk ^^^^^^^^^^\\n\";\n return true;\n}\n\n\nbool DataManager::loadStateFromDisk( const string save_folder_name )\n{\n cout << TermColor::GREEN();\n cout << \"\\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\\n\";\n cout << \"^^^^^^^^^^ DataManager::loadStateFromDisk ^^^^^^^^^^\\n\";\n cout << \"^^^^^^^^^^ DIR=\" << save_folder_name ;\n cout << \"\\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\\n\";\n cout << TermColor::RESET();\n\n //---\n // Load JSON\n string json_fname = save_folder_name+\"/state.json\";\n cout << TermColor::GREEN() << \"[DataManager::loadStateFromDisk]Open file: \" << json_fname << TermColor::RESET() << endl;\n std::ifstream json_fileptr(json_fname);\n if( !json_fileptr )\n {\n ROS_ERROR( \"[DataManager::loadStateFromDisk]Cannot load from previous state\" );\n cout << TermColor::RED() << \"[DataManager::loadStateFromDisk]Fail to open state.json file, perhaps it doesnt exist. Cannot load from previous state.\\nEXIT(1)\"<< endl;\n exit(1);\n }\n json json_obj;\n json_fileptr >> json_obj;\n cout << \"[DataManager::loadStateFromDisk]Successfully opened file and loaded data \"<< json_fname << endl;\n json_fileptr.close();\n\n //---\n // Setup DataNodes\n int n_datanodes = json_obj[\"DataNodes\"].size();\n int n_isPoseAvailable = 0;\n int n_isWholeImageDescriptorAvailable = 0;\n int n_keyframes = 0;\n cout << \"[DataManager::loadStateFromDisk]There appear to be \" << n_datanodes << \" datanodes\\n\";\n for( int i=0 ; isetAsKeyFrame();\n }\n\n // number of tracked features\n if( numberOfSuccessfullyTrackedFeatures >= 0 )\n n->setNumberOfSuccessfullyTrackedFeatures( numberOfSuccessfullyTrackedFeatures );\n\n\n // pose\n if( isPoseAvailable )\n {\n // setPose()\n Matrix4d __wtc;\n // cout << \"+++\\n\" << json_obj[\"DataNodes\"][i][\"w_T_c\"] << \"\\n+++\" << endl;\n bool status = RawFileIO::read_eigen_matrix4d_fromjson( json_obj[\"DataNodes\"][i][\"w_T_c\"], __wtc );\n if( status == false ) {\n cout << \"[DataManager::loadStateFromDisk] RawFileIO::read_eigen_matrix4d_fromjson returned false, this means I cannot convert from json to Matrix4d, perhaps something wrong with json file\\n\";\n exit(1);\n }\n // cout << \"__wtc=\\n\" << __wtc << endl;\n // cout << PoseManipUtils::prettyprintMatrix4d( __wtc );\n // exit(1);\n\n // double t_pose_sec = json_obj[\"DataNodes\"][i][\"w_T_c\"][\"stamp\"];\n int64_t t_pose_sec = json_obj[\"DataNodes\"][i][\"w_T_c\"][\"stampNSec\"];\n\n ros::Time stamp_pose = ros::Time().fromNSec( t_pose_sec );\n n->setPose( stamp_pose,__wtc );\n\n n_isPoseAvailable++;\n }\n\n\n // descriptor\n if( isWholeImageDescriptorAvailable )\n {\n // setWholeImageDescriptor()\n VectorXd __wholeImageDescriptor;\n // cout << \"+++\\n\" << json_obj[\"DataNodes\"][i][\"wholeImageDescriptor\"] << \"\\n+++\" << endl;\n bool status = RawFileIO::read_eigen_vector_fromjson( json_obj[\"DataNodes\"][i][\"wholeImageDescriptor\"], __wholeImageDescriptor );\n if( status == false ) {\n cout << \"[DataManager::loadStateFromDisk] RawFileIO::read_eigen_vector_fromjson returned false, this means I cannot convert from json to Matrix4d, perhaps something wrong with json file\\n\";\n exit(1);\n }\n\n // cout << \"__wholeImageDescriptor=\\n\" << __wholeImageDescriptor.transpose() << endl;\n // exit(1);\n\n n->setWholeImageDescriptor( __wholeImageDescriptor );\n n_isWholeImageDescriptorAvailable++;\n }\n\n\n data_map->insert( std::make_pair(stamp, n) );\n }\n cout << \"\\t n_isPoseAvailable=\" << n_isPoseAvailable << endl;\n cout << \"\\t n_isWholeImageDescriptorAvailable=\" << n_isWholeImageDescriptorAvailable << endl;\n cout << \"\\t n_keyframes=\" << n_keyframes << endl;\n\n\n //---\n // Misc Variables in class DataManager\n\n\n //---\n // Setup ImageDataManager\n // mv folder to /tmp/...\n string system_cmd = string(\"cp -r \")+save_folder_name+\"/cerebro_stash \"+\" /tmp\";\n // string system_cmd = string(\"mv \")+save_folder_name+\"/cerebro_stash \"+\" /tmp\";\n cout << TermColor::YELLOW() << \"[DataManager::loadStateFromDisk] \" << system_cmd << TermColor::RESET() << endl;\n const int mv_dir_err = RawFileIO::exec_cmd( system_cmd );\n if ( mv_dir_err == -1 )\n {\n cout << TermColor::RED() << \"[DataManager::loadStateFromDisk] Error moving directory!\\n\" << TermColor::RESET() << endl;\n exit(1);\n }\n\n img_data_mgr->initStashDir(false);\n img_data_mgr->loadStateFromDisk( json_obj[\"ImageDataManager\"] );\n\n\n\n\n}\n" }, { "path": "src/DataManager.h", "content": "#pragma once\n\n/** DataManager - Inmemory database of data from VINS-estimator\n\n This attempts to store all the data from VINS-estimator\n in a in-memory database. Note that this has multiple threads.\n\n The data will be maintained as a std::map with key as timestamp,\n value as custom class DataNode. Other global data will also be kept.\n Callbacks are also handled here.\n\n Author : Manohar Kuse \n Created : 3rd Oct, 2017\n Major Update : Jun, 2018\n Major Update : Oct, 2018\n\n**/\n\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n\n#include \n#include \n#include \n#include \n#include //needed for std::shared_ptr\n//opencv\n#include \n#include \n#include \n#include \n\n// ros\n#include \n#include \n\n// ros msg\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n// #include \n#include \n#include \n#include \n#include \n\n\n// Eigen\n#include \n#include \n#include \nusing namespace Eigen;\n#include \n\n#include \"PinholeCamera.h\"\n#include \"camodocal/camera_models/Camera.h\" // this is in include/camodocal. src files in src/utils/camodocal_src\n#include \"camodocal/camera_models/CameraFactory.h\"\n\n#include \"DataNode.h\"\n#include \"ImageDataManager.h\"\n\n#include \"utils/nlohmann/json.hpp\"\nusing json = nlohmann::json;\n\n\n#include \"utils/PoseManipUtils.h\"\n#include \"utils/RawFileIO.h\"\n#include \"utils/SafeQueue.h\"\n#include \"utils/ElapsedTime.h\"\n\ntypedef std::map< ros::Time, DataNode* > t__DataNode;\n\nclass DataManager\n{\npublic:\n DataManager( ros::NodeHandle &nh );\n DataManager(const DataManager &obj);\n\n // void setCamera( const PinholeCamera& camera );\n // PinholeCamera& getCameraRef() { return camera;}\n\n void setAbstractCamera( camodocal::CameraPtr abs_camera, short cam_id=0 );\n camodocal::CameraPtr getAbstractCameraRef(short cam_id=0) const;\n bool isAbstractCameraSet(short cam_id=0) const;\n vector getAbstractCameraKeys() const;\n\n void setCameraRelPose( Matrix4d a_T_b, std::pair pair_a_b );\n bool isCameraRelPoseSet( std::pair pair_a_b ) const;\n const Matrix4d& getCameraRelPose( std::pair pair_a_b ) const;\n vector< std::pair > getCameraRelPoseKeys();\n\n // std::map< ros::Time, DataNode* >& getDataMapRef() { return data_map; }\n std::shared_ptr< t__DataNode > getDataMapRef() { return data_map; }\n\n std::shared_ptr< ImageDataManager > getImageManagerRef() { return img_data_mgr; }\n\n const ros::Time getPose0Stamp() const { return pose_0; }\n bool isPose0Available() const { return pose_0_available; }\n\n const Matrix4d& getIMUCamExtrinsic() const;\n bool isIMUCamExtrinsicAvailable() const;\n const ros::Time getIMUCamExtrinsicLastUpdated() const;\n\n\n // generally set fname something like /dev/pts/20, output to a separate terminal.\n // you can know a terminal's device name with the command `tty`\n void print_datamap_status( string fname ) const;\n\npublic:\n ////////\n /////// Kidnap Indicator Publisher\n ///////\n\n // publish true and publish false.\n // The timestamps indicate the start of kidnap and end of kidnap respectively.\n // each of the function will publish messages on '/feature_tracker/rcvd_flag'\n // and on '/feature_tracker/rcvd_flag_header'\n\n bool isKidnapIndicatorPubSet() const;\n void setKidnapIndicatorPublishers( ros::Publisher& pub_bool, ros::Publisher& pub_header );\n void PUBLISH__TRUE( const ros::Time _t ) const;\n void PUBLISH__FALSE( const ros::Time _t ) const;\n\n\nprivate:\n ros::Publisher rcvd_flag_pub;\n ros::Publisher kidnap_indicator_header_pub;\n bool is_kidnapn_indicator_set = false;\n\n\npublic:\n ////////\n //////// Write Data\n ////////\n // returns string as a json. contains everything including wTc, wX, uv, K, D etc.\n json asJson() ;\n\n bool saveStateToDisk( const string save_folder_name );\n bool loadStateFromDisk( const string save_folder_name );\n\n\nprivate:\n /////////\n ///////// Global Variables\n /////////\n std::map< int, camodocal::CameraPtr > all_abstract_cameras;\n std::map< std::pair, Matrix4d > cam_relative_poses; //< pair:a,b then a_T_b\n\n\n ros::NodeHandle nh; //< Node Handle, TODO Not sure why this will be needed here. consider removing it from here.\n // const std::ofstream &out_stream;\n\n // std::map< ros::Time, DataNode* > data_map; //original\n std::shared_ptr< t__DataNode > data_map = std::make_shared();\n\n std::shared_ptr< ImageDataManager> img_data_mgr = std::make_shared();\n\n bool pose_0_available = false;\n ros::Time pose_0; // time of 1st pose\n\n Matrix4d imu_T_cam = Matrix4d::Identity();\n bool imu_T_cam_available = false;\n ros::Time imu_T_cam_stamp;\n\n mutable std::mutex global_vars_mutex;\n\n\npublic:\n /////////\n ///////// Callbacks\n /////////\n void camera_pose_callback( const nav_msgs::Odometry::ConstPtr msg ); ///< w_T_c. pose of camera in the world-cordinate system. All the cameras. only a subset of this will be keyframes\n void keyframe_pose_callback( const nav_msgs::Odometry::ConstPtr msg ); //X /// pose of imu at keyframes. Use it just as a marker, dont use the poses.\n\n\n void raw_image_callback( const sensor_msgs::ImageConstPtr& msg );\n void raw_image_callback_1( const sensor_msgs::ImageConstPtr& msg );\n void depth_image_callback( const sensor_msgs::ImageConstPtr& msg );\n\n void extrinsic_cam_imu_callback( const nav_msgs::Odometry::ConstPtr msg );\n void ptcld_callback( const sensor_msgs::PointCloud::ConstPtr msg );\n void tracked_feat_callback( const sensor_msgs::PointCloud::ConstPtr msg ); //X\n\n\nprivate:\n // double last_image_time=-1;\n ros::Time last_image_time = ros::Time();\n\n // callback-buffers\n std::queue img_buf;\n std::queue img_1_buf;\n std::queue depth_im_buf;\n std::queue pose_buf;\n std::queue kf_pose_buf;\n std::queue ptcld_buf;\n std::queue trackedfeat_buf;\n std::queue extrinsic_cam_imu_buf;\n\n string print_queue_size(int verbose ) const;\n\n\n /////////\n ///////// Threads\n /////////\npublic:\n // This threads monitors the buffer queues and sync them all in a std::map which is indexed with time and holds all the data\n void data_association_thread( int max_loop_rate_in_hz );\n void data_association_thread_enable() { b_data_association_thread = true; }\n void data_association_thread_disable() { b_data_association_thread = false; }\n\n\nprivate:\n atomic b_data_association_thread;\n\n\npublic:\n // Just a trial thread\n void trial_thread();\n void trial_thread_enable() { b_trial_thread = true; }\n void trial_thread_disable() { b_trial_thread = false; }\n\nprivate:\n atomic b_trial_thread;\n\n\npublic:\n // Thread to deallocate images which have no pose info (aka useless images)\n void clean_up_useless_images_thread();\n void clean_up_useless_images_thread_enable() { b_clean_up_useless_images_thread = true; }\n void clean_up_useless_images_thread_disable() { b_clean_up_useless_images_thread = false; }\n\nprivate:\n atomic b_clean_up_useless_images_thread;\n\n\n};\n" }, { "path": "src/DataNode.cpp", "content": "#include \"DataNode.h\"\n\n#if 0\nvoid DataNode::setImageFromMsg( const sensor_msgs::ImageConstPtr msg )\n{\n std::lock_guard lk(m);\n\n auto tmp = cv_bridge::toCvShare(msg, \"bgr8\" )->image;\n\n // this->image = cv_bridge::toCvShare(msg, \"bgr8\" )->image;\n // this->image = (cv_bridge::toCvCopy(msg, \"bgr8\" )->image).clone();\n // this->image = (cv_bridge::toCvShare(msg, \"bgr8\" )->image).clone();\n\n this->image = tmp.clone();\n tmp.release();\n\n\n //\n // Note: (on Opencv and cv_bridge, memory)\n // I was observing a massive memory leak when not using clone().\n // See [this link](https://answers.opencv.org/question/14285/how-to-free-memory-through-cvmat/)\n // So, the point is, if using cv::Mat, it is important to understand who owns the memory.\n // cv::Mat::clone() does a deep copy. it is important to .clone here for\n // the deallocation to work correctly.\n\n\n assert( image.rows > 0 && image.cols > 0 && !image.empty() && \"In DataNode::setImageFromMsg image that is being set from sensor_msgs::Image is invalid.\");\n\n\n this->t_image = msg->header.stamp;\n this->m_image = true;\n}\n\nvoid DataNode::setImageFromMsg( const sensor_msgs::ImageConstPtr msg, short cam_id )\n{\n std::lock_guard lk(m);\n\n // cv::Mat __image = cv_bridge::toCvShare(msg, \"bgr8\" )->image;\n cv::Mat __image = (cv_bridge::toCvCopy(msg, \"bgr8\" )->image).clone();\n this->all_images[cam_id] = __image;\n\n //\n // Note: (on Opencv and cv_bridge, memory)\n // I was observing a massive memory leak when not using clone().\n // See [this link](https://answers.opencv.org/question/14285/how-to-free-memory-through-cvmat/)\n // So, the point is, if using cv::Mat, it is important to understand who owns the memory.\n // cv::Mat::clone() does a deep copy. it is important to .clone here for\n // the deallocation to work correctly.\n\n assert( this->all_images.at(cam_id).rows > 0 && this->all_images.at(cam_id).cols > 0 && ! this->all_images.at(cam_id).empty() && \"In DataNode::setImageFromMsg with cam_id image that is being set from sensor_msgs::Image is invalid.\");\n\n\n this->t_all_images[cam_id] = msg->header.stamp;\n\n __image.release();\n\n}\n\nvoid DataNode::print_image_cvinfo()\n{\n std::lock_guard lk(m);\n cout << \"[DataNode::print_image_cvinfo]\\n\";\n\n if( m_image ) {\n cout << \"[DataNode::print_image_cvinfo] main image: \" << MiscUtils::cvmat_info( image ) << endl;\n cout << TermColor::iBLUE() << \"[DataNode::print_image_cvinfo] refcount=\" << image.u->refcount << TermColor::RESET() << endl;\n }\n\n for( auto it=all_images.begin() ; it!=all_images.end() ; it++ ) {\n cout << TermColor::iBLUE() << \"[DataNode::print_image_cvinfo] image#\" << it->first << \" refcount=\" << it->second.u->refcount << TermColor::RESET() << endl;\n }\n\n cout << \"[DataNode::print_image_cvinfo] Done\\n\";\n}\n\n// #define __DATANODE__deallocate_all_images( msg ) msg;\n#define __DATANODE__deallocate_all_images( msg ) ;\nvoid DataNode::deallocate_all_images()\n{\n __DATANODE__deallocate_all_images(\n cout << \"[DataNode::deallocate_all_images] getT= \" << getT() << \" getT_image()=\"<< getT_image() << \"\\n\";\n )\n std::lock_guard lk(m);\n\n // deallocate main image\n if( m_image ) {\n image.release();\n image.deallocate();\n t_image = ros::Time();\n m_image = false;\n }\n // cout << \"[DataNode::deallocate_all_images] After Deallocate image.u->refcount=\" << image.u->refcount << endl;\n\n // deallocate other images with cam_id\n for( auto it=all_images.begin() ; it!=all_images.end() ; it++ ) {\n it->second.release();\n it->second.deallocate();\n // cout << \"[DataNode::deallocate_all_images] image# \" << it->first << \", After Deallocate it->second.u->refcount=\" << image.u->refcount << endl;\n }\n all_images.clear();\n t_all_images.clear();\n}\n#endif\n\nvoid DataNode::setPoseFromMsg( const nav_msgs::OdometryConstPtr msg )\n{\n std::lock_guard lk(m);\n\n PoseManipUtils::geometry_msgs_Pose_to_eigenmat( msg->pose.pose, wTc );\n\n // vector _q = msg->pose.covariance ;\n wTc_covariance = MatrixXd::Zero(6,6);\n for( int i=0 ; i<36 ; i++ )\n wTc_covariance(int(i/6), i%6) = msg->pose.covariance[i];\n\n t_wTc = msg->header.stamp;\n m_wTc = true;\n}\n\n\nvoid DataNode::setPose( const Matrix4d __wTc )\n{\n std::lock_guard lk(m);\n\n wTc_covariance = MatrixXd::Zero(6,6);\n wTc = Matrix4d( __wTc );\n\n m_wTc = true;\n}\n\nvoid DataNode::setPose( const ros::Time __t, const Matrix4d __wTc )\n{\n std::lock_guard lk(m);\n\n wTc_covariance = MatrixXd::Zero(6,6);\n wTc = Matrix4d( __wTc );\n\n t_wTc = __t;\n m_wTc = true;\n}\n\n\nvoid DataNode::setPointCloudFromMsg( const sensor_msgs::PointCloudConstPtr msg )\n{\n std::lock_guard lk(m);\n\n int n_pts = msg->points.size() ;\n if( n_pts == 0 ) {\n // cout << TermColor::RED() << \"[DataNode::setPointCloudFromMsg]npts=0\" << TermColor::RESET() << endl;\n t_ptcld = msg->header.stamp;\n m_ptcld = true;\n m_ptcld_zero_pts = true;\n return ;\n }\n assert( n_pts > 0 && \"[DataNode::setPointCloudFromMsg] The input pointcloud msg contains zero 3d points, ie. msg->points\\n\");\n\n ptcld = MatrixXd::Zero( 4, n_pts );\n for( int i=0 ; ipoints[i].x;\n ptcld( 1, i ) = msg->points[i].y;\n ptcld( 2, i ) = msg->points[i].z;\n ptcld( 3, i ) = 1.0;\n }\n\n t_ptcld = msg->header.stamp;\n m_ptcld = true;\n}\n\n\nvoid DataNode::setUnVnFromMsg( const sensor_msgs::PointCloudConstPtr msg )\n{\n std::lock_guard lk(m);\n\n int n_pts = msg->channels.size() ;\n if( n_pts == 0 ) {\n // cout << TermColor::RED() << \"[DataNode::setUnVnFromMsg]npts=0\" << TermColor::RESET() << endl;\n t_unvn = msg->header.stamp;\n m_unvn = true;\n m_unvn_zero_pts = true;\n return ;\n }\n assert( n_pts > 0 && \"[DataNode::setUnVnFromMsg] The input pointcloud msg contains zero unvn points, ie. msg->channels\\n\");\n\n unvn = MatrixXd::Zero( 3, n_pts );\n for( int i=0 ; ichannels[i].values[0];\n unvn( 1, i ) = msg->channels[i].values[1];\n unvn( 2, i ) = 1.0;\n }\n\n t_unvn = msg->header.stamp;\n m_unvn = true;\n}\n\n\nvoid DataNode::setUVFromMsg( const sensor_msgs::PointCloudConstPtr msg )\n{\n std::lock_guard lk(m);\n\n int n_pts = msg->channels.size() ;\n if( n_pts == 0 ) {\n // cout << TermColor::RED() << \"[DataNode::setUVFromMsg]npts=0\" << TermColor::RESET() << endl;\n t_uv = msg->header.stamp;\n m_uv = true;\n m_uv_zero_pts = true;\n return ;\n }\n assert( n_pts > 0 && \"[DataNode::setUVFromMsg] The input pointcloud msg contains zero uv points, ie. msg->channels\\n\");\n\n uv = MatrixXd::Zero( 3, n_pts );\n for( int i=0 ; ichannels[i].values[2];\n uv( 1, i ) = msg->channels[i].values[3];\n uv( 2, i ) = 1.0;\n }\n\n t_uv = msg->header.stamp;\n m_uv = true;\n}\n\nvoid DataNode::setTrackedFeatIdsFromMsg( const sensor_msgs::PointCloudConstPtr msg )\n{\n std::lock_guard lk(m);\n\n int n_pts = msg->channels.size() ;\n if( n_pts == 0 ) {\n // cout << TermColor::RED() << \"[DataNode::setTrackedFeatIdsFromMsg]npts=0\" << TermColor::RESET() << endl;\n t_tracked_feat_ids = msg->header.stamp;\n m_tracked_feat_ids = true;\n m_tracked_feat_ids_zero_pts = true;\n return ;\n }\n assert( n_pts > 0 && \"[DataNode::setTrackedFeatIdsFromMsg] The input pointcloud msg contains zero tracked feature point ids, ie. msg->channels\\n\");\n\n tracked_feat_ids = VectorXi::Zero( n_pts );\n for( int i=0 ; ichannels[i].values[4] );\n }\n\n t_tracked_feat_ids = msg->header.stamp;\n m_tracked_feat_ids = true;\n}\n\nvoid DataNode::setNumberOfSuccessfullyTrackedFeatures( int n )\n{\n std::lock_guard lk(m);\n assert( n >= 0 && \"[DataNode::setNumberOfSuccessfullyTrackedFeatures] you cannot set a negative value\");\n numberOfSuccessfullyTrackedFeatures = n;\n}\n\nint DataNode::getNumberOfSuccessfullyTrackedFeatures() const\n{\n std::lock_guard lk(m);\n return numberOfSuccessfullyTrackedFeatures;\n}\n\n\n#if 0\nconst cv::Mat& DataNode::getImage() const {\n std::lock_guard lk(m);\n assert( isImageAvailable() && \"[DataNode::getImage] you requested the image before setting it\");\n return this->image;\n}\n\n\nconst cv::Mat& DataNode::getImage(short cam_id) const {\n std::lock_guard lk(m);\n assert( isImageAvailable(cam_id) && \"[DataNode::getImage with cam_id] you requested the image before setting it\");\n // return this->all_images[cam_id];\n return this->all_images.at(cam_id);\n}\n#endif\n\nconst Matrix4d& DataNode::getPose() const{\n std::lock_guard lk(m);\n assert( m_wTc && \"[DataNode::getPose] you requested the pose before setting it\");\n #if 0\n if( !m_wTc ) { cout << \"ASSERT ERROR \" << \"[DataNode::getPose] you requested the pose before setting it\\n\"; }\n #endif\n return wTc;\n}\n\n\nconst MatrixXd& DataNode::getPoseCovariance() const {\n std::lock_guard lk(m);\n assert( m_wTc && \"[DataNode::getPoseCovariance] you requested the pose-cov before setting it\");\n\n return wTc_covariance;\n}\n\nconst MatrixXd& DataNode::getPointCloud() const {\n std::lock_guard lk(m);\n assert( m_ptcld && \"[DataNode::getPointCloud] you requested the pointcloud before setting it\");\n return ptcld;\n}\n\nconst MatrixXd& DataNode::getUnVn() const {\n // returns a 3xN matrix\n std::lock_guard lk(m);\n assert( m_unvn && \"[DataNode::getUnVn] you requested UnVn before setting it.\\n\" );\n return unvn;\n }\n\nconst MatrixXd& DataNode::getUV() const {\n // returns a 3xN matrix\n std::lock_guard lk(m);\n assert( m_unvn && \"[DataNode::getUV] you requested UnVn before setting it.\\n\" );\n return uv;\n }\n\nconst VectorXi& DataNode::getFeatIds() const {\n // return a N-vector\n std::lock_guard lk(m);\n assert( m_unvn && \"[DataNode::getFeatIds] you requested UnVn before setting it.\\n\" );\n return tracked_feat_ids;\n }\n\n int DataNode::nPts() const {\n std::lock_guard lk(m);\n #if 0\n if( m_tracked_feat_ids == false || m_tracked_feat_ids_zero_pts == true )\n return 0;\n return tracked_feat_ids.size();\n #endif\n\n }\n\n const ros::Time DataNode::getT() const {\n std::lock_guard lk(m);\n return stamp;\n }\n\n #if 0\n const ros::Time DataNode::getT_image() const {\n std::lock_guard lk(m);\n assert( isImageAvailable() );\n return t_image;\n }\n const ros::Time DataNode::getT_image(short cam_id) const {\n std::lock_guard lk(m);\n assert( isImageAvailable(cam_id) );\n // return t_all_images[cam_id];\n return t_all_images.at(cam_id);\n }\n#endif\n\n\n const ros::Time DataNode::getT_pose() const {\n std::lock_guard lk(m);\n return t_wTc;\n }\n const ros::Time DataNode::getT_ptcld() const {\n std::lock_guard lk(m);\n return t_ptcld;\n }\n const ros::Time DataNode::getT_unvn() const {\n std::lock_guard lk(m);\n return t_unvn;\n }\n const ros::Time DataNode::getT_uv() const {\n std::lock_guard lk(m);\n return t_uv;\n }\n\nvoid DataNode::prettyPrint()\n{\n if( this->isKeyFrame() )\n cout << \"\\033[1;32m\";\n else\n cout << \"\\033[1;31m\";;\n\n cout << \"\\t---DataNode\";\n cout << \"\\tt=\"<< this->getT() ;\n cout << \"\\tkeyFrame=\" << (this->isKeyFrame()?\"YES\":\"NO\") << endl ;\n\n\n cout << \"\\t\\twholeImageDesc: \";\n if( this->isWholeImageDescriptorAvailable() ) {\n cout << \" available and of size: \" << this->getWholeImageDescriptor().size() << endl;\n }\n else { cout << \"Not Available\\n\" ;}\n\n #if 0\n if( isImageAvailable() ) {\n cv::Mat _im = this->getImage();\n cout << \"\\t\\tImage: \" << _im.rows << \"x\" << _im.cols << \"x\" << _im.channels()\n << \" \" << MiscUtils::type2str( _im.type() ) << \"\\tt=\" << t_image << endl;\n\n }\n else { cout << \"\\tImage: N/A\\n\"; }\n #endif\n\n if( isPoseAvailable() ) {\n Matrix4d _pose = this->getPose();\n cout << \"\\t\\tPose : \" << PoseManipUtils::prettyprintMatrix4d( _pose ) << \"\\tt=\" << t_wTc << endl;\n }\n else { cout << \"\\t\\tPose: N/A\\n\"; }\n\n if( isPtCldAvailable() ) {\n MatrixXd _ptcld = this->getPointCloud();\n cout << \"\\t\\tPointCloud : \" << _ptcld.rows() << \"x\" << _ptcld.cols() << \"\\tt=\" << t_ptcld << endl;\n }\n else { cout << \"\\t\\tPointCloud : N/A\\n\"; }\n\n if( isUnVnAvailable() ) {\n MatrixXd _unvn = this->getUnVn();\n cout << \"\\t\\tUnVn : \" << _unvn.rows() << \"x\" << _unvn.cols() << \"\\tt=\" << t_unvn << endl;\n }\n else { cout << \"\\t\\tUnVn : N/A\\n\"; }\n\n if( isUVAvailable() ) {\n MatrixXd _uv = this->getUV();\n cout << \"\\t\\tUV : \" << _uv.rows() << \"x\" << _uv.cols() << \"\\tt=\" << t_uv << endl;\n }\n else { cout << \"\\t\\tUV : N/A\\n\"; }\n\n if( isFeatIdsAvailable() ) {\n VectorXi _ids = this->getFeatIds();\n cout << \"\\t\\tFeatIds : \" << _ids.rows() << \"x\" << _ids.cols() << \"\\tt=\" << t_tracked_feat_ids << endl;\n }\n else { cout << \"\\t\\tFeatIds : N/A\\n\"; }\n cout << \"\\033[0m\\n\";\n}\n\n\n\nvoid DataNode::setWholeImageDescriptor( VectorXd vec )\n{\n std::lock_guard lk(m);\n\n assert( !m_img_desc && \"[DataNode::setWholeImageDescriptor] You are trying to set image desc. The descriptor is already set and this action might overwrite the exisiting descriptor. If you think this is not an error feel free to comment this assert\\n\" );\n img_desc = vec;\n m_img_desc = true;\n}\n\n// const VectorXd& DataNode::getWholeImageDescriptor()\nconst VectorXd DataNode::getWholeImageDescriptor() const\n{\n std::lock_guard lk(m);\n assert( m_img_desc && \"[DataNode::getWholeImageDescriptor] You are trying to get the image descriptor before setting it.\" );\n\n return img_desc;\n\n}\n" }, { "path": "src/DataNode.h", "content": "#pragma once\n\n/** This class holds data at 1 instance of time.\n\n Author : Manohar Kuse \n Created : 27th Oct, 2018\n\n**/\n\n#include \n#include \n#include \n#include \n\n\n//opencv\n#include \n#include \n#include \n#include \n\n// ros\n#include \n#include \n\n// ros msg\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n// #include \n#include \n#include \n#include \n\n// Eigen\n#include \n#include \n#include \nusing namespace Eigen;\n#include \n\n#include \"utils/PoseManipUtils.h\"\n#include \"utils/MiscUtils.h\"\n#include \"utils/TermColor.h\"\nclass DataNode\n{\npublic:\n DataNode( ros::Time stamp ): stamp(stamp)\n {\n is_key_frame = false;\n // m_image=false;\n m_wTc=false;\n }\n\n #if 0\n void setImageFromMsg( const sensor_msgs::ImageConstPtr msg ); ///< this sets the primary image\n void setImageFromMsg( const sensor_msgs::ImageConstPtr msg, short cam_id ); ///< this sets additional image. multiple additional images by Node is possible by using ids\n #endif\n\n void setPoseFromMsg( const nav_msgs::OdometryConstPtr msg );\n void setPose( const Matrix4d __wTc );\n void setPose( const ros::Time __t, const Matrix4d __wTc );\n\n void setPointCloudFromMsg( const sensor_msgs::PointCloudConstPtr msg ); // uses msg->points[ \\forall i].x, .y, .z\n void setUnVnFromMsg( const sensor_msgs::PointCloudConstPtr msg );\n void setUVFromMsg( const sensor_msgs::PointCloudConstPtr msg );\n void setTrackedFeatIdsFromMsg( const sensor_msgs::PointCloudConstPtr msg );\n\n void setAsKeyFrame() { is_key_frame = true; }\n void unsetAsKeyFrame() { is_key_frame = false; }\n\n // This is intended to indicate the number of tracked features from /feature_tracker.\n // TODO: In the future if need be implement to save the tracked points. For now I am not retaining those\n void setNumberOfSuccessfullyTrackedFeatures( int n );\n int getNumberOfSuccessfullyTrackedFeatures() const;\n\n\n bool isKeyFrame() const{ return (bool)is_key_frame; }\n #if 0\n bool isImageAvailable() const { return m_image; }\n bool isImageAvailable(short cam_id) const { return ((t_all_images.count(cam_id)>0)?true:false); }\n #endif\n bool isPoseAvailable() const { return m_wTc; }\n bool isPtCldAvailable() const { return m_ptcld; }\n bool isUnVnAvailable() const{ return m_unvn; }\n bool isUVAvailable() const { return m_uv; }\n bool isFeatIdsAvailable() const { return m_tracked_feat_ids; }\n\n #if 0\n const cv::Mat& getImage() const; //< this will give out the default image.\n const cv::Mat& getImage(short cam_id) const ; // this will give out the image of the cam_id. cam_id=0 will have issues. if you want the default camera image do not pass any argument, which will result in the above call.\n #endif\n const Matrix4d& getPose() const ;\n const MatrixXd& getPoseCovariance() const ; //6x6 matrix\n const MatrixXd& getPointCloud() const ; // returns a 4xN matrix\n const MatrixXd& getUnVn() const ; // returns a 3xN matrix\n const MatrixXd& getUV() const ; // returns a 3xN matrix\n const VectorXi& getFeatIds() const; // return a N-vector\n int nPts() const;\n\n const ros::Time getT() const;\n #if 0\n const ros::Time getT_image() const;\n const ros::Time getT_image(short cam_id) const;\n #endif\n const ros::Time getT_pose() const;\n const ros::Time getT_ptcld() const;\n const ros::Time getT_unvn() const ;\n const ros::Time getT_uv() const ;\n\n // Whole Image descriptors setter and getter\n void setWholeImageDescriptor( VectorXd vec );\n // const VectorXd& getWholeImageDescriptor();\n const VectorXd getWholeImageDescriptor() const; //don't know which one is more suitated to me. :(\n bool isWholeImageDescriptorAvailable() const { return m_img_desc; }\n\n\n void prettyPrint();\n #if 0\n void deallocate_all_images();\n void print_image_cvinfo();\n #endif\n\n\nprivate:\n const ros::Time stamp;\n mutable std::mutex m;\n\n // bool is_key_frame = false;\n std::atomic is_key_frame;\n\n #if 0\n // Raw Image\n cv::Mat image;\n ros::Time t_image;\n // bool m_image=false; // TODO better make this atomic\n std::atomic m_image;\n\n // Additional Raw Images\n std::map all_images;\n std::map t_all_images;\n #endif\n\n\n // Pose (odometry pose from vins estimator)\n Matrix4d wTc;\n MatrixXd wTc_covariance; // 6x6\n ros::Time t_wTc;\n // bool m_wTc=false;\n std::atomic m_wTc;\n\n\n // point cloud (3d data)\n MatrixXd ptcld;\n ros::Time t_ptcld;\n std::atomic m_ptcld;\n std::atomic m_ptcld_zero_pts;\n\n\n // unvn - imaged points in normalized cords\n MatrixXd unvn;\n ros::Time t_unvn;\n std::atomic m_unvn;\n std::atomic m_unvn_zero_pts;\n\n\n // uv - imaged points in observed cords.\n MatrixXd uv;\n ros::Time t_uv;\n std::atomic m_uv;\n std::atomic m_uv_zero_pts;\n\n\n // tracked feat ids\n VectorXi tracked_feat_ids;\n ros::Time t_tracked_feat_ids;\n std::atomic m_tracked_feat_ids;\n std::atomic m_tracked_feat_ids_zero_pts;\n\n int numberOfSuccessfullyTrackedFeatures = -1;\n\n // Whole Image Descriptor\n VectorXd img_desc;\n bool m_img_desc = false;\n\n};\n" }, { "path": "src/DlsPnpWithRansac.cpp", "content": "\n#include \"DlsPnpWithRansac.h\"\n\n\n#ifdef __USE_THEIASFM\n// Theia's ICP\n// [Input]\n// uv_X: a 3d point cloud expressed in some frame-of-ref, call it frame-of-ref of `uv`\n// uvd_Y: a 3d point cloud expressed in another frame-of-ref, call it frame-of-ref of `uvd`\n// [Output]\n// uvd_T_uv: Relative pose between the point clouds\n// [Note]\n// uv_X <---> uvd_Y\n// #define ____P3P_ICP_( msg ) msg;\n#define ____P3P_ICP_( msg ) ;\nfloat StaticTheiaPoseCompute::P3P_ICP( const vector& uv_X, const vector& uvd_Y,\n Matrix4d& uvd_T_uv, string & p3p__msg )\n{\n if( uv_X.size() < 20 ) {\n ____P3P_ICP_( cout << \"[StaticTheiaPoseCompute::P3P_ICP] Too few input points. You provided \" << uv_X.size() << endl; )\n return -1; // if you give me too few points, i return error.\n }\n\n // call this theia::AlignPointCloudsUmeyamaWithWeights\n // void AlignPointCloudsUmeyamaWithWeights(\n // const std::vector& left,\n // const std::vector& right,\n // const std::vector& weights, Eigen::Matrix3d* rotation,\n // Eigen::Vector3d* translation, double* scale)\n\n\n p3p__msg = \"\";\n Matrix3d ____R;\n Vector3d ____t; double ___s=1.0;\n\n ElapsedTime timer;\n timer.tic();\n\n #if 0\n theia::AlignPointCloudsUmeyama( uv_X, uvd_Y, &____R, &____t, &___s ); // all weights = 1. TODO: ideally weights could be proportion to point's Z.\n // theia::AlignPointCloudsICP( uv_X, uvd_Y, &____R, &____t ); // all weights = 1. TODO: ideally weights could be proportion to point's Z.\n\n double elapsed_time_p3p = timer.toc_milli();\n\n ____P3P_ICP_(\n cout << \"___R:\\n\" << ____R << endl;\n cout << \"___t: \" << ____t.transpose() << endl;\n cout << \"___s: \" << ___s << endl;\n )\n\n uvd_T_uv = Matrix4d::Identity();\n uvd_T_uv.topLeftCorner<3,3>() = ____R;\n uvd_T_uv.col(3).topRows(3) = ____t;\n\n ____P3P_ICP_(\n cout << TermColor::GREEN() << \"p3p done in (ms) \" << elapsed_time_p3p << \" p3p_ICP: {uvd}_T_{uv} : \" << PoseManipUtils::prettyprintMatrix4d( uvd_T_uv ) << TermColor::RESET() << endl;\n )\n\n\n\n if( min( ___s, 1.0/___s ) < 0.9 ) {\n cout << TermColor::RED() << \"theia::AlignPointCloudsUmeyama scales doesn't look good;this usually implies that estimation is bad. scale= \" << ___s << endl;\n p3p__msg += \"p3p_ICP: scale=\" +to_string( ___s )+\" {uvd}_T_{uv} : \" + PoseManipUtils::prettyprintMatrix4d( uvd_T_uv );\n p3p__msg += \"p3p done in (ms)\" + to_string(elapsed_time_p3p)+\"; theia::AlignPointCloudsUmeyama scales doesn't look good, this usually implies that estimation is bad. scale= \" + to_string(___s);\n return -1;\n }\n\n p3p__msg += \"p3p done in (ms)\" + to_string(elapsed_time_p3p)+\"; p3p_ICP: {uvd}_T_{uv} : \" + PoseManipUtils::prettyprintMatrix4d( uvd_T_uv );\n p3p__msg += \";weight=\"+to_string( min( ___s, 1.0/___s ) );\n return min( ___s, 1.0/___s );\n #endif\n\n // with ransac\n timer.tic();\n vector data_r;\n for( int i=0 ; i ransac_estimator( params, icp_estimator);\n ransac_estimator.Initialize();\n\n theia::RansacSummary summary;\n ransac_estimator.Estimate(data_r, &best_rel_pose, &summary);\n auto elapsed_time_p3p_ransac = timer.toc_milli();\n\n uvd_T_uv = Matrix4d::Identity();\n uvd_T_uv = best_rel_pose.b_T_a ;\n\n\n ____P3P_ICP_(\n cout << TermColor::iGREEN() << \"ICP Ransac:\";\n // for( int i=0; i& w_X, const std::vector& c_uv_normalized,\n Matrix4d& c_T_w,\n string& pnp__msg )\n{\n if( w_X.size() < 20 ) {\n ___P_N_P__( cout << \"[StaticTheiaPoseCompute::PNP] Too few input points. You provided \" << w_X.size() << endl; )\n return -1; // if you give me too few points, i return error.\n }\n pnp__msg = \"\";\n ElapsedTime timer;\n #if 0\n //--- DlsPnp\n std::vector solution_rotations;\n std::vector solution_translations;\n timer.tic();\n theia::DlsPnp( c_uv_normalized, w_X, &solution_rotations, &solution_translations );\n auto elapsed_dls_pnp = timer.toc_milli() ;\n ___P_N_P__(\n cout << elapsed_dls_pnp << \" : (ms) : theia::DlsPnp done in\\n\";\n cout << \"solutions count = \" << solution_rotations.size() << \" \" << solution_translations.size() << endl;\n )\n\n if( solution_rotations.size() == 0 ) {\n ___P_N_P__(\n cout << TermColor::RED() << \" theia::DlsPnp returns no solution\" << TermColor::RESET() << endl;\n )\n pnp__msg = \" theia::DlsPnp returns no solution\";\n return -1.;\n }\n\n if( solution_rotations.size() > 1 ) {\n ___P_N_P__(\n cout << TermColor::RED() << \" theia::DlsPnp returns multiple solution\" << TermColor::RESET() << endl;\n )\n pnp__msg = \" theia::DlsPnp returns multiple solution\";\n return -1.;\n }\n\n // retrive solution\n c_T_w = Matrix4d::Identity();\n c_T_w.topLeftCorner(3,3) = solution_rotations[0].toRotationMatrix();\n c_T_w.col(3).topRows(3) = solution_translations[0];\n\n ___P_N_P__(\n // cout << \"solution_T \" << b_T_a << endl;\n cout << TermColor::GREEN() << \"DlsPnp (c_T_w): \" << PoseManipUtils::prettyprintMatrix4d( c_T_w ) << TermColor::RESET() << endl;\n // out_b_T_a = b_T_a;\n )\n\n pnp__msg += \"DlsPnp (c_T_w): \" + PoseManipUtils::prettyprintMatrix4d( c_T_w ) + \";\";\n pnp__msg += \" elapsed_dls_pnp_ms=\"+to_string(elapsed_dls_pnp)+\";\";\n\n // return 1.0;\n #endif\n\n\n #if 1\n //--- DlsPnpWithRansac\n timer.tic();\n // prep data\n vector data_r;\n for( int i=0 ; i ransac_estimator(params, dlspnp_estimator);\n // Initialize must always be called!\n ransac_estimator.Initialize();\n\n theia::RansacSummary summary;\n ransac_estimator.Estimate(data_r, &best_rel_pose, &summary);\n\n auto elapsed_dls_pnp_ransac=timer.toc_milli();\n ___P_N_P__(\n cout << elapsed_dls_pnp_ransac << \"(ms)!! DlsPnpWithRansac ElapsedTime includes data prep\\n\";\n cout << \"Ransac:\";\n // for( int i=0; i& w_X, const std::vector& c_uv_normalized,\n Matrix4d& c_T_w,\n string& pnp__msg )\n{\n assert( w_X.size() == c_uv_normalized.size() && w_X.size() > 8 );\n if( w_X.size() != c_uv_normalized.size() ) {\n cout << \"[StaticCeresPoseCompute::PNP] w_X.size() != c_uv_normalized.size()\\n\";\n exit( 1 );\n }\n __StaticCeresPoseCompute_PNP_(\n cout << \"w_X.size()=\" << w_X.size() << \" c_uv_normalized.size()=\" << c_uv_normalized.size() << endl;\n )\n\n // setup ceres problem for PNP\n // minimize_{R,t} \\sum_i ( PI( c_(R|t)_w * w_X[i] ) - u[i] )\n ceres::Problem problem;\n\n // optimization variables\n double ypr[3], tr[3];\n PoseManipUtils::eigenmat_to_rawyprt( c_T_w, ypr, tr );\n // double yaw=ypr[0], pitch=ypr[1], roll=ypr[2], tx=tr[0], ty=tr[1], tz=tr[2];\n\n for( int i=0 ; i \" << c_uv_normalized[i].transpose() << endl;\n ceres::CostFunction * cost_function = PNPEulerAngleError::Create( w_X[i], c_uv_normalized[i] );\n problem.AddResidualBlock( cost_function, new ceres::HuberLoss(0.1), &ypr[0], &ypr[1], &ypr[2], &tr[0], &tr[1], &tr[2] );\n // problem.AddResidualBlock( cost_function, NULL, &ypr[0], &ypr[1], &ypr[2], &tr[0], &tr[1], &tr[2] );\n\n }\n\n\n // Local Parameterization (Angle)\n ceres::LocalParameterization *angle_parameterization = AngleLocalParameterization::Create();\n problem.SetParameterization( &ypr[0], angle_parameterization );\n problem.SetParameterization( &ypr[1], angle_parameterization );\n problem.SetParameterization( &ypr[2], angle_parameterization );\n\n // Set as constant\n problem.SetParameterBlockConstant( &ypr[1] ); //pitch\n problem.SetParameterBlockConstant( &ypr[2] ); //roll\n\n\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::DENSE_SCHUR;\n options.minimizer_progress_to_stdout = false;\n // options.trust_region_strategy_type = ceres::DOGLEG;\n // options.dogleg_type = ceres::SUBSPACE_DOGLEG;\n ceres::Solver::Summary summary;\n __StaticCeresPoseCompute_PNP_(\n cout << \"yaw,pitch,roll=\" << ypr[0] << \" \" << ypr[1] << \" \" << ypr[2] << \"; tx,ty,tz=\" << tr[0] << \" \"<< tr[1] << \" \" << tr[2] << endl;\n cout << \"Ceres::Solve()\\n\";\n )\n\n pnp__msg += \"initial_guess: \"+PoseManipUtils::prettyprintMatrix4d( c_T_w ) + \";\";\n ceres::Solve(options, &problem, &summary);\n __StaticCeresPoseCompute_PNP_(\n cout << \"yaw,pitch,roll=\" << ypr[0] << \" \" << ypr[1] << \" \" << ypr[2] << \"; tx,ty,tz=\" << tr[0] << \" \"<< tr[1] << \" \" << tr[2] << endl;\n )\n // cout << summary.FullReport() << endl;\n cout << summary.BriefReport() << endl;\n pnp__msg += summary.BriefReport() + \";\";\n\n\n PoseManipUtils::rawyprt_to_eigenmat( ypr, tr, c_T_w );\n pnp__msg += \"final: \"+PoseManipUtils::prettyprintMatrix4d( c_T_w ) + \";\";\n\n return 1.0;\n}\n\n\n\n\n\n\n#define __StaticCeresPoseCompute_P3P_(msg) msg;\n// #define __StaticCeresPoseCompute_P3P_(msg) ;\nfloat StaticCeresPoseCompute::P3P_ICP( const std::vector& a_X, const std::vector& b_X,\n Matrix4d& b_T_a,\n string& p3p__msg )\n{\n assert( a_X.size() == b_X.size() && a_X.size() > 8 );\n if( a_X.size() != b_X.size() ) {\n cout << \"[StaticCeresPoseCompute::P3P] a_X.size() != b_X.size()\\n\";\n exit( 1 );\n }\n __StaticCeresPoseCompute_P3P_(\n cout << \"a_X.size()=\" << a_X.size() << \" b_X.size()=\" << b_X.size() << endl;\n )\n\n // setup ceres problem for PNP\n // minimize_{R,t} \\sum_i ( c_(R|t)_w * a_X[i] - b_X[i] )\n ceres::Problem problem;\n\n // optimization variables\n double ypr[3], tr[3];\n PoseManipUtils::eigenmat_to_rawyprt( b_T_a, ypr, tr );\n // double yaw=ypr[0], pitch=ypr[1], roll=ypr[2], tx=tr[0], ty=tr[1], tz=tr[2];\n\n for( int i=0 ; i \" << b_X[i].transpose() << endl;\n ceres::CostFunction * cost_function = P3PEulerAngleError::Create( a_X[i], b_X[i] );\n problem.AddResidualBlock( cost_function, new ceres::HuberLoss(0.1), &ypr[0], &ypr[1], &ypr[2], &tr[0], &tr[1], &tr[2] );\n // problem.AddResidualBlock( cost_function, NULL, &ypr[0], &ypr[1], &ypr[2], &tr[0], &tr[1], &tr[2] );\n\n }\n\n\n // Local Parameterization (Angle)\n ceres::LocalParameterization *angle_parameterization = AngleLocalParameterization::Create();\n problem.SetParameterization( &ypr[0], angle_parameterization );\n problem.SetParameterization( &ypr[1], angle_parameterization );\n problem.SetParameterization( &ypr[2], angle_parameterization );\n\n // Set as constant\n problem.SetParameterBlockConstant( &ypr[1] ); //pitch\n problem.SetParameterBlockConstant( &ypr[2] ); //roll\n\n\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::DENSE_SCHUR;\n options.minimizer_progress_to_stdout = false;\n // options.trust_region_strategy_type = ceres::DOGLEG;\n // options.dogleg_type = ceres::SUBSPACE_DOGLEG;\n ceres::Solver::Summary summary;\n __StaticCeresPoseCompute_P3P_(\n cout << \"yaw,pitch,roll=\" << ypr[0] << \" \" << ypr[1] << \" \" << ypr[2] << \"; tx,ty,tz=\" << tr[0] << \" \"<< tr[1] << \" \" << tr[2] << endl;\n cout << \"Ceres::Solve()\\n\";\n )\n p3p__msg += \"initial bTa: \"+PoseManipUtils::prettyprintMatrix4d( b_T_a ) + \";\";\n ceres::Solve(options, &problem, &summary);\n __StaticCeresPoseCompute_P3P_(\n cout << \"yaw,pitch,roll=\" << ypr[0] << \" \" << ypr[1] << \" \" << ypr[2] << \"; tx,ty,tz=\" << tr[0] << \" \"<< tr[1] << \" \" << tr[2] << endl;\n )\n __StaticCeresPoseCompute_P3P_(\n // cout << summary.FullReport() << endl;\n cout << summary.BriefReport() << endl;\n )\n p3p__msg += summary.BriefReport() + \";\";\n\n PoseManipUtils::rawyprt_to_eigenmat( ypr, tr, b_T_a );\n p3p__msg += \"final: \"+PoseManipUtils::prettyprintMatrix4d( b_T_a ) + \";\";\n\n return 1.0;\n}\n" }, { "path": "src/DlsPnpWithRansac.h", "content": "#pragma once\n\n/* This is a RanSAC wrapper for theia::DlsPnp. Based on sample code on theia-sfm website.\n The official website has some obivious issues.\n\n Author : Manohar Kuse \n Created : 4th Jan, 2018\n*/\n\n\n#include \"utils/TermColor.h\"\n#include \"utils/ElapsedTime.h\"\n#include \"utils/PoseManipUtils.h\"\n\n#include \n\n#include \n#include \n\n#include \"PNPCeresCostFunctions.h\"\n\nusing namespace Eigen;\nusing namespace std;\n\n\n#define __USE_THEIASFM //comment this out to compile with theia-sfm\n#ifdef __USE_THEIASFM\n#include \n/////////// DlsPnp-RANSAC ///////////////////\n// Data\nstruct CorrespondencePair_3d2d {\n Vector3d a_X; // 3d point expressed in co-ordinate system of `image-a`\n Vector2d uv_d; //feature point in normalized-image co-ordinates. of the other view (b)\n};\n\n// Model\nstruct RelativePose {\n Matrix4d b_T_a;\n};\n\n\nclass DlsPnpWithRansac: public theia::Estimator {\npublic:\n // Number of points needed to estimate a line.\n double SampleSize() const { return 15; }\n\n // Estimate a pose from N points\n bool EstimateModel( const std::vector& data,\n std::vector* models) const {\n\n std::vector feature_position;\n std::vector world_point;\n for( int i=0 ; i solution_rotations;\n std::vector solution_translations;\n\n theia::DlsPnp( feature_position, world_point, &solution_rotations, &solution_translations );\n if( solution_rotations.size() == 1 ) {\n RelativePose r;\n r.b_T_a = Matrix4d::Identity();\n r.b_T_a.topLeftCorner(3,3) = solution_rotations[0].toRotationMatrix();\n r.b_T_a.col(3).topRows(3) = solution_translations[0];\n models->push_back( r );\n }\n else {\n return false;\n }\n }\n\n\n double Error( const CorrespondencePair_3d2d& point, const RelativePose& r ) const {\n Vector3d b_X = r.b_T_a.topLeftCorner(3,3) * point.a_X + r.b_T_a.col(3).topRows(3);\n double depth = b_X(2);\n b_X /= b_X(2);\n\n double reproj_error = abs(b_X(0) - point.uv_d(0)) + abs(b_X(1) - point.uv_d(1));\n\n // I want nearby points to have smaller Error than far off points.\n // So artificially increase the error when depth is higher for the point.\n double f = 1.;\n #if 0\n if( depth < 1. )\n f = .5;\n if( depth >=1 && depth < 3 )\n f = 5.0;\n if( depth >=3 && depth < 8 )\n f = 2.;\n if( depth >= 8. && depth < 15 )\n f = 1.7;\n if( depth >= 15 )\n f = 0.1;\n #endif\n\n return reproj_error*f;\n }\n};\n/////////// END DlsPnp-RANSAC ///////////////////\n#endif\n\n#ifdef __USE_THEIASFM\n//////////////////// AlignPointCloudsUmeyama with Ransac ///////////////////////\n// Data\nstruct CorrespondencePair_3d3d {\n Vector3d a_X; // 3d point expressed in co-ordinate system of `image-a`\n Vector3d b_X; // 3d point expressed in co-ordinate system of `image-b`\n};\n\n// Model\n// struct RelativePose {\n // Matrix4d b_T_a;\n// };\n\nclass AlignPointCloudsUmeyamaWithRansac: public theia::Estimator {\npublic:\n // Number of points needed to estimate a line.\n double SampleSize() const { return 10; }\n\n bool EstimateModel( const std::vector& data,\n std::vector* models) const {\n //TODO\n\n std::vector a_X;\n std::vector b_X;\n for( int i=0 ; i 0.9 ) {\n RelativePose r;\n r.b_T_a = Matrix4d::Identity();\n r.b_T_a.topLeftCorner(3,3) = ____R;\n r.b_T_a.col(3).topRows(3) = ____t;\n models->push_back( r );\n // cout << \"Estimate s=\" << ___s << \" \" << PoseManipUtils::prettyprintMatrix4d(r.b_T_a)<< endl;\n return true;\n } else {\n return false;\n }\n\n }\n\n double Error( const CorrespondencePair_3d3d& point, const RelativePose& r ) const {\n Vector3d b_X_cap = r.b_T_a.topLeftCorner(3,3) * point.a_X + r.b_T_a.col(3).topRows(3);\n\n double err = (b_X_cap - point.b_X).norm();\n\n double f=1.0;\n if( point.a_X(2) < 1. && point.a_X(2) > 8. )\n f = 1.2;\n\n // cout << \"ICP RAnsac error=\" << err << endl;\n\n return f*err;\n\n }\n\n};\n\n\nclass StaticTheiaPoseCompute\n{\npublic:\n static float P3P_ICP( const vector& uv_X, const vector& uvd_Y,\n Matrix4d& uvd_T_uv, string & p3p__msg );\n\n static float PNP( const std::vector& w_X, const std::vector& c_uv_normalized,\n Matrix4d& c_T_w,\n string& pnp__msg );\n\n};\n\n//////////////////// AlignPointCloudsUmeyama with Ransac ///////////////////////\n\n#endif\n\n// Write an ceres based iterative PnP and P3P for better control and 4DOF optimization\nclass StaticCeresPoseCompute\n{\npublic:\n // PNP With ceres: minimize_{R,t} \\sum_i ( PI( c_(R|t)_w * w_X[i] ) - u[i] )\n // [Input]\n // w_X: World 3d points.\n // c_uv_normalized: Image points in normalized co-ordinates\n // c_T_w: initial guess, this will be modified to reflect the optimized solution\n // [Output]\n // c_T_w\n // pnp__msg: debugging string msg\n static float PNP( const std::vector& w_X, const std::vector& c_uv_normalized,\n Matrix4d& c_T_w,\n string& pnp__msg );\n\n\n // p3p (icp) with ceres: minimize_{R,t} \\sum_i ( b_{R,t}_a * a_X - b_X )\n // [Input]\n // a_X : 3d points in co-ordinate frame of a\n // b_X : 3d points in co-orfinate frame of b\n // b_T_a: initial guess\n // [Output]\n // b_T_a : optimized output\n // p3p_msg: debug msg\n static float P3P_ICP( const std::vector& a_X, const std::vector& b_X,\n Matrix4d& c_T_w,\n string& pnp__msg );\n\n};\n" }, { "path": "src/HypothesisManager.cpp", "content": "#include \"HypothesisManager.h\"\n\n\nHypothesisManager::HypothesisManager()\n{\n std::cout << \"HypothesisManager constructor\\n\";\n}\n\nHypothesisManager::~HypothesisManager()\n{\n std::cout << \"HypothesisManager descrutctor\\n\";\n}\n\n\nbool HypothesisManager::add_node( int a, int b, float dot_prod )\n{\n // See if (a,b) is in a already active hypothesis ?\n // |---(yes) give strength, possibly increase its time-to-live (ttl)\n // |---(no ) is it worth creating new hypothesis?\n // |--- (yes)\n // |--- (no) discard this\n std::lock_guard lk(mx);\n\n cout << \"\\n[HypothesisManager::add_node] add_node(\"<print_active_hypothesis(header_str);\n #endif\n\n for( auto c=it->list_of_nodes_in_this_hypothesis.rbegin() ;\n c!=it->list_of_nodes_in_this_hypothesis.rend() ;\n c++ )\n {\n int _a = std::get<0>( *c );\n int _b = std::get<1>( *c );\n float _dot_prod = std::get<2>( *c );\n\n if( abs(a-_a) < 7 && abs(b-_b) < 7 ) {\n it->list_of_nodes_in_this_hypothesis.push_back( make_tuple(a,b,dot_prod) );\n was_in_one_of_the_active_hyp = true;\n printf( \"Added (%d,%d,%f) in active_hyp#%d\\n\", a,b,dot_prod, it-active_hyp.begin() );\n\n printf( \"Increment time_to_live of this hypothesis\\n\" );\n it->increment_ttl();\n break;\n }\n }\n }\n\n\n\n //-------------b\n if( was_in_one_of_the_active_hyp == false ) {\n cout << \"New Hypothesis added: \"<< a<< \", \" << b << \", \" << dot_prod << endl;\n active_hyp.push_back( Hypothesis( a,b, dot_prod) );\n }\n\n\n}\n\nvoid HypothesisManager::digest()\n{\n std::lock_guard lk(mx);\n\n for( auto it=active_hyp.begin() ; it!=active_hyp.end() ; it++ )\n {\n it->decrement_ttl();\n it->decrement_ttl();\n it->decrement_ttl();\n it->decrement_ttl();\n // it->decrement_ttl();\n }\n}\n\n\nvoid HypothesisManager::monitoring_thread()\n{\n cout << TermColor::GREEN() << \"[HypothesisManager::monitoring_thread] thread started\\n\" << TermColor::RESET() << endl;\n while( b_monitoring_thread )\n {\n { // threadsafe zone\n std::lock_guard lk(mx);\n ofstream myfile;\n myfile.open (\"/dev/pts/1\");\n int n_actives = 0;\n for( int i=0 ; i 0 ) {\n myfile << TermColor::GREEN() << \"Currently active hypothesis=\" << n_actives << TermColor::RESET() << endl;\n }\n else {\n myfile << TermColor::RED() << \"No active hypothesis\" << TermColor::RESET() << endl;\n }\n myfile.close();\n\n } // end of threadsafe zone\n\n std::this_thread::sleep_for(std::chrono::milliseconds(1000));\n\n }\n\n cout << TermColor::RED() << \"[HypothesisManager::monitoring_thread] thread Ended\\n\" << TermColor::RESET() << endl;\n}\n" }, { "path": "src/HypothesisManager.h", "content": "#pragma once\n// This class will manage all the loop hypothesis.\n// This is for implementing the heuristics for multi hypothesis tracking.\n// A new graph-node is created when I have a new putative matching pair.\n// This matching pair is associated with an existing hypothesis or a new\n// hypothesis is created. Allthe hypothesis will have a time-to-live.\n\n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n\n#include \"utils/TermColor.h\"\nusing namespace std;\n\nclass Hypothesis\n{\npublic:\n Hypothesis(int a, int b, float prod )\n {\n list_of_nodes_in_this_hypothesis.push_back( std::make_tuple(a,b, prod) );\n time_to_live = 20;\n }\n std::vector< std::tuple > list_of_nodes_in_this_hypothesis;\n\n string info_string()\n {\n std::stringstream ss;\n ss << \"ttl=\" << time_to_live << \" \";\n ss << \"\\tlen(list) = \" << list_of_nodes_in_this_hypothesis.size() << \"\\t\";\n\n //starts and ends of the hyp\n vector tmp_a, tmp_b;\n for( auto it= list_of_nodes_in_this_hypothesis.begin() ;\n it!= list_of_nodes_in_this_hypothesis.end() ;\n it++ )\n {\n tmp_a.push_back( std::get<0>(*it) );\n tmp_b.push_back( std::get<1>(*it) );\n }\n\n const auto result_a = std::minmax_element( tmp_a.begin(), tmp_a.end() );\n const auto result_b = std::minmax_element( tmp_b.begin(), tmp_b.end() );\n\n ss << *result_a.first << \"---->\" << *result_a.second << \" \";\n ss << *result_b.first << \"---->\" << *result_b.second << \" \";\n\n return ss.str();\n }\n\n #if 0\n void print_active_hypothesis( const string header_str ) {\n\n ofstream myfile;\n myfile.open (\"/dev/pts/1\");\n\n myfile << header_str << \"\\t\\t\";\n myfile << \"ttl=\" << time_to_live << \" \";\n myfile << \"\\tlen(list) = \" << list_of_nodes_in_this_hypothesis.size() << \"\\t\";\n vector tmp_a, tmp_b;\n for( auto it= list_of_nodes_in_this_hypothesis.begin() ;\n it!= list_of_nodes_in_this_hypothesis.end() ;\n it++ )\n {\n tmp_a.push_back( std::get<0>(*it) );\n tmp_b.push_back( std::get<1>(*it) );\n }\n\n // cout <<\n const auto result_a = std::minmax_element( tmp_a.begin(), tmp_a.end() );\n const auto result_b = std::minmax_element( tmp_b.begin(), tmp_b.end() );\n\n myfile << *result_a.first << \"---->\" << *result_a.second << \" \";\n myfile << *result_b.first << \"---->\" << *result_b.second << \" \";\n myfile << endl;\n\n #if 0 //full details\n for( auto it= list_of_nodes_in_this_hypothesis.begin() ;\n it!= list_of_nodes_in_this_hypothesis.end() ;\n it++ )\n {\n myfile << \" (\" << std::get<0>(*it) << \",\" << std::get<1>(*it) << \") \";\n }\n myfile << endl;\n #endif\n\n myfile.close();\n }\n #endif\n\n\npublic:\n void decrement_ttl()\n {\n if( time_to_live <= 0 )\n return;\n time_to_live--;\n }\n\n void increment_ttl()\n {\n // linear increment\n time_to_live++;\n\n\n if( time_to_live > 100 )\n time_to_live++;\n\n // multiplicative increment\n // time_to_live += int(.1*time_to_live);\n\n }\n int get_ttl() { return time_to_live; }\n\n bool is_hypothesis_active() { if(time_to_live<=0) return false; else return true; }\n int n_elements_in_list() { return list_of_nodes_in_this_hypothesis.size(); }\n\nprivate:\n int time_to_live;\n\n};\n\nclass HypothesisManager\n{\npublic:\n HypothesisManager();\n ~HypothesisManager();\n\n // We add a node in the graph for every putative loop candidate.\n // a,b: Indicates a loop connection between node[a] and node[b] with a dot_product=dot_prod\n bool add_node( int a, int b, float dot_prod );\n\n\n //---\n // Info functions\n //---\n // int n_active_hypothesis();\n\n\n //---\n // Loop through each active hypothesis and decrement the time-to-live\n // This is intented to be called every few milisecs.\n void digest();\n\n\n\nprivate:\n mutable mutex mx;\n std::vector active_hyp;\n\n\n//----------------------------\n//--- Monitoring Thread\n//----------------------------\npublic:\n void monitoring_thread();\n void monitoring_thread_enable() { b_monitoring_thread = true;}\n void monitoring_thread_disable() {b_monitoring_thread = false;}\n\nprivate:\n atomic b_monitoring_thread;\n\n};\n" }, { "path": "src/ImageDataManager.cpp", "content": "#include \"ImageDataManager.h\"\n\n\nImageDataManager::ImageDataManager()\n{\n cout << TermColor::iYELLOW() << \"Constructor for ImageDataManager\\n\" << TermColor::RESET();\n\n}\n\nbool ImageDataManager::initStashDir( bool clear_dir, const string __dir )\n{\n STASH_DIR = __dir; //string( \"/tmp/cerebro_stash\" );\n\n if( clear_dir ) // rm -rf && mksir\n {\n //\n // $ rm -rf ${save_dir}\n // $ mkdir ${save_dir}\n // #include \n string system_cmd;\n\n system_cmd = string(\"rm -rf \"+STASH_DIR).c_str();\n cout << TermColor::YELLOW() << \"[ImageDataManager::initStashDir] \" << system_cmd << TermColor::RESET() << endl;\n const int rm_dir_err = RawFileIO::exec_cmd( system_cmd );\n\n if ( rm_dir_err == -1 )\n {\n cout << TermColor::RED() << \"[ImageDataManager::initStashDir] Error removing directory!\\n\" << TermColor::RESET() << endl;\n exit(1);\n }\n\n system_cmd = string(\"mkdir -p \"+STASH_DIR).c_str();\n cout << TermColor::YELLOW() << \"[ImageDataManager::initStashDir] \" << system_cmd << TermColor::RESET() << endl;\n // const int dir_err = system( system_cmd.c_str() );\n const int dir_err = RawFileIO::exec_cmd( system_cmd );\n if ( dir_err == -1 )\n {\n cout << TermColor::RED() << \"[ImageDataManager::initStashDir] Error creating directory!\\n\" << TermColor::RESET() << endl;\n exit(1);\n }\n // done emptying the directory.\n } else {\n // make sure the path is a directory\n if( RawFileIO::is_path_a_directory(STASH_DIR) ) {\n // ok good!\n } else {\n cout << TermColor::RED() << \"[ImageDataManager::initStashDir]You asked me to assume the directory ``\"<< STASH_DIR << \"`` as stash directory, but it doesnt seem to exist...EXIT\" << TermColor::RESET() << endl;\n ROS_ERROR( \"[ImageDataManager::initStashDir]You asked me to assume the directory ``%s`` as stash directory, but it doesnt seem to exist...EXIT\", STASH_DIR.c_str() );\n exit(1);\n }\n }\n\n m_STASH_DIR = true;\n}\n\nImageDataManager::~ImageDataManager()\n{\n std::lock_guard lk(m);\n // TODO\n // rm all available on RAM\n status.clear();\n image_data.clear();\n\n if( rm_stash_dir_in_destructor )\n {\n // rm -rf stash folder\n string system_cmd;\n\n system_cmd = string(\"rm -rf \"+STASH_DIR).c_str();\n cout << TermColor::YELLOW() << \"[ImageDataManager::~ImageDataManager] \" << system_cmd << TermColor::RESET() << endl;\n const int rm_dir_err = RawFileIO::exec_cmd( system_cmd );\n\n if ( rm_dir_err == -1 )\n {\n cout << TermColor::RED() << \"[ImageDataManager::~ImageDataManager] Error removing directory!\\n\" << TermColor::RESET() << endl;\n exit(1);\n }\n }\n else {\n cout << TermColor::YELLOW() << \"[ImageDataManager::~ImageDataManager] not doing rm -rf \" << STASH_DIR << \", because the bool variable was found to be false\\n\";\n }\n\n}\n\nbool ImageDataManager::setImage( const string ns, const ros::Time t, const cv::Mat img )\n{\n cout << \"[ImageDataManager::setImage] not implemented\\n\";\n exit(1);\n}\n\n// #define __ImageDataManager__setImageFromMsg( msg ) msg;\n#define __ImageDataManager__setImageFromMsg( msg ) ;\nbool ImageDataManager::setNewImageFromMsg( const string ns, const sensor_msgs::ImageConstPtr msg )\n{\n std::lock_guard lk(m);\n cv::Mat __image = (cv_bridge::toCvCopy(msg)->image).clone();\n\n\n auto key = std::make_pair(ns, msg->header.stamp);\n __ImageDataManager__setImageFromMsg(\n cout << TermColor::iWHITE() << \"[ImageDataManager::setImageFromMsg] insert at(\" << ns << \",\" << msg->header.stamp << \")\" << TermColor::RESET() << endl;\n )\n\n // TODO: throw error if attempting to over-write\n status[ key ] = MEMSTAT::AVAILABLE_ON_RAM;\n image_data[ key ] = __image;\n return true;\n}\n\n\n// #define __ImageDataManager__getImage( msg ) msg;\n#define __ImageDataManager__getImage( msg ) ;\nbool ImageDataManager::getImage( const string ns, const ros::Time t, cv::Mat& outImg )\n{\n ensure_init();\n decrement_hit_counts_and_deallocate_expired();\n\n std::lock_guard lk(m);\n\n auto key = std::make_pair(ns,t);\n if( status.count(key) > 0 ) {\n if( status.at(key) == MEMSTAT::AVAILABLE_ON_RAM ) {\n __ImageDataManager__getImage(\n cout << TermColor::iGREEN() << \"[ImageDataManager::getImage] retrived (from ram) at ns=\" << ns << \" t=\" << t << TermColor::RESET() << endl;\n )\n outImg = image_data.at( key );\n return true;\n }\n else {\n if( status.at(key) == MEMSTAT::AVAILABLE_ON_DISK )\n {\n const string fname = key_to_imagename(ns, t);\n __ImageDataManager__getImage(\n cout << TermColor::iYELLOW() << \"[ImageDataManager::getImage] retrived (fname=\" << fname << \") at ns=\" << ns << \" t=\" << t << TermColor::RESET() << endl;\n )\n\n if( ns == \"depth_image\" ) {\n outImg = cv::imread( fname+\".png\", -1 );\n } else {\n outImg = cv::imread( fname, -1 ); //-1 is for read as it is.\n }\n if( !outImg.data )\n {\n cout << TermColor::RED() << \"[ImageDataManager::getImage] failed to load image \" << fname << TermColor::RESET() << endl;\n exit(1);\n return false;\n }\n\n //=----------------cache hit this\n __ImageDataManager__getImage(\n cout << TermColor::iYELLOW() << \"\\t\\tchange the status to MEMSTAT::AVAILABLE_ON_RAM_DUETO_HIT and set expiry to 10\\n\" << TermColor::RESET();\n )\n status[key] = MEMSTAT::AVAILABLE_ON_RAM_DUETO_HIT;\n image_data[key] = outImg;\n hit_count[key] = 10;\n //=----------------\n return true;\n }\n\n if( status.at(key) == MEMSTAT::AVAILABLE_ON_RAM_DUETO_HIT )\n {\n __ImageDataManager__getImage(\n cout << TermColor::iBLUE() << \"[ImageDataManager::getImage] retrived (AVAILABLE_ON_RAM_DUETO_HIT) at ns=\" << ns << \" t=\" << t << TermColor::RESET() << endl;\n )\n outImg = image_data.at(key);\n hit_count[key] +=5;\n return true;\n }\n\n if( status.at(key) == MEMSTAT::UNAVAILABLE )\n {\n cout << TermColor::iYELLOW() << \"[ImageDataManager::getImage] WARNING you requested image which was MEMSTAT::UNAVAILABLE, this should not be happening but for now return false, at ns=\" << ns << \" t=\" << t << TermColor::RESET() << endl;\n return false;\n }\n\n\n cout << TermColor::iGREEN() << \"[ImageDataManager::getImage] got a corrupt key. Report this error to authors if this occurs\\n\" << TermColor::RESET();\n exit(1);\n }\n }\n else\n {\n cout << TermColor::RED() << \"[ImageDataManager::getImage] FATAL-ERROR you requested image ns=\" << ns << \", t=\" << t << \"; However it was not found on the map. FATAL ERRR\\n\" << TermColor::RESET();\n exit(1);\n return false;\n }\n\n return false;\n}\n\n\n// #define __ImageDataManager__rmImage(msg) msg;\n#define __ImageDataManager__rmImage(msg) ;\nbool ImageDataManager::rmImage( const string ns, const ros::Time t )\n{\n ensure_init();\n std::lock_guard lk(m);\n auto key = std::make_pair(ns, t);\n if( status.count(key) > 0 )\n {\n if( status.at(key) == MEMSTAT::AVAILABLE_ON_RAM ) {\n __ImageDataManager__rmImage(\n cout << TermColor::iWHITE() << \"[ImageDataManager::rmImage] ns=\" << ns << \", t=\" << t << \". FOUND, available on ram, do complete removal\" << TermColor::RESET() << endl;\n )\n status[key] = MEMSTAT::UNAVAILABLE;\n\n auto it_b = image_data.find( key );\n image_data.erase( it_b );\n return true;\n }\n else {\n __ImageDataManager__rmImage(\n cout << TermColor::iWHITE() << \"[ImageDataManager::rmImage] ns=\" << ns << \", t=\" << t << \".\";\n cout << \"FOUND, however its status is UNAVAILABLE or AVAILABLE_ON_DISK, this means it was previously removed or stashed. No action to take now.\" << TermColor::RESET() << endl;\n )\n return false;\n }\n\n }\n else\n {\n cout << TermColor::RED() << \"[ImageDataManager::rmImage] FATAL-ERROR you requested to remove ns=\" << ns << \", t=\" << t << \"; However it was not found on the map. FATAL ERRR\\n\" << TermColor::RESET();\n // exit(1);\n cout << \"[ImageDataManager::rmImage]No action taken for now.....\\n\";\n return false;\n }\n\n}\n\n// #define __ImageDataManager__stashImage(msg) msg;\n#define __ImageDataManager__stashImage(msg) ;\nbool ImageDataManager::stashImage( const string ns, const ros::Time t )\n{\n ensure_init();\n std::lock_guard lk(m);\n __ImageDataManager__stashImage(\n cout << TermColor::iCYAN() << \"[ImageDataManager::stashImage] ns=\" << ns << \", t=\" << t << TermColor::RESET() << endl;\n )\n auto key = std::make_pair( ns, t );\n if( status.count( key ) > 0 )\n {\n if( status.at(key) == MEMSTAT::AVAILABLE_ON_RAM )\n {\n // save to disk\n auto it_b = image_data.find( key );\n // string sfname = STASH_DIR+\"/\"+ns+\"__\"+to_string(t.toNSec())+\".jpg\";\n string sfname = key_to_imagename( ns, t );\n ElapsedTime elp; elp.tic();\n __ImageDataManager__stashImage(\n cout << TermColor::iCYAN() << \"[ImageDataManager] imwrite(\" << sfname << \")\\t elapsed_time_ms=\" << elp.toc_milli() << TermColor::RESET() << endl ;\n )\n\n // todo: tune jpg quality for saving on more disk space. default is 95/100 for jpg.\n if( it_b->second.type() == CV_16UC1 ) {\n // write PNG for depth image\n cv::imwrite( sfname+\".png\", it_b->second );\n }else {\n cv::imwrite( sfname, it_b->second );\n }\n\n // erase from map\n image_data.erase( it_b );\n\n // change status\n status[key] = MEMSTAT::AVAILABLE_ON_DISK;\n return true;\n } else {\n __ImageDataManager__stashImage(\n cout << TermColor::iCYAN() << \"[ImageDataManager::stashImage] warning status is not AVAILABLE_ON_RAM so do nothing. You asked me to stash an image which doesnt exists on RAM.\\n\" << TermColor::RESET();\n )\n return false;\n }\n }\n else\n {\n cout << TermColor::RED() << \"[ImageDataManager::stashImage] FATAL-ERROR you requested to stash ns=\" << ns << \", t=\" << t << \"; However it was not found on the map. FATAL ERRR\\n\" << TermColor::RESET();\n exit(1);\n }\n}\n\nbool ImageDataManager::isImageRetrivable( const string ns, const ros::Time t ) const\n{\n ensure_init();\n std::lock_guard lk(m);\n auto key = std::make_pair( ns, t );\n if( status.count( key ) > 0 ) {\n if(\n status.at( key ) == MEMSTAT::AVAILABLE_ON_RAM\n ||\n status.at( key ) == MEMSTAT::AVAILABLE_ON_DISK\n ||\n status.at( key ) == MEMSTAT::AVAILABLE_ON_RAM_DUETO_HIT\n )\n {\n return true;\n }\n else {\n cout << TermColor::iRED() << \"[ImageDataManager::isImageRetrivable] returning false because the requested (ns=\" << ns << \", t=\" << t << \") was on my list, but type was \"<< memstat_to_str( status.at( key ) ) << \", which is something other than AVAILABLE_ON_RAM or AVAILABLE_ON_DISK or AVAILABLE_ON_RAM_DUETO_HIT\" << TermColor::RESET() << endl;\n return false;\n }\n }\n else {\n cout << TermColor::iRED() << \"[ImageDataManager::isImageRetrivable] returning false because the requested (ns=\" << ns << \", t=\" << t << \") was not in my list\" << TermColor::RESET() << endl;\n return false;\n }\n}\n\n// _ImageDataManager_printstatus_cout cout\n#define _ImageDataManager_printstatus_cout myfile\nbool ImageDataManager::print_status( string fname ) const\n{\n // std::lock_guard lk(m); //no need of locks in a const function (readonly).\n\n ofstream myfile;\n myfile.open (fname);\n\n int AVAILABLE_ON_RAM=0, AVAILABLE_ON_DISK=0, UNAVAILABLE=0, ETC=0;\n for( auto it=status.begin() ; it!=status.end() ; it++ )\n {\n if( it->second == MEMSTAT::AVAILABLE_ON_RAM ) {\n _ImageDataManager_printstatus_cout << TermColor::iGREEN() << \" \" << TermColor::RESET();\n AVAILABLE_ON_RAM++;\n }\n else\n if( it->second == MEMSTAT::AVAILABLE_ON_DISK ) {\n _ImageDataManager_printstatus_cout << TermColor::iYELLOW() << \" \" << TermColor::RESET();\n AVAILABLE_ON_DISK++;\n }\n else\n if( it->second == MEMSTAT::UNAVAILABLE ) {\n _ImageDataManager_printstatus_cout << TermColor::iMAGENTA() << \" \" << TermColor::RESET();\n UNAVAILABLE++;\n }\n else {\n if( it->second == MEMSTAT::AVAILABLE_ON_RAM_DUETO_HIT )\n _ImageDataManager_printstatus_cout << TermColor::iBLUE() << std::setw(2) << hit_count.at(it->first) << TermColor::RESET();\n ETC++;\n }\n\n\n }\n _ImageDataManager_printstatus_cout << endl;\n _ImageDataManager_printstatus_cout << TermColor::iGREEN() << \"AVAILABLE_ON_RAM=\" << AVAILABLE_ON_RAM << \" \" << TermColor::RESET() ;\n _ImageDataManager_printstatus_cout << TermColor::iYELLOW() << \"AVAILABLE_ON_DISK=\" << AVAILABLE_ON_DISK << \" \" << TermColor::RESET();\n _ImageDataManager_printstatus_cout << TermColor::iMAGENTA() << \"UNAVAILABLE=\" << UNAVAILABLE << \" \" << TermColor::RESET();\n _ImageDataManager_printstatus_cout << TermColor::iBLUE() << \"ETC=\" << ETC << \" \" << TermColor::RESET();\n _ImageDataManager_printstatus_cout << \"TOTAL=\" << AVAILABLE_ON_RAM+AVAILABLE_ON_DISK+UNAVAILABLE+ETC << \" \";\n _ImageDataManager_printstatus_cout << endl;\n\n myfile.close();\n return true;\n}\n\nbool ImageDataManager::print_status( ) const\n{\n // std::lock_guard lk(m); //no need of locks in a const function (readonly).\n\n int AVAILABLE_ON_RAM=0, AVAILABLE_ON_DISK=0, UNAVAILABLE=0, ETC=0;\n for( auto it=status.begin() ; it!=status.end() ; it++ )\n {\n if( it->second == MEMSTAT::AVAILABLE_ON_RAM ) {\n cout << TermColor::iGREEN() << \" \" << TermColor::RESET();\n cout << std::get<0>(it->first) << \",\" << std::get<1>(it->first) << \": \" << \"AVAILABLE_ON_RAM\" << endl;\n AVAILABLE_ON_RAM++;\n }\n else\n if( it->second == MEMSTAT::AVAILABLE_ON_DISK ) {\n cout << TermColor::iYELLOW() << \" \" << TermColor::RESET();\n // cout << std::get<0>(it->first) << \",\" << std::get<1>(it->first) << \": \" << \"AVAILABLE_ON_DISK\" << endl;\n AVAILABLE_ON_DISK++;\n }\n else\n if( it->second == MEMSTAT::UNAVAILABLE ) {\n cout << TermColor::iMAGENTA() << \" \" << TermColor::RESET();\n // cout << std::get<0>(it->first) << \",\" << std::get<1>(it->first) << \": \" << \"UNAVAILABLE\" << endl;\n UNAVAILABLE++;\n }\n else {\n if( it->second == MEMSTAT::AVAILABLE_ON_RAM_DUETO_HIT )\n cout << TermColor::iBLUE() << std::setw(2) << hit_count.at(it->first) << TermColor::RESET();\n // cout << std::get<0>(it->first) << \",\" << std::get<1>(it->first) << \": \" << \"AVAILABLE_ON_RAM_DUETO_HIT\" << endl;\n ETC++;\n }\n\n\n }\n cout << endl;\n cout << TermColor::iGREEN() << \"AVAILABLE_ON_RAM=\" << AVAILABLE_ON_RAM << \" \" << TermColor::RESET() ;\n cout << TermColor::iYELLOW() << \"AVAILABLE_ON_DISK=\" << AVAILABLE_ON_DISK << \" \" << TermColor::RESET();\n cout << TermColor::iMAGENTA() << \"UNAVAILABLE=\" << UNAVAILABLE << \" \" << TermColor::RESET();\n cout << TermColor::iBLUE() << \"ETC=\" << ETC << \" \" << TermColor::RESET();\n cout << \"TOTAL=\" << AVAILABLE_ON_RAM+AVAILABLE_ON_DISK+UNAVAILABLE+ETC << \" \";\n cout << endl;\n\n\n return true;\n}\n\n\n// #define __ImageDataManager__decrement_hit_counts_and_deallocate_expired(msg) msg;\n#define __ImageDataManager__decrement_hit_counts_and_deallocate_expired(msg) ;\nint ImageDataManager::decrement_hit_counts_and_deallocate_expired()\n{\n std::lock_guard lk(m);\n\n // decrement all by 1\n int n_elements_in_hitlist = 0;\n for( auto it=hit_count.begin() ; it!=hit_count.end() ; it++ )\n {\n it->second--;\n __ImageDataManager__decrement_hit_counts_and_deallocate_expired(\n cout << \"[decrement_hit_counts_and_deallocate_expired]\" << std::get<0>(it->first) << \",\" << std::get<1>(it->first) << \" expry=\" << it->second << endl;\n )\n n_elements_in_hitlist++;\n }\n\n // deallocate expired\n int n_removed = 0;\n for( auto it=hit_count.begin() ; it!=hit_count.end() ; it++ )\n {\n if( it->second <= 0 )\n {\n __ImageDataManager__decrement_hit_counts_and_deallocate_expired(\n cout << \"[decrement_hit_counts_and_deallocate_expired] REMOVE: \" << std::get<0>(it->first) << \",\" << std::get<1>(it->first) << endl;\n )\n auto key = it->first;\n image_data.erase(key);\n hit_count.erase(key);\n status[key] = MEMSTAT::AVAILABLE_ON_DISK;\n n_removed++;\n }\n }\n\n __ImageDataManager__decrement_hit_counts_and_deallocate_expired(\n cout << \"[decrement_hit_counts_and_deallocate_expired] n_elements_in_hitlist=\" << n_elements_in_hitlist << \"\\tn_removed=\" << n_removed<< endl;\n )\n return n_removed;\n\n}\n\n\njson ImageDataManager::stashAll()\n{\n ensure_init();\n // std::lock_guard lk(m); //dont lock here as stashImage already locks. If you lock here it will cause a deadlock.\n json obj;\n\n // go over all and stash all\n int AVAILABLE_ON_RAM = 0;\n int AVAILABLE_ON_DISK = 0;\n int UNAVAILABLE = 0;\n int AVAILABLE_ON_RAM_DUETO_HIT=0;\n int n_stashed = 0;\n for( auto it=status.begin() ; it!=status.end() ; it++ )\n {\n string __ns_x = std::get<0>(it->first);\n ros::Time __t_x = std::get<1>(it->first);\n\n if( it->second == MEMSTAT::AVAILABLE_ON_RAM ) {\n // cout << \"[ImageDataManager::stashAll] stash( \" << __ns_x << \", \" << __t_x << \" )\\n\";\n stashImage( __ns_x, __t_x );\n n_stashed++;\n }\n\n json this_json;\n this_json[\"namespace\"] = __ns_x;\n // this_json[\"stamp\"] = __t_x.toSec(); // this is buggy, have overflow, better stick to using Nsec int64_t\n this_json[\"stampNSec\"] = __t_x.toNSec();\n\n if( it->second == MEMSTAT::AVAILABLE_ON_RAM ) {\n this_json[\"status\"] = \"AVAILABLE_ON_RAM\";\n AVAILABLE_ON_RAM++;\n }\n else\n if( it->second == MEMSTAT::AVAILABLE_ON_DISK ) {\n this_json[\"status\"] = \"AVAILABLE_ON_DISK\";\n AVAILABLE_ON_DISK++;\n }\n else\n if( it->second == MEMSTAT::UNAVAILABLE ) {\n this_json[\"status\"] = \"UNAVAILABLE\";\n UNAVAILABLE++;\n }\n else {\n if( it->second == MEMSTAT::AVAILABLE_ON_RAM_DUETO_HIT )\n this_json[\"status\"] = \"AVAILABLE_ON_RAM_DUETO_HIT\";\n AVAILABLE_ON_RAM_DUETO_HIT++;\n }\n obj.push_back( this_json );\n\n }\n\n cout << \"[ImageDataManager::stashAll]\";\n cout << \"AVAILABLE_ON_RAM=\" << AVAILABLE_ON_RAM << \"\\t\";\n cout << \"AVAILABLE_ON_DISK=\" << AVAILABLE_ON_DISK << \"\\t\";\n cout << \"UNAVAILABLE=\" << UNAVAILABLE << \"\\t\";\n cout << \"AVAILABLE_ON_RAM_DUETO_HIT=\" << AVAILABLE_ON_RAM_DUETO_HIT << \"\\t\";\n cout << \"n_stashed=\" << n_stashed << \"\\t\";\n cout << endl;\n return obj;\n\n\n\n}\n\n\n\n//\n// \"ImageDataManager\": [\n// {\n// \"namespace\": \"left_image\",\n// \"stamp\": 1542707155.6136055,\n// \"status\": \"AVAILABLE_ON_DISK\"\n// },\n// {\n// .\n// },\n// .\n// .\n// .\n// ]\nbool ImageDataManager::loadStateFromDisk( const json json_obj )\n{\n cout << \"^^^^^^^^ IN [ImageDataManager::loadStateFromDisk] ^^^^^^^^^^\\n\";\n ensure_init();\n // loop thru json object and create the map\n int n_images = json_obj.size();\n cout << \"[ImageDataManager::loadStateFromDisk] n_images=\" << n_images << endl;\n\n int AVAILABLE_ON_DISK = 0;\n int UNAVAILABLE = 0;\n for( int i=0 ; i\n// Created : 12th June, 2019\n//\n\n#include \n#include \n#include \n#include \n#include //std::pair\n#include \n\n// threading\n#include \n#include \n#include \n\n//opencv\n#include \n#include \n#include \n#include \n\n#include \n\n#include \"utils/TermColor.h\"\n#include \"utils/ElapsedTime.h\"\n#include \"utils/RawFileIO.h\"\n\n\n#include \"utils/nlohmann/json.hpp\"\nusing json = nlohmann::json;\n\nusing namespace std;\n\nenum MEMSTAT { AVAILABLE_ON_RAM, AVAILABLE_ON_DISK, UNAVAILABLE, AVAILABLE_ON_RAM_DUETO_HIT };\n\nclass ImageDataManager\n{\npublic:\n ImageDataManager();\n bool initStashDir( bool clear_dir, const string __dir=string(\"/tmp/cerebro_stash/\") ); //< clear_dir: setting this to true will cause `rm -rf DIR && mkdir DIR` ;;;__dir: The directory usually set it to '/tmp/cerebro_stash' .\n\n ~ImageDataManager();\n bool setImage( const string ns, const ros::Time t, const cv::Mat img );\n bool setNewImageFromMsg( const string ns, const sensor_msgs::ImageConstPtr msg );\n bool getImage( const string ns, const ros::Time t, cv::Mat& outImg );\n\n bool rmImage( const string ns, const ros::Time t );\n bool stashImage( const string ns, const ros::Time t );\n\n bool isImageRetrivable( const string ns, const ros::Time t ) const;\n\n bool print_status( string fname ) const;\n bool print_status( ) const;\n\n // - go over all status and stash all the images that remain on RAM.\n // Also retuns the map status as json object\n json stashAll();\n bool loadStateFromDisk( const json json_obj );\n\n\nprivate:\n mutable std::mutex m;\n string STASH_DIR = string(\"\"); bool m_STASH_DIR = false;\n const string key_to_imagename( const string ns, const ros::Time t ) const\n {\n return STASH_DIR+\"/\"+ns+\"__\"+to_string(t.toNSec())+\".jpg\";\n }\n\n const string memstat_to_str( MEMSTAT m ) const\n {\n if( m == MEMSTAT::AVAILABLE_ON_RAM )\n return \"AVAILABLE_ON_RAM\";\n\n if( m == MEMSTAT::AVAILABLE_ON_DISK )\n return \"AVAILABLE_ON_DISK\";\n\n if( m == MEMSTAT::UNAVAILABLE )\n return \"UNAVAILABLE\";\n\n if( m == MEMSTAT::AVAILABLE_ON_RAM_DUETO_HIT )\n return \"AVAILABLE_ON_RAM_DUETO_HIT\";\n\n return \"N.A.\";\n }\n\n // key: (namespace, t)\n std::map< std::pair, MEMSTAT > status; //status at each timestamp (entris not be esared)\n std::map< std::pair, cv::Mat > image_data;\n\n\n // when getImage finds that AVAILABLE_ON_DISK, it loads the image (from disk) in the map image_data.\n // Also it stores say 10 in this map. This 10 means that the image be deleted again if 10 consecutive\n // getImage requests dont request this image.\n // CACHE-Algorithm: 5-minute-rule.\n std::map< std::pair, int > hit_count;\n int decrement_hit_counts_and_deallocate_expired(); //returns how many expired\n\n\n bool rm_stash_dir_in_destructor = true;\n\npublic:\n const string getStashDir() const { return STASH_DIR; }\n void set_rm_stash_dir_in_destructor_as_false() {rm_stash_dir_in_destructor=false; }\n void set_rm_stash_dir_in_destructor_as_true() {rm_stash_dir_in_destructor=true; }\n\n void ensure_init() const {\n if( !m_STASH_DIR) {\n ROS_ERROR( \"m_STASH_DIR is false. This means you have not initialized the ImageDataManager. You need to call the function initStashDir() befre you can start using it\\n\");\n exit(1);\n }\n }\n};\n" }, { "path": "src/PNPCeresCostFunctions.h", "content": "#pragma once\n\n//ceres\n#include \n\ntemplate \nT NormalizeAngle(const T& angle_degrees) {\n if (angle_degrees > T(180.0))\n \treturn angle_degrees - T(360.0);\n else if (angle_degrees < T(-180.0))\n \treturn angle_degrees + T(360.0);\n else\n \treturn angle_degrees;\n};\n\nclass AngleLocalParameterization {\n public:\n\n template \n bool operator()(const T* theta_radians, const T* delta_theta_radians,\n T* theta_radians_plus_delta) const {\n *theta_radians_plus_delta =\n NormalizeAngle(*theta_radians + *delta_theta_radians);\n\n return true;\n }\n\n static ceres::LocalParameterization* Create() {\n return (new ceres::AutoDiffLocalParameterization);\n }\n};\n\ntemplate \nvoid YawPitchRollToRotationMatrix(const T yaw, const T pitch, const T roll, T R[9])\n{\n\n\tT y = yaw / T(180.0) * T(M_PI);\n\tT p = pitch / T(180.0) * T(M_PI);\n\tT r = roll / T(180.0) * T(M_PI);\n\n\n\tR[0] = cos(y) * cos(p);\n\tR[1] = -sin(y) * cos(r) + cos(y) * sin(p) * sin(r);\n\tR[2] = sin(y) * sin(r) + cos(y) * sin(p) * cos(r);\n\tR[3] = sin(y) * cos(p);\n\tR[4] = cos(y) * cos(r) + sin(y) * sin(p) * sin(r);\n\tR[5] = -cos(y) * sin(r) + sin(y) * sin(p) * cos(r);\n\tR[6] = -sin(p);\n\tR[7] = cos(p) * sin(r);\n\tR[8] = cos(p) * cos(r);\n};\n\ntemplate \nvoid RotationMatrixTranspose(const T R[9], T inv_R[9])\n{\n\tinv_R[0] = R[0];\n\tinv_R[1] = R[3];\n\tinv_R[2] = R[6];\n\tinv_R[3] = R[1];\n\tinv_R[4] = R[4];\n\tinv_R[5] = R[7];\n\tinv_R[6] = R[2];\n\tinv_R[7] = R[5];\n\tinv_R[8] = R[8];\n};\n\ntemplate \nvoid RotationMatrixRotatePoint(const T R[9], const T t[3], T r_t[3])\n{\n\tr_t[0] = R[0] * t[0] + R[1] * t[1] + R[2] * t[2];\n\tr_t[1] = R[3] * t[0] + R[4] * t[1] + R[5] * t[2];\n\tr_t[2] = R[6] * t[0] + R[7] * t[1] + R[8] * t[2];\n};\n\nclass PNPEulerAngleError\n{\npublic:\n PNPEulerAngleError( Vector3d _w_X, Vector2d _uv_normed_cords ): w_X(_w_X), uv_normed_cords(_uv_normed_cords)\n {}\n\n // // minimize_{R,t} \\sum_i ( PI( c_(R|t)_w * w_X[i] ) - u[i] )\n\n template \n bool operator()( const T* yaw, const T* pitch, const T* roll, const T* tx, const T* ty , const T* tz, T * residue ) const\n {\n T R[9]; // c_T_a.\n YawPitchRollToRotationMatrix( yaw[0], pitch[0], roll[0], R );\n\n T out[3];\n out[0] = R[0] * T(w_X(0)) + R[1] * T(w_X(1)) + R[2] * T(w_X(2)) + tx[0];\n out[1] = R[3] * T(w_X(0)) + R[4] * T(w_X(1)) + R[5] * T(w_X(2)) + ty[0];\n out[2] = R[6] * T(w_X(0)) + R[7] * T(w_X(1)) + R[8] * T(w_X(2)) + tz[0];\n\n\n // Simple\n residue[0] = out[0] / out[2] - T(uv_normed_cords(0));\n residue[1] = out[1] / out[2] - T(uv_normed_cords(1));\n\n // Weight by Z. A point that is far away need to be down weighted\n // just search plot 1/ (1 + exp( x-10) ) on google. also called sigmoid function\n T wt = T( 1.0 / ( 1.0 + exp(w_X(2) - 10.) ) );\n residue[0] *= wt;\n residue[1] *= wt;\n\n return true;\n }\n\n static ceres::CostFunction* Create( Vector3d _w_X, Vector2d _uv_normed_cords )\n\t{\n\t return (new ceres::AutoDiffCostFunction<\n\t PNPEulerAngleError, 2, 1,1,1, 1,1,1 >(\n\t \tnew PNPEulerAngleError( _w_X, _uv_normed_cords ) ) );\n\t}\n\n\nprivate:\n const Vector3d w_X;\n const Vector2d uv_normed_cords;\n};\n\n\n\nclass P3PEulerAngleError\n{\npublic:\n // [Input]:\n // _a_X: 3dpts expressed in co-ordinate frame of a\n // _b_X: 3dpts expressed in co-ordinate frame of b\n P3PEulerAngleError( Vector3d _a_X, Vector3d _b_X ): a_X(_a_X), b_X(_b_X)\n {}\n\n // // minimize_{R,t} \\sum_i b_(R|t)_a * a_X[i] - b_X[i]\n\n template \n bool operator()( const T* yaw, const T* pitch, const T* roll, const T* tx, const T* ty , const T* tz, T * residue ) const\n {\n T R[9]; // b_T_a.\n YawPitchRollToRotationMatrix( yaw[0], pitch[0], roll[0], R );\n\n T out[3];\n out[0] = R[0] * T(a_X(0)) + R[1] * T(a_X(1)) + R[2] * T(a_X(2)) + tx[0];\n out[1] = R[3] * T(a_X(0)) + R[4] * T(a_X(1)) + R[5] * T(a_X(2)) + ty[0];\n out[2] = R[6] * T(a_X(0)) + R[7] * T(a_X(1)) + R[8] * T(a_X(2)) + tz[0];\n\n\n // Simple\n residue[0] = out[0] - T(b_X(0));\n residue[1] = out[1] - T(b_X(1));\n residue[2] = out[2] - T(b_X(2));\n\n\n // Weight by Z. A point that is far away need to be down weighted\n // just search plot 1/ (1 + exp( x-10) ) on google. also called sigmoid function\n T wt = T( 1.0 / ( 1.0 + exp(a_X(2) - 10.) ) );\n residue[0] *= wt;\n residue[1] *= wt;\n residue[2] *= wt;\n\n return true;\n }\n\n static ceres::CostFunction* Create( Vector3d _a_X, Vector3d _b_X )\n\t{\n\t return (new ceres::AutoDiffCostFunction<\n\t P3PEulerAngleError, 3, 1,1,1, 1,1,1 >(\n\t \tnew P3PEulerAngleError( _a_X, _b_X ) ) );\n\t}\n\n\nprivate:\n const Vector3d a_X;\n const Vector3d b_X;\n};\n\n\n/*\nstruct PNPFourDOFError\n{\n\tPNPFourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)\n\t\t\t\t :t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){}\n\n\ttemplate \n\tbool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const\n\t{\n\t\tT t_w_ij[3];\n\t\tt_w_ij[0] = tj[0] - ti[0];\n\t\tt_w_ij[1] = tj[1] - ti[1];\n\t\tt_w_ij[2] = tj[2] - ti[2];\n\n\t\t// euler to rotation\n\t\tT w_R_i[9];\n\t\tYawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);\n\t\t// rotation transpose\n\t\tT i_R_w[9];\n\t\tRotationMatrixTranspose(w_R_i, i_R_w);\n\t\t// rotation matrix rotate point\n\t\tT t_i_ij[3];\n\t\tRotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);\n\n\t\tresiduals[0] = (t_i_ij[0] - T(t_x));\n\t\tresiduals[1] = (t_i_ij[1] - T(t_y));\n\t\tresiduals[2] = (t_i_ij[2] - T(t_z));\n\t\tresiduals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));\n\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,\n\t\t\t\t\t\t\t\t\t const double relative_yaw, const double pitch_i, const double roll_i)\n\t{\n\t return (new ceres::AutoDiffCostFunction<\n\t PNPFourDOFError, 4, 1, 3, 1, 3>(\n\t \tnew PNPFourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));\n\t}\n\n\tdouble t_x, t_y, t_z;\n\tdouble relative_yaw, pitch_i, roll_i;\n\n};\n*/\n" }, { "path": "src/PinholeCamera.cpp", "content": "#include \"PinholeCamera.h\"\n\n\nPinholeCamera::PinholeCamera( string config_file )\n{\n\n cv::FileStorage fs( config_file, cv::FileStorage::READ );\n if( !fs.isOpened() )\n {\n ROS_ERROR( \"Cannot open config file : %s\", config_file.c_str() );\n ROS_ERROR( \"Quit\");\n mValid = false;\n exit(1);\n }\n this->config_file_name = string(config_file);\n\n cout << \"---Camera Config---\\n\";\n fs[\"model_type\"] >> config_model_type; cout << \"config_model_type:\"<< config_model_type << endl;\n fs[\"camera_name\"] >> config_camera_name; cout << \"config_camera_name:\" << config_camera_name << endl;\n fs[\"image_width\"] >> config_image_width; cout << \"config_image_width:\" << config_image_width << endl;\n fs[\"image_height\"] >> config_image_height; cout << \"config_image_height:\" << config_image_height << endl;\n // assert( config_model_type == \"PINHOLE\" && (bool)\"The class PinholeCamera is designed only for PINHOLE camera model. In this config_file: \"+config_file + \" the camera model was \"+config_model_type+\" this was not expected\\n\" );\n\n fs[\"projection_parameters\"][\"fx\"] >> _fx;\n fs[\"projection_parameters\"][\"fy\"] >> _fy;\n fs[\"projection_parameters\"][\"cx\"] >> _cx;\n fs[\"projection_parameters\"][\"cy\"] >> _cy;\n\n cout << \"projection_parameters :: \" << _fx << \" \" << _fy << \" \" << _cx << \" \" << _cy << \" \" << endl;\n\n fs[\"distortion_parameters\"][\"k1\"] >> _k1;\n fs[\"distortion_parameters\"][\"k2\"] >> _k2;\n fs[\"distortion_parameters\"][\"p1\"] >> _p1;\n fs[\"distortion_parameters\"][\"p2\"] >> _p2;\n cout << \"distortion_parameters :: \" << _k1 << \" \" << _k2 << \" \" << _p1 << \" \" << _p2 << \" \" << endl;\n cout << \"--- ---\\n\";\n\n // Define the 3x3 Projection matrix eigen and/or cv::Mat.\n m_K = cv::Mat::zeros( 3,3,CV_32F );\n m_K.at(0,0) = _fx;\n m_K.at(1,1) = _fy;\n m_K.at(0,2) = _cx;\n m_K.at(1,2) = _cy;\n m_K.at(2,2) = 1.0;\n\n m_D = cv::Mat::zeros( 4, 1, CV_32F );\n m_D.at(0,0) = _k1;\n m_D.at(1,0) = _k2;\n m_D.at(2,0) = _p1;\n m_D.at(3,0) = _p2;\n cout << \"m_K\" << m_K << endl;\n cout << \"m_D\" << m_D << endl;\n\n // Define 4x1 vector of distortion params eigen and/or cv::Mat.\n e_K << _fx, 0.0, _cx,\n 0.0, _fy, _cy,\n 0.0, 0.0, 1.0;\n\n e_D << _k1 , _k2, _p1 , _p2;\n cout << \"e_K:\\n\" << e_K << endl;\n cout << \"e_D:\\n\" << e_D << endl;\n mValid = true;\n\n}\n\n\nPinholeCamera::PinholeCamera( double __fx, double __fy, double __cx, double __cy,\n int __im_rows, int __im_cols,\n double __k1, double __k2, double __p1, double __p2\n ): _fx( __fx ), _fy( __fy ), _cx( __cx ), _cy( __cy ),\n config_image_width( __im_cols ), config_image_height( __im_rows ),\n _k1( __k1 ), _k2( __k2 ), _p1( __p1 ), _p2( __p2 )\n{\n ROS_WARN( \"[PinholeCamera::PinholeCamera] Setting camera intrinsics explicitly.....\\n\");\n\n // Define the 3x3 Projection matrix eigen and/or cv::Mat.\n m_K = cv::Mat::zeros( 3,3,CV_32F );\n m_K.at(0,0) = _fx;\n m_K.at(1,1) = _fy;\n m_K.at(0,2) = _cx;\n m_K.at(1,2) = _cy;\n m_K.at(2,2) = 1.0;\n\n m_D = cv::Mat::zeros( 4, 1, CV_32F );\n m_D.at(0,0) = _k1;\n m_D.at(1,0) = _k2;\n m_D.at(2,0) = _p1;\n m_D.at(3,0) = _p2;\n cout << \"m_K\" << m_K << endl;\n cout << \"m_D\" << m_D << endl;\n\n // Define 4x1 vector of distortion params eigen and/or cv::Mat.\n e_K << _fx, 0.0, _cx,\n 0.0, _fy, _cy,\n 0.0, 0.0, 1.0;\n\n e_D << _k1 , _k2, _p1 , _p2;\n cout << \"e_K:\\n\" << e_K << endl;\n cout << \"e_D:\\n\" << e_D << endl;\n mValid = true;\n\n\n}\n\n\nvoid PinholeCamera::printCameraInfo( int verbosity ) const\n{\n cout << \"====== PinholeCamera::printCameraInfo ======\\n\";\n if( !isValid() )\n cout << \"camera not set\\n\";\n\n cout << \"config_file: \" << this->config_file_name << endl;\n cout << \"Image Rows: \" << getImageRows() << endl;\n cout << \"Image Cols: \" << getImageCols() << endl;\n cout << \"fx: \" << fx() << \" ; fy: \" << fy() << endl;\n cout << \"cx: \" << cx() << \" ; cy: \" << cy() << endl;\n cout << \"(distortion params)k1,k2,p1,p2\" << \" \" << k1() << \" \" << k2() << \" \" << p1() << \" \" << p2() << endl;\n\n if( verbosity >= 1 ) {\n cout << \"--cv::Mat \\n\";\n cout << \"m_K\\n\" << m_K << endl;\n cout << \"m_D\\n\" << m_D << endl;\n\n cout << \"--Eigen::Matrix \\n\";\n cout << \"e_K\\n\" << e_K << endl;\n cout << \"e_K\\n\" << e_D << endl;\n }\n\n cout << \"================================================\\n\";\n\n}\n\nstring PinholeCamera::getCameraInfoAsJson() const\n{\n if( !isValid() )\n return string(\"{\\\"isValid\\\": 1}\\n\");\n std::stringstream buffer;\n buffer << \"{\\n\";\n buffer << \"\\\"config_file\\\": \\\"\" << this->config_file_name << \"\\\"\"<< endl;\n buffer << \",\\\"image_rows\\\": \" << getImageRows() << endl;\n buffer << \",\\\"image_cols\\\": \" << getImageCols() << endl;\n buffer << \",\\\"fx\\\": \" << fx() << \" \\n,fy: \" << fy() << endl;\n buffer << \",\\\"cx\\\": \" << cx() << \" \\n,cy: \" << cy() << endl;\n buffer << \",\\\"k1\\\": \" << k1() << endl;\n buffer << \",\\\"k2\\\": \" << k2() << endl;\n buffer << \",\\\"p1\\\": \" << p1() << endl;\n buffer << \",\\\"p2\\\": \" << p2() << endl;\n buffer << \"}\\n\" ;\n return buffer.str();\n}\n\n\nvoid PinholeCamera::print_cvmat_info( string msg, const cv::Mat& A )\n{\n cout << msg << \":\" << \"rows=\" << A.rows << \", cols=\" << A.cols << \", ch=\" << A.channels() << \", type=\" << type2str( A.type() ) << endl;\n}\n\nstring PinholeCamera::type2str(int type) {\n string r;\n\n uchar depth = type & CV_MAT_DEPTH_MASK;\n uchar chans = 1 + (type >> CV_CN_SHIFT);\n\n switch ( depth ) {\n case CV_8U: r = \"8U\"; break;\n case CV_8S: r = \"8S\"; break;\n case CV_16U: r = \"16U\"; break;\n case CV_16S: r = \"16S\"; break;\n case CV_32S: r = \"32S\"; break;\n case CV_32F: r = \"32F\"; break;\n case CV_64F: r = \"64F\"; break;\n default: r = \"User\"; break;\n }\n\n r += \"C\";\n r += (chans+'0');\n\n return r;\n}\n\n\n// Input 3d points in homogeneous co-ordinates 4xN matrix. Eigen I/O\nvoid PinholeCamera::perspectiveProject3DPoints( const MatrixXd& c_X,\n MatrixXd& out_pts )\n{\n\n // DIY - Do It Yourself Projection\n // c_X.row(0).array() /= c_X.row(3).array();\n // c_X.row(1).array() /= c_X.row(3).array();\n // c_X.row(2).array() /= c_X.row(3).array();\n // c_X.row(3).array() /= c_X.row(3).array();\n\n\n\n // K [ I | 0 ]\n MatrixXd I_0;\n I_0 = Matrix4d::Identity().topLeftCorner<3,4>();\n // MatrixXf P1;\n // P1 = cam_intrin * I_0; //3x4\n\n // Project and Perspective Divide\n MatrixXd im_pts;\n im_pts = I_0 * c_X; //in normalized image co-ordinate. Beware that distortion need to be applied in normalized co-ordinates\n im_pts.row(0).array() /= im_pts.row(2).array();\n im_pts.row(1).array() /= im_pts.row(2).array();\n im_pts.row(2).array() /= im_pts.row(2).array();\n\n // Apply Distortion\n MatrixXd Xdd = MatrixXd( im_pts.rows(), im_pts.cols() );\n for( int i=0 ; i\n Created : 3rd Oct, 2017\n Modified: 26th Oct, 2018\n*/\n\n\n#include \n#include \n#include \n\n\n//opencv\n#include \n#include \n#include \n#include \n\n//ros\n#include \n#include \n\n\n// Eigen3\n#include \n#include \nusing namespace Eigen;\n#include \n\nusing namespace std;\n\n\n\nclass PinholeCamera {\n\npublic:\n PinholeCamera() { mValid = false; }\n PinholeCamera( string config_file );\n PinholeCamera( double __fx, double __fy, double __cx, double __cy,\n int __im_rows, int __im_cols,\n double __k1, double __k2, double __p1, double __p2\n );\n\n void printCameraInfo( int verbosity=2 ) const;\n string getCameraInfoAsJson() const;\n\n\n const cv::Mat get_mK() const {return m_K; }\n const cv::Mat get_mD() const {return m_D; }\n\n const Matrix3d get_eK() const {return e_K; }\n const Vector4d get_eD() const {return e_D; }\n\n // have a const after function, signals the compiler that these functions will not attempt to modify the variables inside\n bool isValid() const { return mValid; }\n double fx() const { return _fx; }\n double fy() const { return _fy; }\n double cx() const { return _cx; }\n double cy() const { return _cy; }\n double k1() const { return _k1; }\n double k2() const { return _k2; }\n double p1() const { return _p1; }\n double p2() const { return _p2; }\n\n string getModelType() const { return config_model_type; }\n string getCameraName() const { return config_camera_name; }\n string getConfigFileName() const { return config_file_name; }\n int getImageWidth() const { return config_image_width; }\n int getImageHeight() const { return config_image_height; }\n\n int getImageRows() const { return this->getImageHeight(); }\n int getImageCols() const { return this->getImageWidth(); }\n\n\n // Projection\n // Input 3d points in homogeneous co-ordinates 4xN matrix. Eigen I/O.\n // uses the camera matrix and D from this class\n void perspectiveProject3DPoints( const MatrixXd& c_X, MatrixXd& out_pts );\n\nprivate:\n string config_model_type, config_camera_name;\n string config_file_name;\n int config_image_width, config_image_height;\n\n double _fx, _fy, _cx, _cy;\n double _k1, _k2, _p1, _p2;\n\n bool mValid;\n\n void print_cvmat_info( string msg, const cv::Mat& A );\n string type2str(int type);\n\n cv::Mat m_K; //3x3\n cv::Mat m_D; //4x1\n\n Matrix3d e_K;\n Vector4d e_D;\n\n\n};\n" }, { "path": "src/ProcessedLoopCandidate.cpp", "content": "#include \"ProcessedLoopCandidate.h\"\n\nProcessedLoopCandidate::ProcessedLoopCandidate( int idx_from_raw_candidates_list,\n DataNode * node_1, DataNode * node_2 )\n{\n // cout << \"ProcessedLoopCandidate::ProcessedLoopCandidate\\n\";\n this->node_1 = node_1;\n this->node_2 = node_2;\n this->idx_from_raw_candidates_list = idx_from_raw_candidates_list;\n\n opX_b_T_a.clear();\n debug_images.clear();\n}\n\n\nbool ProcessedLoopCandidate::makeLoopEdgeMsg( cerebro::LoopEdge& msg )\n{\n if( isSet_3d2d__2T1 == false ) {\n cout << TermColor::RED() << \"[ProcessedLoopCandidate::makeLoopEdgeMsg]\\\n You asked me to make cerebro::LoopEdge of ProcessedLoopCandidate in\\\n which there is no valid pose data\" << TermColor::RESET() << endl;\n return false;\n }\n msg.timestamp0 = node_1->getT();\n msg.timestamp1 = node_2->getT();\n\n geometry_msgs::Pose pose;\n PoseManipUtils::eigenmat_to_geometry_msgs_Pose( _3d2d__2T1, pose );\n msg.pose_1T0 = pose;\n\n msg.weight = _3d2d__2T1__ransac_confidence; //1.0;\n msg.description = to_string(idx_from_datamanager_1)+\"<=>\"+to_string(idx_from_datamanager_2);\n msg.description += \" this pose is: \"+to_string(idx_from_datamanager_2)+\"_T_\"+to_string(idx_from_datamanager_1);\n\n return true;\n}\n\n// #define __ProcessedLoopCandidate_makeLoopEdgeMsgWithConsistencyCheck( msg ) msg;\n#define __ProcessedLoopCandidate_makeLoopEdgeMsgWithConsistencyCheck( msg ) ;\nbool ProcessedLoopCandidate::makeLoopEdgeMsgWithConsistencyCheck( cerebro::LoopEdge& msg )\n{\n if( opX_b_T_a.size() != 3 ) {\n cout << TermColor::RED() << \"[ProcessedLoopCandidate::makeLoopEdgeMsgWithConsistencyCheck] \\\n I was expecting only 3 candidates\" << TermColor::RED() << endl;\n return false;\n }\n\n\n ros::Duration diff = node_1->getT() - node_2->getT();\n if( abs(diff.sec) < 10 ) // the candidates are too near in time, so ignore them\n {\n __ProcessedLoopCandidate_makeLoopEdgeMsgWithConsistencyCheck(\n cout << \"[ ProcessedLoopCandidate::makeLoopEdgeMsgWithConsistencyCheck] abs(diff.sec) < 10. diff= \"<< diff << endl;\n )\n return false;\n }\n\n ///////////////////////////////////////////////////////////\n // Look at all the options and determine if they are consistent.\n\n\n // if |del_angle| < 2.0 degrees and |del_translation| < 0.1m then not consistent\n Matrix4d op1__b_T_a = opX_b_T_a[0];\n Matrix4d op2__b_T_a = opX_b_T_a[1];\n Matrix4d icp_b_T_a = opX_b_T_a[2];\n Matrix4d op1_m_op2 = op1__b_T_a.inverse() * op2__b_T_a;\n Matrix4d op1_m_icp = op1__b_T_a.inverse() * icp_b_T_a;\n Matrix4d op2_m_icp = op2__b_T_a.inverse() * icp_b_T_a;\n Vector3d op1_m_op2_ypr, op1_m_op2_tr; // ypr and translation of delta pose\n Vector3d op1_m_icp_ypr, op1_m_icp_tr;\n Vector3d op2_m_icp_ypr,op2_m_icp_tr;\n PoseManipUtils::eigenmat_to_rawyprt( op1_m_op2, op1_m_op2_ypr, op1_m_op2_tr);\n PoseManipUtils::eigenmat_to_rawyprt( op1_m_icp, op1_m_icp_ypr, op1_m_icp_tr);\n PoseManipUtils::eigenmat_to_rawyprt( op2_m_icp, op2_m_icp_ypr, op2_m_icp_tr);\n bool is_consistent_ypr = false, is_consistent_tr=false;\n\n if( op1_m_op2_ypr.lpNorm() < 5.0 &&\n op1_m_icp_ypr.lpNorm() < 5.0 &&\n op2_m_icp_ypr.lpNorm() < 5.0\n )\n { is_consistent_ypr = true;}\n\n if( op1_m_icp_tr.lpNorm() < .2 &&\n op1_m_icp_tr.lpNorm() < .2 &&\n op2_m_icp_tr.lpNorm() < .2\n )\n { is_consistent_tr = true;}\n\n #if 0 //just priniting\n __ProcessedLoopCandidate_makeLoopEdgeMsgWithConsistencyCheck(\n Matrix4d odom_b_T_a = node_2->getPose().inverse() * node_1->getPose();\n cout << \"---\" << to_string(idx_from_datamanager_1)+\"<=>\"+to_string(idx_from_datamanager_2);\n cout << \"[ProcessedLoopCandidate::makeLoopEdgeMsgWithConsistencyCheck]\\n\" << TermColor::YELLOW() << \"odom_b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) << TermColor::RESET() << endl;\n\n cout << \"op1\" << \"confidence=\" << opX_goodness[0]<< \" \" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a ) << endl;\n cout << \"op2\" << \"confidence=\" << opX_goodness[1]<< \" \" << PoseManipUtils::prettyprintMatrix4d( op2__b_T_a ) << endl;\n cout << \"icp\" << \"confidence=\" << opX_goodness[2]<< \" \" << PoseManipUtils::prettyprintMatrix4d( icp_b_T_a ) << endl;\n\n cout << \"|op1 - op2|\" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a.inverse() * op2__b_T_a ) << endl;\n cout << \"|op1 - icp|\" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a.inverse() * icp_b_T_a ) << endl;\n cout << \"|op2 - icp|\" << PoseManipUtils::prettyprintMatrix4d( op2__b_T_a.inverse() * icp_b_T_a ) << endl;\n cout << TermColor::YELLOW() ;\n cout << \"is_consistent_ypr=\" << is_consistent_ypr << \"\\t\" ;\n cout << \"is_consistent_tr=\" << is_consistent_tr << \"\\t\";\n cout << TermColor::RESET() << endl;\n )\n #endif\n\n\n ///////////////////////////////// Done with consistency check /////////////////////\n\n if( pf_matches > 800 && (is_consistent_ypr && is_consistent_tr) ) {\n // put the final pose into `3d2d__2T1`\n _3d2d__2T1 = opX_b_T_a[0];\n isSet_3d2d__2T1 = true;\n _3d2d__2T1__ransac_confidence = max( max( opX_goodness[0], opX_goodness[1] ), opX_goodness[2] );\n\n // call the above function,\n return makeLoopEdgeMsg( msg );\n }\n else {\n cout << \"[ ProcessedLoopCandidate::makeLoopEdgeMsgWithConsistencyCheck] Insonsistent poses from 3 methods, so don't publish this pose\" << endl;\n return false;\n }\n}\n\n\nbool ProcessedLoopCandidate::asJson( json& out_json )\n{\n json x;\n if( node_1 == NULL || node_2 == NULL )\n return false;\n\n x[\"timestamp_a\"] = node_1->getT().toSec();\n x[\"timestamp_b\"] = node_2->getT().toSec();\n x[\"idx_a\"] = idx_from_datamanager_1;\n x[\"idx_b\"] = idx_from_datamanager_2;\n x[\"pf_matches\"] = pf_matches;\n x[\"final_pose_available\"] = isSet_3d2d__2T1;\n\n if( isSet_3d2d__2T1 ) {\n x[\"3d2d__2T1\"] = PoseManipUtils::prettyprintMatrix4d( _3d2d__2T1 );\n x[\"_3d2d__2T1__ransac_confidence\"] = _3d2d__2T1__ransac_confidence;\n }\n\n\n // write the poses which were computed with various methods\n for( int i=0 ; i\n#include \n#include \n#include \n\n\n//opencv\n#include \n#include \n#include \n#include \n\n// ros\n// #include \n// #include \n\n// ros msg\n// #include \n// #include \n// #include \n// #include \n// #include \n// #include \n// #include \n// // #include \n// #include \n// #include \n// #include \n\n// Eigen\n#include \n#include \n#include \nusing namespace Eigen;\n#include \n\n#include \"DataNode.h\"\n#include \"utils/PoseManipUtils.h\"\n// #include \"utils/MiscUtils.h\"\n#include \"utils/TermColor.h\"\n\n#include \"../utils/nlohmann/json.hpp\"\nusing json = nlohmann::json;\n\n// Publishing message\n#include \n\nclass ProcessedLoopCandidate\n{\npublic:\n ProcessedLoopCandidate( int idx_from_raw_candidates_list, DataNode * node_1, DataNode * node_2 );\n ProcessedLoopCandidate() { idx_from_raw_candidates_list=-1; opX_b_T_a.clear(); debug_images.clear(); }\n\n bool makeLoopEdgeMsg( cerebro::LoopEdge& msg );\n\n // Looks at all the candidate relative poses and decides if they are all consistent.\n // Returns true if consistent, returns false if not consistent\n bool makeLoopEdgeMsgWithConsistencyCheck( cerebro::LoopEdge& msg );\n\n bool asJson( json& out_json );\n\n// private:\n DataNode * node_1;\n DataNode * node_2;\n\n int idx_from_datamanager_1;\n int idx_from_datamanager_2;\n\n int idx_from_raw_candidates_list;\n\n int pf_matches; //pf==>point-features\n\n\n int _3d2d_n_pfvalid_depth;\n Matrix4d _3d2d__2T1; //used 3d points from 1st view, 2d points from 2nd view.\n bool isSet_3d2d__2T1=false; //< final_pose_available\n float _3d2d__2T1__ransac_confidence;\n\n\n // std::mutex _mutex;\n\n // All computed poses\n vector opX_b_T_a; // b_T_a is same as 2T1\n vector opX_goodness;\n vector opX_b_T_a_name;\n vector opX_b_T_a_debugmsg;\n\n // debug images\n vector debug_images;\n vector debug_images_titles;\n\n\n\n};\n" }, { "path": "src/Visualization.cpp", "content": "#include \"Visualization.h\"\n\nVisualization::Visualization( ros::NodeHandle &nh )\n{\n b_run_thread = false;\n this->nh = nh;\n\n}\n\nvoid Visualization::setDataManager( DataManager* dataManager )\n{\n this->dataManager = dataManager;\n m_dataManager_available = true;\n}\n\nvoid Visualization::setCerebro( Cerebro* cerebro )\n{\n this->cerebro = cerebro;\n m_cerebro_available = true;\n}\n\n\nvoid Visualization::setVizPublishers( const string base_topic_name )\n{\n //\n // Test Publisher\n string pub_topic_test = base_topic_name+\"/chatter\";\n ROS_INFO( \"Visualization Publisher pub_topic_test: %s\", pub_topic_test.c_str() );\n chatter_pub = nh.advertise(pub_topic_test, 1000);\n\n //\n // Publish Markers of frame data\n string framedata_pub_topic = base_topic_name+\"/framedata\";\n ROS_INFO( \"Visualization Publisher framedata_pub_topic: %s\", framedata_pub_topic.c_str() );\n framedata_pub = nh.advertise(framedata_pub_topic, 1000);\n\n // can add image publish if need be here.\n string imagepaire_pub_topic = base_topic_name+\"/imagepaire\";\n ROS_INFO( \"Visualization Publisher imagepair_pub_topic: %s\", imagepaire_pub_topic.c_str() );\n imagepaire_pub = nh.advertise(imagepaire_pub_topic, 1000);\n}\n\nvoid Visualization::run( const int looprate )\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Visualization before execution of the run() thread can begin. You can set the dataManager by call to Visualization::setDataManager()\\n\");\n assert( b_run_thread && \"you need to call run_thread_enable() before run() can start executing\\n\" );\n assert( looprate > 0 && \"[ Visualization::run] looprate need to be postive\\n\");\n\n // Adjust these for debugging.\n bool bpub_framepositions = false; //< This publishes text-marker with node id\n bool bpub_loopcandidates = true; //< this publishes line marker\n bool bpub_processed_loopcandidates = true; //this publoshes the image-pair as image.\n\n ros::Rate rate( looprate );\n while( b_run_thread )\n {\n // cout << \"Visualization::run() \" << dataManager->getDataMapRef().size() <publish_frames();\n\n if( bpub_loopcandidates )\n this->publish_loopcandidates(); //< this publishes marker\n // this->publish_test_string();\n\n if( bpub_processed_loopcandidates )\n this->publish_processed_loopcandidates(); //< this publoshes the image-pair as image.\n\n rate.sleep();\n }\n}\n\n\n// #define __Visualization___publish_processed_loopcandidates(msg) msg;\n#define __Visualization___publish_processed_loopcandidates(msg) ;\nvoid Visualization::publish_processed_loopcandidates()\n{\n assert( m_dataManager_available && m_cerebro_available && \"You need to set cerebro and DataManager in class Visualization before execution of the run() thread can begin. You can set the cerebro by call to Visualization::setCerebro() and dataManager as setDataManager.\\n\");\n\n static int prev_count = -1;\n\n if( prev_count == cerebro->processedLoops_count() || prev_count<0 ) {\n prev_count = cerebro->processedLoops_count();\n // nothing new\n return;\n }\n\n int new_count = cerebro->processedLoops_count();\n // [prev_count , new_count ] are new\n\n // cout << \"[Visualization::publish_processed_loopcandidates]\" << new_count << endl;\n __Visualization___publish_processed_loopcandidates(\n cout << \"#new procs_loops=\" << new_count - prev_count << \" from [\" << prev_count << \",\" << new_count-1 << \"]\\n\";\n )\n\n visualization_msgs::Marker marker;\n RosMarkerUtils::init_line_marker( marker );\n marker.ns = \"processed_loopcandidates_line\";\n\n // loop over all the new\n int loop_start = prev_count;\n int loop_end = new_count;\n bool publish_image = true;\n bool publish_loop_line_marker = false;\n // if( rand()%100 < 2 ) {\n // loop_start=0;\n // publish_image=false;\n // }\n\n for( int i=loop_start ; i< loop_end; i++ )\n {\n // publish green colored line\n ProcessedLoopCandidate candidate_i = cerebro->processedLoops_i( i );\n __Visualization___publish_processed_loopcandidates(\n cout << \"---\\nUsing processedLoop[\"<getT() << \"<--->\" << candidate_i.node_2->getT() << endl;\n cout << \"isPoseAvailable: \" << candidate_i.node_1->isPoseAvailable() << endl;\n )\n\n\n if( publish_loop_line_marker )\n {\n Vector4d w_t_1 = candidate_i.node_1->getPose().col(3);\n Vector4d w_t_2 = candidate_i.node_2->getPose().col(3);\n RosMarkerUtils::add_point_to_marker( w_t_1, marker, true );\n RosMarkerUtils::add_point_to_marker( w_t_2, marker, false );\n RosMarkerUtils::setcolor_to_marker( 0.0, 1.0, 0.0 , marker );\n marker.ns = \"processed_loopcandidates_line\";\n marker.id = i;\n framedata_pub.publish( marker );\n\n\n // Publish marker line\n if( candidate_i.isSet_3d2d__2T1 == false )\n {\n cout << \"[Visualization::publish_processed_loopcandidates] _3d2d__2T1 is not set. This means the final pose is not set. This is because the candidate relative poses do not appear to be consistent with each other. Ignoring this ProcessedLoopCandidate\" << endl;;\n continue;\n }\n\n Matrix4d w_T_2__new = candidate_i.node_1->getPose() * (candidate_i._3d2d__2T1).inverse();\n Vector4d w_t_2__new = w_T_2__new.col(3);\n RosMarkerUtils::add_point_to_marker( w_t_1, marker, true );\n RosMarkerUtils::add_point_to_marker( w_t_2__new, marker, false );\n RosMarkerUtils::setcolor_to_marker( 1.0, 1.0, 1.0 , marker );\n marker.ns = \"processed_loopcandidates_new_position_of_2\";\n marker.id = i;\n framedata_pub.publish( marker );\n }\n\n\n // publish image\n if( publish_image)\n {\n #if 0\n if( candidate_i.node_1->isImageAvailable() && candidate_i.node_2->isImageAvailable() )\n #else\n auto img_data_mgr = dataManager->getImageManagerRef();\n if(\n img_data_mgr->isImageRetrivable(\"left_image\", candidate_i.node_1->getT()) &&\n img_data_mgr->isImageRetrivable(\"left_image\", candidate_i.node_2->getT())\n )\n #endif\n {\n\n #if 0 // code where data_node had image data, mark this for removal\n cv::Mat _node_1_im = candidate_i.node_1->getImage();\n cv::Mat _node_2_im = candidate_i.node_2->getImage();\n #else // image manager has the image data\n cv::Mat _node_1_im, _node_2_im;\n bool status1 = img_data_mgr->getImage( \"left_image\", candidate_i.node_1->getT(), _node_1_im );\n bool status2 = img_data_mgr->getImage( \"left_image\", candidate_i.node_2->getT(), _node_2_im );\n assert( status1 && status2 && \"[Visualization::publish_processed_loopcandidates]\\n\");\n #endif\n\n // use image from node\n cv::Mat side_by_side_impair;\n MiscUtils::side_by_side( _node_1_im, _node_2_im, side_by_side_impair );\n\n string sgg = std::to_string( candidate_i.idx_from_datamanager_1 )\n + \" \"+\n std::to_string( candidate_i.idx_from_datamanager_2 );\n string sgg_time = std::to_string( candidate_i.node_1->getT().toSec() )\n + \" \"+\n std::to_string( candidate_i.node_2->getT().toSec() );\n\n\n MiscUtils::append_status_image( side_by_side_impair, \";;\"+sgg,\n 2.0, cv::Scalar(0,0,0), cv::Scalar(255,255,255), 3.0 );\n MiscUtils::append_status_image( side_by_side_impair, \";\"+sgg_time+\";;processedLoop[\"+to_string(i)+\"]\",\n 1.5, cv::Scalar(0,0,0), cv::Scalar(255,255,255), 3.0 );\n\n if( candidate_i.isSet_3d2d__2T1 ) {\n MiscUtils::append_status_image( side_by_side_impair, \";Pose Estimation was consistent, so Accept\",\n 1.5, cv::Scalar(0,0,0), cv::Scalar(0,255,0), 3.0 );\n } else {\n MiscUtils::append_status_image( side_by_side_impair, \";Inconsistent Pose Estimation, so Reject\",\n 1.5, cv::Scalar(0,0,0), cv::Scalar(0,0,255), 3.0 );\n }\n\n cv::Mat buffer;\n cv::resize(side_by_side_impair, buffer, cv::Size(), 0.5, 0.5 );\n\n cv_bridge::CvImage cv_image;\n cv_image.image = buffer;\n if( buffer.channels() == 1 )\n cv_image.encoding = \"mono8\";\n else if( buffer.channels() == 3 )\n cv_image.encoding = \"bgr8\";\n else {\n cout << TermColor::RED() << \"[Visualization::publish_processed_loopcandidates]invalid number of channels EXIT\\n\";\n exit(1);\n }\n\n sensor_msgs::Image ros_image_msg;\n cv_image.toImageMsg(ros_image_msg);\n __Visualization___publish_processed_loopcandidates(\n cout << MiscUtils::imgmsg_info( ros_image_msg ) << endl;\n )\n imagepaire_pub.publish( ros_image_msg );\n }\n }\n\n\n }\n prev_count = new_count;\n}\n\n\n// #define __Visualization__publish_loopcandidates(msg) msg\n#define __Visualization__publish_loopcandidates(msg) ;\nvoid Visualization::publish_loopcandidates()\n{\n assert( m_dataManager_available && m_cerebro_available && \"You need to set cerebro and DataManager in class Visualization before execution of the run() thread can begin. You can set the cerebro by call to Visualization::setCerebro() and dataManager as setDataManager.\\n\");\n\n // last 10 publish\n int n = cerebro->foundLoops_count();\n\n int start = max( 0, n - 10 );\n // 5% of the time start from 0.\n if( rand() % 100 < 2 ) start = 0;\n __Visualization__publish_loopcandidates(cout << \"[Visualization::publish_loopcandidates] start=\" << start << \" end=\" << n << endl;)\n\n if( n <= 0 ) return;\n\n auto data_map = dataManager->getDataMapRef();\n visualization_msgs::Marker marker;\n RosMarkerUtils::init_line_marker( marker );\n marker.ns = \"loopcandidates_line\";\n RosMarkerUtils::setcolor_to_marker( 1.0, 0.0, 0.0 , marker );\n // TODO If need be can also publish text for each (which could be score of the edge)\n\n\n // for( int i=0 ; ifoundLoops_i( i );\n ros::Time t_curr = std::get<0>(u);\n ros::Time t_prev = std::get<1>(u);\n double score = std::get<2>(u);\n\n assert( data_map.count( t_curr ) > 0 && data_map.count( t_prev ) > 0 && \"One or both of the timestamps in foundloops where not in the data_map. This cannot be happening...fatal...\\n\" );\n int idx_1 = std::distance( data_map->begin(), data_map->find( t_curr ) );\n int idx_2 = std::distance( data_map->begin(), data_map->find( t_prev ) );\n\n Vector4d w_t_curr = data_map->at(t_curr)->getPose().col(3);\n Vector4d w_t_prev = data_map->at(t_prev)->getPose().col(3);\n\n // TODO - need to test. looks like alright.\n // add_point_to_marker with w_t_curr\n // add_point_to_marker with w_t_prev\n RosMarkerUtils::add_point_to_marker( w_t_curr, marker, true );\n RosMarkerUtils::add_point_to_marker( w_t_prev, marker, false );\n\n\n\n framedata_pub.publish( marker );\n }\n\n}\n\n// #define __Visualization__publish_frames( cmd ) cmd\n#define __Visualization__publish_frames( cmd ) ;\nvoid Visualization::publish_frames()\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Visualization before execution of the run() thread can begin. You can set the dataManager by call to Visualization::setDataManager()\\n\");\n\n // Adjust these manually to change behaviour\n bool publish_camera_visual = false;\n bool publish_camera_as_point = true;\n bool publish_txt = true;\n bool publish_verbose_txt = false;\n\n\n static std::map< ros::Time, int > XC;\n\n __Visualization__publish_frames(cout << TermColor::RED() << \"---\" << TermColor::RESET() << endl;)\n __Visualization__publish_frames(cout << \"start... sizeof(XC)=\" << XC.size() << endl;)\n auto data_map = dataManager->getDataMapRef();\n auto pose0 = dataManager->getPose0Stamp();\n\n\n visualization_msgs::Marker cam_vis; //visualize every pose as a cam-visual\n RosMarkerUtils::init_camera_marker( cam_vis , .5 );\n cam_vis.ns = \"cam_pose_vis\";\n\n\n visualization_msgs::Marker pt_vis; //visualize every pose as a point.\n RosMarkerUtils::init_points_marker( pt_vis );\n geometry_msgs::Point zer; zer.x =0.; zer.y=0.; zer.z = 0;\n pt_vis.points.push_back( zer );\n pt_vis.ns = \"cam_pose_pt\";\n pt_vis.scale.x = 0.015;\n pt_vis.scale.y = 0.015;\n\n\n visualization_msgs::Marker txt_vis;\n RosMarkerUtils::init_text_marker( txt_vis );\n txt_vis.ns = \"cam_pose_txt\";\n txt_vis.scale.z = 0.03;\n\n\n for( auto it = data_map->begin() ; it != data_map->end() ; it++ )\n {\n if( XC[ it->first ] < 10 || rand() % 100 < 2 ) { // publish only if not already published 10 times\n int seq_id = std::distance( data_map->begin() , it );\n cam_vis.id = seq_id;\n pt_vis.id = seq_id;\n txt_vis.id = seq_id;\n\n // Set Colors\n if( it->second->isKeyFrame() ) {\n RosMarkerUtils::setcolor_to_marker( 0., 1., 0. , cam_vis );\n RosMarkerUtils::setcolor_to_marker( 0., 1., 0. , pt_vis );\n RosMarkerUtils::setcolor_to_marker( 1., 1., 1. , txt_vis );\n\n // if( it->second->isWholeImageDescriptorAvailable() )\n // RosMarkerUtils::setcolor_to_marker( 0., 0, 1. , cam_vis );\n }\n else {\n RosMarkerUtils::setcolor_to_marker( 1., 0., 0. , cam_vis );\n RosMarkerUtils::setcolor_to_marker( 1., 0., 0. , pt_vis );\n RosMarkerUtils::setcolor_to_marker( 1., 1., 1. , txt_vis );\n }\n\n\n // Set Pose\n if( it->second->isPoseAvailable() ) {\n auto wTc = it->second->getPose();\n RosMarkerUtils::setpose_to_marker( wTc , cam_vis );\n RosMarkerUtils::setpose_to_marker( wTc , pt_vis );\n\n RosMarkerUtils::setpose_to_marker( wTc , txt_vis );\n txt_vis.text = \"\";\n txt_vis.text += std::to_string(seq_id) + \";\";\n if( publish_verbose_txt ) {\n txt_vis.text += std::to_string( (it->first - pose0).toSec() ) +\";\";\n txt_vis.text += std::to_string( (it->first).toSec() ) +\";\";\n }\n\n if( publish_camera_visual )\n framedata_pub.publish( cam_vis );\n if( publish_camera_as_point )\n framedata_pub.publish( pt_vis );\n if( publish_txt || publish_verbose_txt )\n framedata_pub.publish( txt_vis );\n }\n\n\n if( it->second->isKeyFrame() )\n __Visualization__publish_frames(cout << TermColor::GREEN() );\n __Visualization__publish_frames( cout << \"Publish seq_id=\" << seq_id << \"\\t xc=\" << XC[ it->first ] << \"\\t t=\"<< it->first - pose0 << \"\\t\" << it->first << TermColor::RESET() << endl; )\n\n XC[ it->first ]++;\n }\n }\n __Visualization__publish_frames( cout << \"Done... sizeof(XC)=\" << XC.size() << endl; )\n}\n/*\nvoid Visualization::publish_frames()\n{\n assert( m_dataManager_available && \"You need to set the DataManager in class Visualization before execution of the run() thread can begin. You can set the dataManager by call to Visualization::setDataManager()\\n\");\n\n static std::map XC;\n static visualization_msgs::Marker linestrip_marker;\n RosMarkerUtils::init_line_strip_marker( linestrip_marker );\n linestrip_marker.ns = \"cam_line_strip\";\n linestrip_marker.id = 0;\n\n\n auto data_map = dataManager->getDataMapRef();\n cout << \"---\\n\";\n if( data_map.begin() == data_map.end() ) {\n cout << \"nothing to vizualize\\n\";\n return;\n }\n\n // cout << data_map.rbegin()->first << \"\\t\"<< data_map.rbegin()->first - dataManager->getPose0Stamp() << endl;\n ros::Time lb = data_map.rbegin()->first - ros::Duration(3);\n for( auto it = data_map.lower_bound( lb ); it != data_map.end() ; it++ ) {\n // cout << std::distance( it, data_map.begin() ) << \"] \" << it->first << \"\\t\" << it->first - dataManager->getPose0Stamp() << endl;\n\n if( XC.count( it->first) == 0 ) {\n cout << \"sizeof(XC)=\" << XC.size() << \" \"<< it->first << \"\\t\" << it->first - dataManager->getPose0Stamp() << endl;\n\n if( it->second->isPoseAvailable() ) {\n auto w_T_c = it->second->getPose();\n geometry_msgs::Point pt;\n pt.x = w_T_c(0,3);\n pt.y = w_T_c(1,3);\n pt.z = w_T_c(2,3);\n linestrip_marker.points.push_back( pt );\n framedata_pub.publish( linestrip_marker );\n XC[it->first] = true;\n }\n }\n }\n return ;\n\n}\n*/\n\nvoid Visualization::publish_test_string()\n{\n std_msgs::String msg;\n std::stringstream ss;\n ss << \"hello world \" << ros::Time::now();\n msg.data = ss.str();\n chatter_pub.publish( msg );\n}\n" }, { "path": "src/Visualization.h", "content": "#pragma once\n\n/** Visualization class\n This holds a reference to the DataManager. Will publish messages\n to be seen with rviz.\n\n Author : Manohar Kuse\n Created : 29th Oct, 2018\n\n based on\n https://github.com/mpkuse/nap/blob/master-desktop/src/DataManager_rviz_visualization.cpp\n\n*/\n\n\n\n#include \n#include \n\n// ros msg\n#include \n#include \n\n#include \"utils/RosMarkerUtils.h\"\n#include \"utils/TermColor.h\"\n\n\n#include \"PinholeCamera.h\"\n#include \"DataManager.h\"\n#include \"Cerebro.h\"\n\n\nclass Visualization\n{\npublic:\n Visualization( ros::NodeHandle &nh );\n\n void setDataManager( DataManager* dataManager );\n void setCerebro( Cerebro* cerebro );\n\n void setVizPublishers( const string base_topic_name );\n\n // This is supposed to be run in a separate thread.\n void run_thread_enable() { b_run_thread = true; }\n void run_thread_disable() { b_run_thread = false; }\n void run( const int looprate );\n\nprivate:\n ros::NodeHandle nh;\n\n bool m_dataManager_available=false;\n DataManager * dataManager;\n\n bool m_cerebro_available=false;\n Cerebro * cerebro;\n\n atomic b_run_thread;\n\n\nprivate:\n void publish_frames(); //< publishes last 10 frames as markers. (occasionally all)\n void publish_loopcandidates(); //< publishes last 10 loop candidates. cerebro->foundLoops_count().\n void publish_processed_loopcandidates(); //< uses Cerebro::processedLoops_i() and publishes only the newly found loops. cerebro->processedLoops_i( i )\n void publish_test_string();\n\n\n //publishers\nprivate:\n ros::Publisher chatter_pub;\n ros::Publisher framedata_pub;\n ros::Publisher imagepaire_pub;\n\n};\n" }, { "path": "src/cerebro_node.cpp", "content": "/** Main for cerebro node.\n\n This contains the main for the cerebro. It subscribes to various topics\n from VINS-estimator and setups of threads a) vis-thread, b) redis-association-thread, c) etc\n\n Author : Manohar Kuse \n Created : 26th Oct, 2018\n**/\n\n\n#include \n#include \n\n\n#include \"PinholeCamera.h\"\n#include \"camodocal/camera_models/Camera.h\" // this is in include/camodocal. src files in src/utils/camodocal_src\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \n\n\n#include \"DataManager.h\"\n#include \"Cerebro.h\"\n#include \"Visualization.h\"\n\n#include \"utils/nlohmann/json.hpp\"\nusing json = nlohmann::json;\n\n\nint main( int argc, char ** argv )\n{\n //--- ROS INIT ---//\n ros::init( argc, argv, \"cerebro_node\" );\n ros::NodeHandle nh(\"~\");\n // ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);\n\n\n //-- Set debug directory --//\n // string debug_output_dir;\n // nh.getParam( \"debug_output_dir\", debug_output_dir );\n // ROS_WARN( \"debug_output_dir : %s\", debug_output_dir.c_str() );\n\n\n //--- loadStateFromDisk, saveStateToDisk ---//\n string loadStateFromDisk = \"\";\n if( nh.getParam( \"loadStateFromDisk\", loadStateFromDisk ) == true )\n {\n if( loadStateFromDisk.compare(\"\") == 0 ) {\n ROS_WARN( \"[cerebro_node] loadStateFromDisk cmdline parameter was found, but with a empty string, so I will not loadStateFromDisk()\");\n } else {\n // now make sure it is a directory\n if( RawFileIO::is_path_a_directory(loadStateFromDisk) ) {\n ROS_INFO( \"[cerebro_node] loadStateFromDisk=%s, Directory exists...OK!\", loadStateFromDisk.c_str() );\n cout << TermColor::GREEN() << \"[cerebro_node] loadStateFromDisk=\" << loadStateFromDisk << \", Directory exists...OK!\" << TermColor::RESET() << endl;\n }\n else {\n ROS_ERROR( \"[cerebro_node] You specified a directory for loadStateFromDisk=`%s`, This path need to exist\\n...EXIT\\n\", loadStateFromDisk.c_str());\n exit(1);\n }\n }\n } else {\n ROS_WARN( \"[cerebro_node] loadStateFromDisk cmdline parameter was not found, so I will not loadStateFromDisk()\");\n }\n\n string saveStateToDisk = \"\";\n if( nh.getParam( \"saveStateToDisk\", saveStateToDisk ) == true )\n {\n if( saveStateToDisk.compare(\"\") == 0 ) {\n ROS_WARN( \"[cerebro_node] saveStateToDisk cmdline parameter was found, but with a empty string, so I will not saveStateToDisk()\");\n } else {\n // now make sure it is a directory\n if( RawFileIO::is_path_a_directory(saveStateToDisk) ) {\n ROS_INFO( \"[cerebro_node] saveStateToDisk=%s, Directory exists...OK!\", saveStateToDisk.c_str() );\n cout << TermColor::GREEN() << \"[cerebro_node] saveStateToDisk=\" << saveStateToDisk << \", Directory exists...OK!\" << TermColor::RESET() << endl;\n }\n else {\n ROS_ERROR( \"[cerebro_node] You specified a directory for saveStateToDisk=`%s`, This path need to exist and be writable\\n...EXIT\\n\", saveStateToDisk.c_str());\n exit(1);\n }\n }\n } else {\n ROS_WARN( \"[cerebro_node] saveStateToDisk cmdline parameter was not found, so I will not saveStateToDisk()\");\n }\n\n //--- END loadStateFromDisk, saveStateToDisk ---//\n\n\n\n //--- Config File ---//\n string config_file;\n if( !nh.getParam( \"config_file\", config_file ) )\n {\n ROS_ERROR( \"[cerebro_node] config_file cmdline parameter required to read the camera matrix.\\nThis is fatal error\\n..quit...\" );\n exit(1);\n }\n ROS_WARN( \"Config File Name : %s\", config_file.c_str() );\n\n // check if file exists - depends on boost\n if ( !boost::filesystem::exists( config_file ) )\n {\n std::cout << \"[cerebro_node] Can't find my file: \" << config_file << std::endl;\n ROS_ERROR_STREAM( \"[cerebro_node] Can't find config_file:\" << config_file );\n exit(1);\n }\n\n cv::FileStorage fs(config_file, cv::FileStorage::READ);\n if( !fs.isOpened() )\n {\n ROS_ERROR_STREAM( \"[cerebro_node] cannot open config_file: \"<< config_file << \"\\nThe file seems to be stated on the cmdline but it cannot be opened, possibly file is not found on the stated path or it does not have read permission\\n...fatal...quit()\\n\" );\n exit(1);\n }\n\n\n\n // --- Get yaml config for primary camera. (cam0) ---//\n camodocal::CameraPtr abstract_camera;\n // abstract_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(config_file);\n // assert( abstract_camera && \"Even after loading yaml the camera seem invalid. Something wrong\\n\");\n {\n string cam0_calib;\n if( !fs[\"cam0_calib\"].isString() )\n {\n ROS_WARN_STREAM( \"[cerebro_node] cannot find key `cam0_calib` in config_file=\" << config_file );\n }\n else {\n fs[\"cam0_calib\"] >> cam0_calib;\n ROS_INFO( \"cam0_calib : %s\", cam0_calib.c_str() );\n\n vector ___path = MiscUtils::split( config_file, '/' );\n // does::> $(python) '/'.join( ___path[0:-1] )\n string ___cam0_path = string(\"\");\n for( int _i = 0 ; _i<___path.size()-1 ; _i++ ) {\n ___cam0_path += ___path[_i] + \"/\";\n }\n ___cam0_path += \"/\"+cam0_calib;\n cout << \"cam0_fullpath=\" << ___cam0_path << endl;\n\n\n // check if file exists - depends on boost\n if ( !boost::filesystem::exists( ___cam0_path ) )\n {\n std::cout << \"[cerebro_node] Can't find my file for primary camera: \" << ___cam0_path << std::endl;\n ROS_ERROR_STREAM( \"[cerebro_node] Can't find my file for primary camera\" << ___cam0_path );\n exit(1);\n }\n\n // Make Abstract Camera\n // camodocal::CameraPtr abstract_camera_1;\n cout << TermColor::GREEN() << \"Load file : \" << ___cam0_path << TermColor::RESET() << endl;\n abstract_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(___cam0_path);\n assert( abstract_camera && \"Even after loading yaml the camera seem invalid. Something wrong\\n\");\n\n\n // dataManager.setAbstractCamera( abstract_camera_1, 1 );\n }\n }\n\n\n\n\n // PinholeCamera camera = PinholeCamera( config_file );\n // camera.printCameraInfo(2); // Not in use, using camodocal camera instead.\n\n\n //--- DataManager ---//\n DataManager dataManager = DataManager(nh);\n // dataManager.setCamera(camera);\n dataManager.setAbstractCamera( abstract_camera );\n\n\n\n //--- Subscribers ---// TODO: move all subscribers this to dataManager class ?\n\n // [A]\n // w_T_c: Pose of camera (all, only a subset are keyframes) in world-cordinate system.\n string camera_pose_topic = string(\"/vins_estimator/camera_pose\");\n ROS_INFO( \"Subscribe to camera_pose_topic: %s\", camera_pose_topic.c_str() );\n ros::Subscriber sub_odometry = nh.subscribe( camera_pose_topic, 1000, &DataManager::camera_pose_callback, &dataManager );\n\n\n // // [A.1] TODO: not needed. consider not subscribing\n // // Marker for keyframes. Caution, these poses are of imu (NOT of camera).\n // string keyframe_pose_topic = string(\"/vins_estimator/keyframe_pose\");\n // ROS_INFO( \"Subscribe to keyframe_camera_pose_topic: %s\", keyframe_pose_topic.c_str() );\n // ros::Subscriber sub_kf_odometry = nh.subscribe( keyframe_pose_topic, 1000, &DataManager::keyframe_pose_callback, &dataManager );\n\n\n\n // [B]\n // Raw Image (all). The topic's name is in the config_file\n\n string raw_image_topic;\n if( !fs[\"image0_topic\"].isString() )\n {\n ROS_ERROR_STREAM( \"[cerebro_node] cannot find key 'image0_topic' in config_file(\"<< config_file << \")\\n...fatal...quit\" );\n exit(1);\n }\n fs[\"image0_topic\"] >> raw_image_topic;\n ROS_INFO( \"Subscribe to raw_image_topic: %s\", raw_image_topic.c_str() );\n ros::Subscriber sub_image = nh.subscribe( raw_image_topic, 10, &DataManager::raw_image_callback, &dataManager );\n\n // [B.1]\n // Additional Image topic (stereo pair)\n string raw_image_topic_1;\n ros::Subscriber sub_image_1;\n if( !fs[\"image1_topic\"].isString() )\n {\n ROS_WARN_STREAM( \"[cerebro_node] cannot find key `image1_topic` in config_file=\" << config_file << \". This was optional so nothing to worry if you don't need the stereo pair\" );\n exit(1);\n }\n else {\n fs[\"image1_topic\"] >> raw_image_topic_1;\n ROS_INFO( \"Subscribe to image_topic_1: %s\", raw_image_topic_1.c_str() );\n sub_image_1 = nh.subscribe( raw_image_topic_1, 10, &DataManager::raw_image_callback_1, &dataManager );\n }\n\n\n // [B.2]\n // Additional Image topic (realsense depth) 16UC1\n string depth_image_topic = \"/camera/depth/image_rect_raw\";\n ros::Subscriber sub_depth_image;\n ROS_INFO( \"******\\nSubscribe to depth_image_topic: %s\\n********\", depth_image_topic.c_str() );\n sub_depth_image = nh.subscribe( depth_image_topic, 10, &DataManager::depth_image_callback, &dataManager );\n\n\n\n\n\n // [B.2]\n // Additional Cameras (yaml)\n string camera_yaml_1;\n if( !fs[\"cam1_calib\"].isString() )\n {\n ROS_WARN_STREAM( \"[cerebro_node] cannot find key `cam1_calib` in config_file=\" << config_file << \". This was optional so nothing to worry if you don't need the 2nd camera\" );\n exit(1);\n }\n else {\n fs[\"cam1_calib\"] >> camera_yaml_1;\n ROS_INFO( \"cam1_calib : %s\", camera_yaml_1.c_str() );\n\n vector ___path = MiscUtils::split( config_file, '/' );\n // does::> $(python) '/'.join( ___path[0:-1] )\n string ___camera_yaml_1_path = string(\"\");\n for( int _i = 0 ; _i<___path.size()-1 ; _i++ ) {\n ___camera_yaml_1_path += ___path[_i] + \"/\";\n }\n ___camera_yaml_1_path += \"/\"+camera_yaml_1;\n cout << \"camera_yaml_1_fullpath=\" << ___camera_yaml_1_path << endl;\n\n\n // check if file exists - depends on boost\n if ( !boost::filesystem::exists( ___camera_yaml_1_path ) )\n {\n std::cout << \"[cerebro_node] Can't find my file for additional camera: \" << ___camera_yaml_1_path << std::endl;\n ROS_ERROR_STREAM( \"[cerebro_node] Can't find my file for additional camera\" << ___camera_yaml_1_path );\n exit(1);\n }\n\n // Make Abstract Camera\n camodocal::CameraPtr abstract_camera_1;\n cout << TermColor::GREEN() << \"Load file : \" << ___camera_yaml_1_path << TermColor::RESET() << endl;\n abstract_camera_1 = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(___camera_yaml_1_path);\n assert( abstract_camera_1 && \"Even after loading yaml the camera_1 seem invalid. Something wrong\\n\");\n\n dataManager.setAbstractCamera( abstract_camera_1, 1 );\n }\n\n\n // [B.3]\n // Camera baseline. Set stereobaseline transform ie. right_T_left from yaml file\n if( !fs[\"extrinsic_1_T_0\"].isString() )\n {\n ROS_WARN_STREAM( \"[cerebro_node] cannot find key `extrinsic_1_T_0` in config file=\" << config_file << \" this was not needed for monocular camera, but needed for a stereo camera\" );\n cout << TermColor::RED() << \"[cerebro_node] cannot find key `extrinsic_1_T_0` in config file=\" << config_file << \" this was not needed for monocular camera, but needed for a stereo camera\" << TermColor::RESET() << endl;\n // exit(1);\n\n\n #if 1\n // look for the existence of `body_T_cam0` and `body_T_cam1`\n cout << TermColor::YELLOW() << \"Since, the key `extrinsic_1_T_0`, I am looking for the existence of `body_T_cam0` and `body_T_cam1`\" << TermColor::RESET() << endl;\n\n if( fs[\"body_T_cam0\"].empty() || fs[\"body_T_cam1\"].empty() ) {\n cout << TermColor::RED() << \"Either of the keys `body_T_cam0` or `body_T_cam1` cannot be found in config file. FATAL ERROR\\n\" << TermColor::RESET() ;\n exit(1);\n }\n\n\n cout << TermColor::iGREEN() << \"The keys `body_T_cam0` and `body_T_cam1` exists, compute extrinsic_1_T_0 from these two as ` ee_body_T_cam1.inverse() * ee_body_T_cam0;`\\n\" << TermColor::RESET() ;\n cv::Mat body_T_cam0, body_T_cam1;\n fs[\"body_T_cam0\"] >> body_T_cam0;\n fs[\"body_T_cam1\"] >> body_T_cam1;\n Matrix4d ee_body_T_cam0, ee_body_T_cam1;\n cv::cv2eigen( body_T_cam0, ee_body_T_cam0 );\n cv::cv2eigen( body_T_cam1, ee_body_T_cam1 );\n\n Matrix4d cam1_T_cam0 = ee_body_T_cam1.inverse() * ee_body_T_cam0;\n cout << \"Matrix4d cam1_T_cam0 = \" << PoseManipUtils::prettyprintMatrix4d(cam1_T_cam0) << endl;\n {\n cout << \"raw matrix of cam1_T_cam0:\\n\" << cam1_T_cam0 << \"----\" << endl;\n\n double q_xyzw[5], t_xyz[5];\n PoseManipUtils::eigenmat_to_raw_xyzw( cam1_T_cam0, q_xyzw, t_xyz );\n cout << \"q_xyzw: \" << q_xyzw[0] << \",\" << q_xyzw[1] << \",\" << q_xyzw[2] << \",\" << q_xyzw[3] << endl;\n cout << \" t_xyz: \" << t_xyz[0] << \",\" << t_xyz[1] << \",\" << t_xyz[2] << endl;\n }\n dataManager.setCameraRelPose( cam1_T_cam0, std::make_pair(1,0) );\n\n #endif\n\n }\n else {\n // Extract the fname from the config_file\n string extrinsic_1_T_0;\n fs[\"extrinsic_1_T_0\"] >> extrinsic_1_T_0;\n ROS_INFO( \"in config_file=%s; extrinsic_1_T_0 : %s\", config_file.c_str(), extrinsic_1_T_0.c_str() );\n\n // Make full path from fname\n vector ___path = MiscUtils::split( config_file, '/' );\n // does::> $(python) '/'.join( ___path[0:-1] )\n string ___extrinsic_1_T_0_path = string(\"\");\n for( int _i = 0 ; _i<___path.size()-1 ; _i++ ) {\n ___extrinsic_1_T_0_path += ___path[_i] + \"/\";\n }\n ___extrinsic_1_T_0_path += \"/\"+extrinsic_1_T_0;\n cout << \"___extrinsic_1_T_0_fullpath=\" << ___extrinsic_1_T_0_path << endl;\n\n\n // Open fullpath of extrinsic.yaml\n cout << \"opencv yaml reading: open file: \" << ___extrinsic_1_T_0_path << endl;\n cv::FileStorage fs(___extrinsic_1_T_0_path, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n ROS_ERROR_STREAM( \"config_file asked to open extrinsicbasline file but it cannot be opened.\\nTHIS IS FATAL, QUITING\" );\n exit(1);\n }\n\n cout << TermColor::GREEN() << \"successfully opened file \"<< ___extrinsic_1_T_0_path << TermColor::RESET() << endl;\n cv::FileNode n = fs[\"transform\"];\n if( n.empty() ) {\n cout << TermColor::RED() << \"I was looking for the key `transform` in the file but it doesnt seem to exist. FATAL ERROR\" << TermColor::RESET() << endl;\n exit(1);\n }\n Vector4d q_xyzw;\n q_xyzw << (double)n[\"q_x\"] , (double)n[\"q_y\"] ,(double) n[\"q_z\"] ,(double) n[\"q_w\"];\n\n Vector3d tr_xyz;\n tr_xyz << (double)n[\"t_x\"] , (double)n[\"t_y\"] ,(double) n[\"t_z\"];\n\n cout << \"--values from file--\\n\" << TermColor::iGREEN();\n cout << \"q_xyzw:\\n\" << q_xyzw << endl;\n cout << \"tr_xyz:\\n\" << tr_xyz << endl;\n cout << TermColor::RESET() << endl;\n\n Matrix4d _1_T_0;\n PoseManipUtils::raw_xyzw_to_eigenmat( q_xyzw, tr_xyz/1000., _1_T_0 ); cout << \"translation divided by 1000 to convert from mm (in file) to meters (as needed)\\n\";\n // cout << TermColor::iBLUE() << \"_1_T_0:\\n\" << _1_T_0 << TermColor::RESET() << endl;\n cout << TermColor::iBLUE() << \"_1_T_0: \" << PoseManipUtils::prettyprintMatrix4d( _1_T_0 ) << TermColor::RESET() << endl;\n cout << \"_1_T_0:\\n\" << _1_T_0 << endl;\n\n\n dataManager.setCameraRelPose( _1_T_0, std::make_pair(1,0) );\n\n\n #if 0\n // verify if it was correctly set --> Works! Verified !\n cout << \"isCameraRelPoseSet ? \" << dataManager.isCameraRelPoseSet( std::make_pair(1,0) ) << endl;\n cout << \"isCameraRelPoseSet ? \" << dataManager.isCameraRelPoseSet( std::make_pair(6,10) ) << endl;\n // cout << \"getCameraRelPose:\\n\" << dataManager.getCameraRelPose( std::make_pair(1,0) ) << endl;\n cout << \"getCameraRelPose:\\n\" << PoseManipUtils::prettyprintMatrix4d( dataManager.getCameraRelPose( std::make_pair(1,0) ) ) << endl;\n auto ___all_available_keys = dataManager.getCameraRelPoseKeys();\n cout << \"# relative poses available = \" << ___all_available_keys.size() << endl;\n #endif\n }\n\n\n\n // [C]\n // imu_T_cam : imu camera extrinsic calib. Will store this just in case there is a need\n string extrinsic_cam_imu_topic = string(\"/vins_estimator/extrinsic\");\n ROS_INFO( \"Subscribe to extrinsic_cam_imu_topic: %s\", extrinsic_cam_imu_topic.c_str() );\n ros::Subscriber sub_extrinsic = nh.subscribe( extrinsic_cam_imu_topic, 1000, &DataManager::extrinsic_cam_imu_callback, &dataManager );\n\n\n // [D]\n // PointCloud (all): has 5 channels\n string ptcld_topic = string(\"/vins_estimator/keyframe_point\");\n // string ptcld_topic = string(\"/feature_tracker/feature\");\n ROS_INFO( \"Subscribe to ptcld_topic: %s\", ptcld_topic.c_str() );\n ros::Subscriber sub_ptcld = nh.subscribe( ptcld_topic, 1000, &DataManager::ptcld_callback, &dataManager );\n\n // [E]\n // Tracked features. these contains a) unvn in msg->points b) chnl[0] id of the point c) chnl[1], chnl[2] := (u,v) d) chnl[3], chnl[4] not very sure.\n // string tracked_feat_topic = string(\"/feature_tracker/feature\");\n // ROS_INFO( \"Subscribe to tracked_feat_topic: %s\", tracked_feat_topic.c_str() );\n // ros::Subscriber sub_tracked_feat = nh.subscribe( tracked_feat_topic, 1000, &DataManager::tracked_feat_callback, &dataManager );\n\n\n\n // set this to 1 to enable loading state, set this to 0 to not load state\n // If you dont loadStateFromDisk, make sure you initialize the ImageDataManager.\n #define __LOAD_STATE__ 0\n #if __LOAD_STATE__\n //--- Load State From Disk\n dataManager.loadStateFromDisk( \"/Bulk_Data/chkpts_cerebro\" );\n // cout << \"PREMATURE EXIT\\n\";\n // exit(1);\n #else\n dataManager.getImageManagerRef()->initStashDir(true); // this will use the default /tmp/cerebro_stash as the stashing directory\n #endif\n\n\n\n // If you dont loadStateFromDisk, make sure you initialize the ImageDataManager.\n if( loadStateFromDisk.compare(\"\") != 0 )\n {\n dataManager.loadStateFromDisk( loadStateFromDisk );\n }\n else\n { //empty, fresh start\n dataManager.getImageManagerRef()->initStashDir(true); // this will use the default /tmp/cerebro_stash as the stashing directory\n }\n\n\n //--- Start Threads ---//\n\n // [A]\n // Data associate thread: looks at the callback buffers and sets the data in the std::map\n dataManager.data_association_thread_enable();\n // dataManager.data_association_thread_disable();\n std::thread data_association_th( &DataManager::data_association_thread, &dataManager, 15 );\n\n dataManager.trial_thread_enable();\n dataManager.trial_thread_disable();\n std::thread dm_trial_th( &DataManager::trial_thread, &dataManager ); //< this threads prints datamanagers status to /dev/pts/20 modify as need be.\n\n // [A.1] Another thread in class dataManager which will deallocate images in nonkeyframes.\n dataManager.clean_up_useless_images_thread_enable();\n // dataManager.clean_up_useless_images_thread_disable();\n std::thread dm_cleanup_th( &DataManager::clean_up_useless_images_thread, &dataManager );\n\n\n Cerebro cer( nh );\n cer.setDataManager( &dataManager );\n cer.setPublishers( \"/cerebro\" );\n\n\n\n\n // [B.00]\n // Kidnap Message Publisher. See also comments below for `kidnap message subscriber`\n // Setup Publisher for sending kidnaped message to\n // a) vins_estimator and b) pose_graph solver\n string pub_topic_test = \"/feature_tracker/rcvd_flag\";\n ROS_INFO( \"main : Publisher pub_topic_test: %s\", pub_topic_test.c_str() );\n ros::Publisher rcvd_flag_pub = nh.advertise(pub_topic_test, 1000);\n // We publish std_msgs::Header to the pose-graph-solver.\n // The purpose is that, this will serve as carrier of timestamp according to which\n // we can eliminate the odometry edges from the cost function.\n string pub_topic_header = \"/feature_tracker/rcvd_flag_header\";\n ROS_INFO( \"main : Publisher pub_topic_header: %s\", pub_topic_header.c_str() );\n ros::Publisher kidnap_indicator_header_pub = nh.advertise(pub_topic_header, 1000);\n dataManager.setKidnapIndicatorPublishers( rcvd_flag_pub, kidnap_indicator_header_pub );\n\n\n // [B.01]\n // Kidnap message subscriber. There are two kinds of kidnap messages\n // a. Bool b. Header. Look at the documentation of the kidnaped thread to know the details.\n // This has been done only for compatibility. I (mpkuse) prefer the Header because it\n // can also contain the timestamp when kidnap started and ended.\n string kidnap_bool_topic = \"/feature_tracker/rcvd_flag\";\n ROS_INFO( \"Subscribe to kidnap_bool_topic: %s\", kidnap_bool_topic.c_str() );\n ros::Subscriber sub_kidnap_bool = nh.subscribe( kidnap_bool_topic, 1000, &Cerebro::kidnap_bool_callback, &cer );\n\n string kidnap_header_topic = \"/feature_tracker/rcvd_flag_header\";\n ROS_INFO( \"Subscribe to kidnap_header_topic: %s\", kidnap_header_topic.c_str() );\n ros::Subscriber sub_kidnap_header = nh.subscribe( kidnap_header_topic, 1000, &Cerebro::kidnap_header_callback, &cer );\n\n\n // [B]\n // Descriptor Computation Thread.\n // It monitors data_map. If new keyframes are available then\n // it queries the ros-service with the image to get the whole-image-descriptor.\n // This whole image descriptor is stored in `node->setWholeImageDescriptor`\n // and the index of computation stored in `wholeImageComputedList`.\n cer.descriptor_computer_thread_enable();\n // cer.descriptor_computer_thread_disable();\n std::thread desc_th( &Cerebro::descriptor_computer_thread, &cer ); //runs @20hz\n\n\n // [C]\n // loopcandidates producer\n // This thread looks at `wholeImageComputedList`, if there is something new\n // in it (new descriptors) it does dot( 0-->T-50, T ), dot( 0-->T-50, T-1 )\n // and dot( 0-->T-50, T-2 ). If threshold-test and locality-test both\n // comeout to be +ve it declares 'loop found' and queues up this pair into the\n // list `foundLoops`\n cer.run_thread_enable();\n // cer.run_thread_disable();\n std::thread dot_product_th( &Cerebro::run, &cer ); //< descrip_N__dot__descrip_0_N. runs @ 10hz\n\n\n // [C.1]\n // loopcandidates consumer\n // Monitors the list `foundLoops` aka the putative loop candidates.\n // If new candidates are present in the list it computes the relative-pose\n // using GMSMatcher and theia-sfm's pnp. The depth is computed from stereogeom (class StereoGeometry)\n cer.loopcandidate_consumer_enable();\n // cer.loopcandidate_consumer_disable();\n std::thread loopcandidate_consumer_th( &Cerebro::loopcandiate_consumer_thread, &cer ); // runs @1hz\n\n // [D]\n // Kidnap Identification Thread\n cer.kidnaped_thread_enable();\n // cer.kidnaped_thread_disable();\n std::thread kidnap_th( &Cerebro::kidnaped_thread, &cer, 5 );\n\n // [E]\n // Visualization -\n Visualization viz(nh);\n viz.setDataManager( &dataManager );\n viz.setCerebro( &cer );\n viz.setVizPublishers( \"/cerebro_node/viz/\" );\n viz.run_thread_enable();\n // viz.run_thread_disable();\n std::thread viz_th( &Visualization::run, &viz, 10 ); //TODO something wrong with the logic in publish. another solution could be we keep #seq in DataNode.\n\n\n fs.release();\n\n ros::spin();\n\n // seem like user has pressed CTRL+C, so, stop all threads.\n dataManager.data_association_thread_disable();\n dataManager.trial_thread_disable();\n dataManager.clean_up_useless_images_thread_disable();\n #if 1\n cer.run_thread_disable();\n cer.descriptor_computer_thread_disable();\n cer.loopcandidate_consumer_disable();\n cer.kidnaped_thread_disable();\n viz.run_thread_disable();\n #endif\n\n data_association_th.join(); cout << \"data_association_th.join()\\n\";\n dm_trial_th.join(); cout << \"t1_trial.join()\\n\";\n dm_cleanup_th.join(); cout << \"dm_cleanup_th.join()\\n\";\n\n #if 1\n dot_product_th.join(); cout << \"dot_product_th.join()\\n\";\n desc_th.join(); cout << \"desc_th.join()\\n\";\n loopcandidate_consumer_th.join(); cout << \"loopcandidate_consumer_th.join()\\n\";\n kidnap_th.join(); cout << \"kidnap_th.join()\\n\";\n viz_th.join(); cout << \"viz_th.join()\\n\";\n #endif\n\n\n\n //make this to 1 to save state to file upon exit\n #define __SAVE_STATE__ 0\n #if __SAVE_STATE__\n // Save State (for relocalization)\n dataManager.saveStateToDisk( \"/Bulk_Data/chkpts_cerebro\" );\n #endif\n\n if( saveStateToDisk.compare(\"\") != 0 )\n {\n dataManager.saveStateToDisk( saveStateToDisk );\n }\n\n\n\n ///////////////////////////////////////////////////\n // A demo of how to look inside dataManager. //\n ///////////////////////////////////////////////////\n #if 0\n {\n std::map< ros::Time, DataNode* > data_map = dataManager.getDataMapRef();\n // auto descriptor_map = cer.getDescriptorMapRef();\n for( auto it = data_map.begin() ; it!= data_map.end() ; it++ )\n {\n cout << TermColor::RED() << \"---\" << TermColor::RESET() << endl;\n cout << \"Map-Key: \" << it->first << \"\\tseq=\" << std::distance( data_map.begin(), it ) << \"\\t\" << it->first - dataManager.getPose0Stamp() << endl;\n cout << \"Map-Value:\\n\";\n it->second->prettyPrint( );\n\n if( false && it->second->isImageAvailable() ) {\n cv::imshow( \"win\", it->second->getImage() );\n cv::waitKey(30);\n }\n }\n\n // Printing Global Variables\n cout << \"Pose0 : isAvailable=\" << dataManager.isPose0Available() << \"\\t\";\n cout << \"stamp=\" << dataManager.getPose0Stamp() ;\n cout << endl;\n\n cout << \"Camera:\\n\" ;\n dataManager.getCameraRef().printCameraInfo(2);\n\n cout << \"IMUCamExtrinsic : isAvailable=\" << dataManager.isIMUCamExtrinsicAvailable() << \"\\t\";\n cout << \"last updated : \" << dataManager.getIMUCamExtrinsicLastUpdated() << \"\\t\" << dataManager.getIMUCamExtrinsicLastUpdated() -dataManager.getPose0Stamp() ;\n cout << \"\\nimu_T_cam : \\n\" << PoseManipUtils::prettyprintMatrix4d( dataManager.getIMUCamExtrinsic() ) << endl;\n }\n #endif\n\n\n\n\n\n ///////////////////////\n // Actual Logging. //\n //////////////////////\n #define __LOGGING__ 0 // make this 1 to enable logging. 0 to disable logging. rememeber to catkin_make after this change\n #if __LOGGING__\n // Note: If using roslaunch to launch this node and when LOGGING is enabled,\n // roslaunch sends a sigterm and kills this thread when ros::ok() returns false ie.\n // when you press CTRL+C. The timeout is governed by roslaunch.\n //\n // If you wish to increase this timeout, you need to edit \"/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/nodeprocess.py\"\n // then edit these two vars.\n // _TIMEOUT_SIGINT = 15.0 #seconds\n // _TIMEOUT_SIGTERM = 2.0 #seconds\n // ^^^^ Borrowed from : https://answers.ros.org/question/11353/how-to-delay-escalation-to-sig-term-after-sending-sig-int-to-roslaunch/\n\n // Write json log\n // string save_dir = \"/Bulk_Data/_tmp/\";\n string save_dir = \"/tmp/_tmp/\";\n\n ROS_INFO( \"cerebro_node logging to : %s.\\nThis will take upto 2 minutes.\", save_dir.c_str());\n ROS_ERROR( \"cerebro_node logging to : %s.\\nThis will take upto 2 minutes.\", save_dir.c_str());\n ROS_WARN( \"cerebro_node logging to : %s.\\nThis will take upto 2 minutes.\", save_dir.c_str());\n\n //\n // $ rm -rf ${save_dir}\n // $ mkdir ${save_dir}\n #include \n string system_cmd;\n\n system_cmd = string(\"rm -rf \"+save_dir).c_str();\n // cout << TermColor::YELLOW() << system_cmd << TermColor::RESET() << endl;\n // int rm_dir_err = system( system_cmd.c_str() );\n const int rm_dir_err = RawFileIO::exec_cmd( system_cmd );\n\n if ( rm_dir_err == -1 )\n {\n cout << TermColor::RED() << \"[cerebro_node] Error removing directory!\\n\" << TermColor::RESET() << endl;\n exit(1);\n }\n\n system_cmd = string(\"mkdir -p \"+save_dir).c_str();\n // cout << TermColor::YELLOW() << system_cmd << TermColor::RESET() << endl;\n // const int dir_err = system( system_cmd.c_str() );\n const int dir_err = RawFileIO::exec_cmd( system_cmd );\n if ( dir_err == -1 )\n {\n cout << TermColor::RED() << \"[cerebro_node] Error creating directory!\\n\" << TermColor::RESET() << endl;\n exit(1);\n }\n // done emptying the directory.\n\n\n RawFileIO::write_string( save_dir+\"/log.json\", dataManager.asJson().dump(4) );\n // RawFileIO::write_string( save_dir+\"/log.json\", dataManager.metaDataAsJson() );\n // RawFileIO::write_string( save_dir+\"/log.txt\", dataManager.metaDataAsFlatFile() ); //TODO remove. Since i can read json in python as well as c++ with ease, these is no point of storing stuff as txt\n // return 0;\n\n #if 1\n // std::map< ros::Time, DataNode* > data_map = dataManager.getDataMapRef(); //old - can remove\n auto data_map = dataManager.getDataMapRef();\n auto img_data_mgr = dataManager.getImageManagerRef();\n for( auto it = data_map->begin() ; it!= data_map->end() ; it++ )\n {\n int seq_id = std::distance( data_map->begin() , it );\n\n string fname = save_dir+\"/\"+to_string(seq_id );\n #if 0\n //old code where we used images inside Nodes. - mark for removal\n if( it->second->isImageAvailable() )\n RawFileIO::write_image( fname+\".jpg\", it->second->getImage() );\n\n // additional image\n if( it->second->isImageAvailable(1) )\n RawFileIO::write_image( fname+\"_1.jpg\", it->second->getImage(1) );\n\n #else\n if( img_data_mgr->isImageRetrivable( \"left_image\", it->first ) )\n {\n cv::Mat outimg;\n img_data_mgr->getImage( \"left_image\", it->first, outimg );\n RawFileIO::write_image( fname+\".jpg\", outimg );\n }\n\n if( img_data_mgr->isImageRetrivable( \"right_image\", it->first ) )\n {\n cv::Mat outimg;\n img_data_mgr->getImage( \"right_image\", it->first, outimg );\n RawFileIO::write_image( fname+\"_1.jpg\", outimg );\n }\n #endif\n\n\n\n // Save VINS pose\n if( it->second->isPoseAvailable() ) {\n RawFileIO::write_EigenMatrix( fname+\".wTc\", it->second->getPose() );\n }\n\n // Save Point Cloud\n #if 0\n if( it->second->isPtCldAvailable() ) {\n RawFileIO::write_EigenMatrix( fname+\".wX.pointcloud\", it->second->getPointCloud() );\n if( it->second->isPoseAvailable() ) {\n RawFileIO::write_EigenMatrix( fname+\".cX.pointcloud\", it->second->getPose().inverse() * it->second->getPointCloud() );\n }\n else\n ROS_WARN( \"ptclod is available but pose is not available at seq_id=%d\", seq_id );\n }\n #endif\n\n\n // Save Tracked Points\n #if 0\n if( it->second->isUVAvailable() ) {\n RawFileIO::write_EigenMatrix( fname+\".unvn\", it->second->getUnVn() );\n RawFileIO::write_EigenMatrix( fname+\".uv\", it->second->getUV() );\n RawFileIO::write_EigenMatrix( fname+\".id\", it->second->getFeatIds() );\n }\n\n // Save Whole Image Descriptor\n if( it->second->isWholeImageDescriptorAvailable() ) {\n RawFileIO::write_EigenMatrix( fname+\".imgdesc\", it->second->getWholeImageDescriptor() );\n }\n #endif\n }\n #endif\n\n\n // Save Camera Matrix and IMUCamExtrinsic\n vector cam_keys = dataManager.getAbstractCameraKeys();\n for( short _icam=0 ; _icam < cam_keys.size() ; _icam++ )\n {\n short key=cam_keys[_icam];\n if( dataManager.isAbstractCameraSet(key) ) {\n auto abstract_camera = dataManager.getAbstractCameraRef(key);\n RawFileIO::write_string( save_dir+\"/cameraIntrinsic.\"+std::to_string(key)+\".info\", abstract_camera->parametersToString() );\n abstract_camera->writeParametersToYamlFile( save_dir+\"/cameraIntrinsic.\"+std::to_string(key)+\".yaml\" );\n }\n }\n\n\n // Save Camera-IMU Extrinsics\n cout << \"IMUCamExtrinsic : isAvailable=\" << dataManager.isIMUCamExtrinsicAvailable() << \"\\t\";\n cout << \"last updated : \" << dataManager.getIMUCamExtrinsicLastUpdated() << \"\\t\" << dataManager.getIMUCamExtrinsicLastUpdated() -dataManager.getPose0Stamp() ;\n cout << \"\\nimu_T_cam : \\n\" << PoseManipUtils::prettyprintMatrix4d( dataManager.getIMUCamExtrinsic() ) << endl;\n if( dataManager.isIMUCamExtrinsicAvailable() ) {\n RawFileIO::write_EigenMatrix( save_dir+\"/IMUCamExtrinsic.imu_T_cam\", dataManager.getIMUCamExtrinsic() );\n\n std::stringstream buffer;\n buffer << \"IMUCamExtrinsicLastUpdated: \"<< dataManager.getIMUCamExtrinsicLastUpdated() << endl;\n buffer << \"IMUCamExtrinsicLastUpdated Rel: \"<< dataManager.getIMUCamExtrinsicLastUpdated() -dataManager.getPose0Stamp() << endl;\n RawFileIO::write_string( save_dir+\"/IMUCamExtrinsic.info\", buffer.str() );\n }\n else {\n ROS_ERROR( \"[cerebro_node.cpp] IMUCamExtrinsic appear to be not set...something seem wrong\\n\" );\n }\n\n\n // Save foundLoops from Cerebro class\n json jsonout_obj = cer.foundLoops_as_JSON();\n RawFileIO::write_string( save_dir+\"/loopcandidates_liverun.json\", jsonout_obj.dump(4) );\n\n // Save kidnap info\n json json_kidnap_info = cer.kidnap_info_as_json();\n RawFileIO::write_string( save_dir+\"/kidnap_info.json\", json_kidnap_info.dump(4) );\n\n\n\n // Save matching images from ProcessedLoopCandidate\n string cmd_ = string(\"mkdir -p \")+save_dir+\"/matching/\";\n RawFileIO::exec_cmd( cmd_ );\n\n\n //******\n // If you are looking to ***save*** these matchings image pairs\n // besure to set the #define just before `Cerebro::loopcandiate_consumer_thread()`.\n\n json all_procloops_json_obj;\n for( int i=0 ; i 0 && __imm.cols > 0 ) {\n RawFileIO::write_image( save_dir+\"/matching/im_pair_\"+to_string(i)+\"_\"+c.debug_images_titles[l]+\".jpg\", __imm );\n }\n else {\n cout << \"in logging, matching_im_pair of ProcessedLoopCandidate[\" << i << \"] is not available\\n\";\n }\n }\n\n\n // Json of the object\n json procloops_json_obj;\n if( c.asJson(procloops_json_obj) ) {\n procloops_json_obj[\"processedLoops_i\"] = i;\n all_procloops_json_obj.push_back( procloops_json_obj );\n }\n else {\n cout << \"cannot make json for this ProcessedLoopCandidate[\" << i << \"]\\n\";\n }\n\n // Write the final poses as files\n if( c.isSet_3d2d__2T1 )\n RawFileIO::write_EigenMatrix( save_dir+\"/matching/im_pair_\"+to_string(i)+\".3d2d__2T1\", c._3d2d__2T1 );\n\n // Write final pose status image.\n string __final_pose_status_im_string = \"\";\n if( c.isSet_3d2d__2T1 ) {\n __final_pose_status_im_string = \"TRUE (poses were consistent);\";\n __final_pose_status_im_string += PoseManipUtils::prettyprintMatrix4d( c._3d2d__2T1 );\n } else {\n __final_pose_status_im_string = \"FALSE (non consistent poses);\";\n }\n cv::Mat __final_pose_status_im = cv::Mat::zeros(cv::Size(400, 20), CV_8UC3);\n MiscUtils::append_status_image( __final_pose_status_im, __final_pose_status_im_string );\n RawFileIO::write_image( save_dir+\"/matching/im_pair_\"+to_string(i)+\"_aaconsistency_status.jpg\", __final_pose_status_im );\n\n }\n RawFileIO::write_string( save_dir+\"/matching/ProcessedLoopCandidate.json\", all_procloops_json_obj.dump(4) );\n\n #endif\n\n\n\n\n return 0 ;\n\n}\n" }, { "path": "src/unittest/unittest_camera_geom_class_usage.cpp", "content": "#include \nusing namespace std;\n\n#include \n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n//opencv\n#include \n#include \n#include \n#include \n\n#include \"../utils/MiscUtils.h\"\n#include \"../utils/ElapsedTime.h\"\n#include \"../utils/PoseManipUtils.h\"\n#include \"../utils/TermColor.h\"\n#include \"../utils/CameraGeometry.h\"\n#include \"../utils/RawFileIO.h\"\n\n// #include \"gms_matcher.h\"\n\n#include \n\n#include \nusing namespace ceres;\n\n\n// Monocular\nint monocular_demo()\n{\n const std::string BASE = \"/Bulk_Data/_tmp/\";\n\n\n // Abstract Camera\n camodocal::CameraPtr m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.0.yaml\");\n cout << m_camera->parametersToString() << endl;\n\n // Init Monocular Geometry\n MonoGeometry monogeom( m_camera );\n\n // Raw Image - Image from camera\n int frame_id = 23;\n cv::Mat im_raw = cv::imread( BASE+\"/\"+std::to_string(frame_id)+\".jpg\" );\n\n cv::Mat im_undistorted;\n monogeom.do_image_undistortion( im_raw, im_undistorted );\n\n\n cv::imshow( \"im_raw\", im_raw );\n cv::imshow( \"im_undistorted\", im_undistorted );\n cv::waitKey(0);\n}\n\n\n// Stereo\nint stereo_demo()\n{\n const std::string BASE = \"/Bulk_Data/_tmp/\";\n\n //--------- Intrinsics load\n camodocal::CameraPtr left_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.0.yaml\");\n camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.1.yaml\");\n cout << left_camera->parametersToString() << endl;\n cout << right_camera->parametersToString() << endl;\n\n //----------- Stereo Base line load (alsoed called extrinsic calibration)\n // mynt eye\n // TODO function to load extrinsic from yaml file.\n // Vector4d q_xyzw = Vector4d( -1.8252509868889259e-04,-1.6291774489779708e-03,-1.2462127842978489e-03,9.9999787970731446e-01 );\n // Vector3d tr_xyz = Vector3d( -1.2075905420832895e+02/1000.,5.4110610639412482e-01/1000.,2.4484815673909591e-01/1000. );\n Vector4d q_xyzw = Vector4d( -7.0955103716253032e-04,-1.5775578725333590e-03,-1.2732644461763854e-03,9.9999769332040711e-01 );\n Vector3d tr_xyz = Vector3d( -1.2006984141573309e+02/1000.,3.3956264524978619e-01/1000.,-1.6784055634087214e-01/1000. );\n Matrix4d right_T_left;\n PoseManipUtils::raw_xyzw_to_eigenmat( q_xyzw, tr_xyz, right_T_left );\n\n\n std::shared_ptr stereogeom;\n stereogeom = std::make_shared( left_camera,right_camera, right_T_left );\n stereogeom->set_K( 375.0, 375.0, 376.0, 240.0 );\n\n\n int frame_id = 1005;\n cout << \"READ IMAGE \" << frame_id << endl;\n cv::Mat imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_id)+\".jpg\", 0 );\n cv::Mat imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_id)+\"_1.jpg\", 0 );\n\n cout << \"imleft_raw: \" << MiscUtils::cvmat_info( imleft_raw ) << endl;\n cout << \"imright_raw: \" << MiscUtils::cvmat_info( imright_raw ) << endl;\n\n\n if( imleft_raw.empty() || imright_raw.empty() ) {\n cout << \"imleft_raw is empty OR imright_raw is empty\\n\";\n return 0;\n }\n\n // will get 3d points, stereo-rectified image, and disparity false colormap\n MatrixXd _3dpts; //4xN\n cv::Mat imleft_srectified, imright_srectified;\n cv::Mat disparity_for_visualization;\n\n ElapsedTime timer;\n timer.tic();\n stereogeom->get_srectifiedim_and_3dpoints_and_disparity_from_raw_images(imleft_raw, imright_raw,\n imleft_srectified, imright_srectified,\n _3dpts, disparity_for_visualization );\n cout << timer.toc_milli() << \" (ms)!! get_srectifiedim_and_3dpoints_and_disparity_from_raw_images\\n\";\n\n cv::imshow( \"imleft_srectified\", imleft_srectified );\n cv::imshow( \"imright_srectified\", imright_srectified );\n cv::imshow( \"disparity_for_visualization\", disparity_for_visualization );\n\n cv::waitKey(0);\n return 0;\n\n}\n\n\n\n\nint main()\n{\n cout << \"Gello Horld\\n\";\n // monocular_demo();\n stereo_demo();\n}\n" }, { "path": "src/unittest/unittest_camodocal_proj.cpp", "content": "// A usage of my Stereo class.\n#include \nusing namespace std;\n\n\n\n\n/** Experiments to project 3d points on images.\n*/\n\n#include \n\n#include \"../utils/PoseManipUtils.h\"\n#include \"../utils/RawFileIO.h\"\n#include \"../utils/MiscUtils.h\"\n\n// #include \"PinholeCamera.h\"\n#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n\n// const string base_path = \"/Bulk_Data/_tmp_cerebro/tum_magistrale1/\";\nconst string base_path = \"/Bulk_Data/_tmp/\";\n\n// TODO Should return read status, ie. false if file-not-found\nbool load_i( int i, cv::Mat& im, Matrix4d& wTc, MatrixXd& uv, MatrixXd& cX )\n{\n string fname = base_path+\"/\"+std::to_string( i );\n\n // Image\n im = cv::imread( fname+\".jpg\" );\n // cv::imshow( \"win\", im );\n\n // VINS-pose (odometry)\n RawFileIO::read_eigen_matrix( fname+\".wTc\", wTc );\n\n // 3D points - in camera cords\n RawFileIO::read_eigen_matrix( fname+\".cX.pointcloud\", cX );\n\n // 2D points - tracked points\n RawFileIO::read_eigen_matrix( fname+\".uv\", uv );\n\n}\n\n// TODO Should return read status, ie. false if file-not-found\nbool load_i( int i, cv::Mat& im, Matrix4d& wTc, MatrixXd& uv, MatrixXd& cX, VectorXi& uv_ids )\n{\n string fname = base_path+\"/\"+std::to_string( i );\n\n // Image\n im = cv::imread( fname+\".jpg\" );\n // cv::imshow( \"win\", im );\n\n // VINS-pose (odometry)\n RawFileIO::read_eigen_matrix( fname+\".wTc\", wTc );\n\n // 3D points - in camera cords\n RawFileIO::read_eigen_matrix( fname+\".cX.pointcloud\", cX );\n\n // 2D points - tracked points\n RawFileIO::read_eigen_matrix( fname+\".uv\", uv );\n\n // IDs\n RawFileIO::read_eigen_matrix( fname+\".id\", uv_ids );\n\n}\n\nint main()\n{\n //\n // Load Camera - from yaml\n std::string calib_file = base_path+\"/cameraIntrinsic.0.yaml\";\n camodocal::CameraPtr m_camera;\n m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(calib_file);\n std::cout << ((m_camera)?\"cam is initialized\":\"cam is not initiazed\") << std::endl; //this should print 'initialized'\n\n\n\n\n\n // Load 1st\n int _idx_1 = 1331;\n cv::Mat im_1;\n Matrix4d wTc_1;\n MatrixXd uv_1, cX_1;\n VectorXi uv_id_1;\n load_i( _idx_1, im_1, wTc_1, uv_1, cX_1, uv_id_1 );\n // cv::imshow( \"533\", im_1 );\n {\n cv::Mat dst;\n MiscUtils::plot_point_sets( im_1, uv_1, dst, cv::Scalar(255,0,0), uv_id_1, \"self uv\" );\n cv::imshow( std::to_string(_idx_1), dst );\n }\n\n\n // // Load 2nd\n int _idx_2 = 839;\n cv::Mat im_2;\n Matrix4d wTc_2;\n MatrixXd uv_2, cX_2;\n VectorXi uv_id_2;\n // load_i( 536, im_2, wTc_2, uv_2, cX_2, uv_id_2 );\n // load_i( 887, im_2, wTc_2, uv_2, cX_2 );\n load_i( _idx_2, im_2, wTc_2, uv_2, cX_2, uv_id_2 );\n // cv::imshow( \"535\", im_2 );\n {\n cv::Mat dst;\n MiscUtils::plot_point_sets( im_2, uv_2, dst, cv::Scalar(255,255,255), uv_id_2, \"self uv\" );\n cv::imshow( std::to_string(_idx_2), dst );\n }\n\n\n // project 1st 3d points using camodocal\n MatrixXd reprojected_uv_1 = MatrixXd::Zero( 2, cX_1.cols() );\n for( int i=0 ; ispaceToPlane( P, p );\n cout << \"---\\n\" << p << endl;\n reprojected_uv_1.col(i) = p;\n }\n cv::Mat dst;\n // MiscUtils::plot_point_sets( im_1, uv_1, dst, cv::Scalar(255,0,0), uv_id_1, \"self uv in blue\" );\n MiscUtils::plot_point_sets( im_2, reprojected_uv_1, dst, cv::Scalar(0,0,255), uv_id_1, \";self reproejct in red\" );\n cv::imshow( \"reproejct\", dst );\n\n\n cv::waitKey(0);\n return 0;\n\n}\n" }, { "path": "src/unittest/unittest_plot2mat.cpp", "content": "#include \nusing namespace std;\n\n#include \"../utils/Plot2Mat.h\"\nint demo()\n{\n cout << \"Hello\\n\";\n Plot2Mat han;\n han.setYminmax( -2, 2 );\n\n for( int i=10; i<50; i++ ) {\n VectorXd y = VectorXd::Random(10*i);\n cout << \"y=\" << y.transpose() << endl;\n\n han.mark( i );\n\n han.plot(y);\n cv::imshow(\"canvas\", han.getCanvasConstPtr() );\n cv::waitKey(0);\n\n han.resetCanvas();\n }\n\n cout << \"Finished...!\\n\";\n}\n\n\nint demo_easy() {\n cout <<\"demo_easy\\n\";\n\n Plot2Mat han;\n han.setYminmax( -2, 2 );\n\n VectorXd y = VectorXd::Random(10);\n cout << \"y=\" << y.transpose() << endl;\n\n han.mark( 4 );\n han.plot(y, cv::Scalar(0,255,0), true, true);\n // han.plot(y);\n cv::imshow(\"canvas\", han.getCanvasConstPtr() );\n cv::waitKey(0);\n\n\n}\n\nint main() {\n demo_easy();\n // demo();\n}\n" }, { "path": "src/unittest/unittest_pose_tester.cpp", "content": "// This unittest will test and evaluate\n// ` StaticTheiaPoseCompute::P3P_ICP` and `StaticTheiaPoseCompute::PNP`\n// from DlsPnpWithRansac.h/cpp\n// Author : Manohar Kuse \n// Created : 14th May, 2019\n//\n\n\n#include \n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n//opencv\n#include \n#include \n#include \n#include \n\n// Eigen\n#include \n#include \nusing namespace Eigen;\nusing namespace std;\n\n// custom\n#include \"../utils/CameraGeometry.h\"\n#include \"../utils/TermColor.h\"\n#include \"../utils/RawFileIO.h\"\n\n#include \"../utils/PointFeatureMatching.h\"\n// #include \"../utils/GMSMatcher/gms_matcher.h\"\n\n\n#include \"../DlsPnpWithRansac.h\"\n\nconst std::string BASE = \"/Bulk_Data/_tmp/\";\n\nstd::shared_ptr make_stereo_geom()\n{\n //--------- Intrinsics load\n camodocal::CameraPtr left_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.0.yaml\");\n camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.1.yaml\");\n // camodocal::CameraPtr left_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(string(BASE+\"../leveled_cam_pinhole/\")+\"/camera_left.yaml\");\n // camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(string(BASE+\"../leveled_cam_pinhole/\")+\"/camera_right.yaml\");\n\n cout << left_camera->parametersToString() << endl;\n cout << right_camera->parametersToString() << endl;\n\n if( !left_camera || !right_camera )\n {\n cout << \"\\nERROR : Abstract stereo Camera is not set. You need to init camera before setting it to geometry clases\\n\";\n exit(10);\n }\n\n\n //----------- Stereo Base line load (alsoed called extrinsic calibration)\n // mynt eye\n // Vector4d q_xyzw = Vector4d( -7.0955103716253032e-04,-1.5775578725333590e-03,-1.2732644461763854e-03,9.9999769332040711e-01 );\n // Vector3d tr_xyz = Vector3d( -1.2006984141573309e+02/1000.,3.3956264524978619e-01/1000.,-1.6784055634087214e-01/1000. );\n\n // realsense\n Vector4d q_xyzw = Vector4d( 0.0, 0.0, 0.0, 1.0 );\n Vector3d tr_xyz = Vector3d( -49.8/1000., 0.0/1000., 0.0/1000. );\n\n Matrix4d right_T_left;\n PoseManipUtils::raw_xyzw_to_eigenmat( q_xyzw, tr_xyz, right_T_left );\n cout << \"XXright_T_left: \" << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;\n\n // StereoGeometry* sthm = new StereoGeometry( left_camera, right_camera, right_T_left );\n // cout << \"sthn->get_K() \" << sthm->get_K() << endl;\n\n\n // StereoGeometry NNN = StereoGeometry( left_camera, right_camera, right_T_left );\n // cout << \"NNN->get_K() \" << NNN.get_K() << endl;\n\n //-------------------- init stereogeom\n std::shared_ptr stereogeom;\n stereogeom = std::make_shared( left_camera,right_camera, right_T_left );\n // stereogeom->set_K( 375.0, 375.0, 376.0, 240.0 );\n\n cout << \"[in make_stereo_geom]\" << stereogeom->get_K() << endl;\n\n return stereogeom;\n\n}\n\nbool compute_rel_pose( std::shared_ptr stereogeom, int frame_a, int frame_b )\n{\n //------------------------------------------------\n //------ 3d points from frame_a\n //------------------------------------------------\n cout << TermColor::iGREEN() << \"LOAD[A]: BASE/\"+std::to_string(frame_a)+\".jpg\" << TermColor::RESET() << endl;\n cv::Mat a_imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_a)+\".jpg\", 0 );\n cv::Mat a_imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_a)+\"_1.jpg\", 0 );\n Matrix4d wTA;\n RawFileIO::read_eigen_matrix( BASE+\"/\"+std::to_string(frame_a)+\".wTc\", wTA );\n cout << \"wTA: \" << PoseManipUtils::prettyprintMatrix4d( wTA ) << endl;\n if( a_imright_raw.empty() || a_imright_raw.empty() )\n {\n cout << TermColor::RED() << \"Cannot read image with idx=\" << frame_a << \" perhaps filenotfound\" << TermColor::RESET() << endl;\n return false;\n }\n\n\n cv::Mat a_imleft_srectified, a_imright_srectified;\n cv::Mat a_3dImage;\n MatrixXd a_3dpts;\n cv::Mat a_disp_viz;\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images( a_imleft_raw, a_imright_raw,\n a_imleft_srectified,a_imright_srectified, a_3dpts, a_3dImage, a_disp_viz );\n\n\n\n //-----------------------------------------------------\n //--------------- 3d points from frame_b\n //-----------------------------------------------------\n cout << TermColor::iGREEN() << \"LOAD[B]: BASE/\"+std::to_string(frame_b)+\".jpg\" << TermColor::RESET() << endl;\n cv::Mat b_imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_b)+\".jpg\", 0 );\n cv::Mat b_imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_b)+\"_1.jpg\", 0 );\n Matrix4d wTB;\n RawFileIO::read_eigen_matrix( BASE+\"/\"+std::to_string(frame_b)+\".wTc\", wTB );\n cout << \"wTB: \" << PoseManipUtils::prettyprintMatrix4d( wTB ) << endl;\n if( b_imleft_raw.empty() || b_imright_raw.empty() )\n {\n cout << TermColor::RED() << \"Cannot read image with idx=\" << frame_b << \" perhaps filenotfound\" << TermColor::RESET() << endl;\n return false;\n }\n cv::Mat b_imleft_srectified, b_imright_srectified;\n cv::Mat b_3dImage;\n MatrixXd b_3dpts;\n cv::Mat b_disp_viz;\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images( b_imleft_raw, b_imright_raw,\n b_imleft_srectified, b_imright_srectified, b_3dpts, b_3dImage, b_disp_viz );\n\n\n cout << \"ODOM b_T_a: \" << PoseManipUtils::prettyprintMatrix4d( wTB.inverse() * wTA ) << endl;\n\n //---------------------------------------------------------------------\n //------------ point matches between a_left, b_left\n //---------------------------------------------------------------------\n cout << TermColor::iGREEN() << \"point matches between a_left, b_left\" << TermColor::RESET() << endl;\n MatrixXd uv, uv_d; // u is from frame_a; ud is from frame_b\n ElapsedTime timer;\n timer.tic();\n StaticPointFeatureMatching::gms_point_feature_matches(a_imleft_srectified, b_imleft_srectified, uv, uv_d );\n string msg_pf_matches = to_string( timer.toc_milli() )+\" (ms) elapsed time for point_feature_matches computation\";\n if( uv.cols() < 30 ) {\n cout << TermColor::RED() << \"too few gms matches between \" << frame_a << \" and \" << frame_b << TermColor::RESET() << endl;\n return false;\n }\n\n\n #if 1 // plot ppoint-feature matches\n cv::Mat dst_feat_matches;\n MiscUtils::plot_point_pair( a_imleft_srectified, uv, frame_a,\n b_imleft_srectified, uv_d, frame_b,\n dst_feat_matches, 3, msg_pf_matches+\";#pf-matches: \"+to_string( uv.cols() ) );\n cv::resize(dst_feat_matches, dst_feat_matches, cv::Size(), 0.5, 0.5 );\n cv::imshow( \"dst_feat_matches\", dst_feat_matches );\n #endif\n\n #if 0 // disparty maps of both images\n cv::Mat dst_disp;\n MiscUtils::side_by_side( a_disp_viz, b_disp_viz, dst_disp );\n MiscUtils::append_status_image( dst_disp, \"a=\"+ to_string(frame_a)+\" b=\"+to_string(frame_b), .8 );\n cv::resize(dst_disp, dst_disp, cv::Size(), 0.5, 0.5 );\n cv::imshow( \"dst_disp\", dst_disp );\n #endif\n\n\n //----------------------------------------------------------------------\n //----------- compute pose\n //----------------------------------------------------------------------\n cout << TermColor::iGREEN() << \"compute pose\" << TermColor::RESET() << endl;\n #if 0 // pnp( 3d_pts_of_a, 2d_pts_of_b )\n cout << TermColor::iBLUE() << \" pnp( 3d_pts_of_a, 2d_pts_of_b )\" << TermColor::RESET() << endl;\n\n\n //-----------prep data\n std::vector feature_position_uv; //< these will be normalized image co-ordinates\n std::vector feature_position_uv_d; //< these will be normalized image co-ordinates\n std::vector world_point_uv;\n StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity(\n stereogeom, uv, a_3dImage, uv_d,\n feature_position_uv, feature_position_uv_d, world_point_uv);\n\n\n //----------initial guess\n Matrix4d out_b_T_a = Matrix4d::Identity();\n out_b_T_a = wTB.inverse() * wTA ;\n cout << TermColor::YELLOW() << \"initial_guess___out_b_T_a : \" << PoseManipUtils::prettyprintMatrix4d( out_b_T_a ) << TermColor::RESET() << endl;\n\n\n //---------- PNP\n string ceres_pnp_msh;\n StaticCeresPoseCompute::PNP( world_point_uv, feature_position_uv_d, out_b_T_a, ceres_pnp_msh );\n\n Matrix4d out_b_T_a111 = Matrix4d::Identity();\n string usual_msg;\n StaticTheiaPoseCompute::PNP( world_point_uv, feature_position_uv_d, out_b_T_a111, usual_msg );\n cout << TermColor::YELLOW() << \"final_soltion___out_b_T_a(theia) : \" << PoseManipUtils::prettyprintMatrix4d( out_b_T_a111 ) << TermColor::RESET() << endl;\n\n\n //--------final solution\n cout << TermColor::YELLOW() << \"final_soltion___out_b_T_a(ceres) : \" << PoseManipUtils::prettyprintMatrix4d( out_b_T_a ) << TermColor::RESET() << endl;\n\n\n #if 1 //verification\n\n //---------------------------------------------\n //--- Use the theia's estimated pose do PI( b_T_a * 3dpts ) and plot these\n //--- points on B. Also plot detected points of B\n //---------------------------------------------\n #if 1\n MatrixXd _3dpts_of_A_projectedonB = MatrixXd::Zero(3, world_point_uv.size() );\n MatrixXd _detectedpts_of_B = MatrixXd::Zero(3, world_point_uv.size() );\n MatrixXd _detectedpts_of_A = MatrixXd::Zero(3, world_point_uv.size() );\n\n int n_good = 0;\n for( int i=0 ; iget_K() * _X_i; //< scaling with camera-intrinsic matrix\n\n\n Vector3d _tmp;\n _tmp << feature_position_uv_d[i], 1.0 ;\n _detectedpts_of_B.col(i) = stereogeom->get_K() * _tmp;\n\n _tmp << feature_position_uv[i], 1.0 ;\n _detectedpts_of_A.col(i) = stereogeom->get_K() * _tmp;\n\n\n Vector3d delta = _3dpts_of_A_projectedonB.col(i) - _detectedpts_of_B.col(i);\n // cout << \"i=\" << i << \" \";\n if( abs(delta(0)) < 2. && abs(delta(1)) < 2. ) {\n // cout << \" delta=\" << delta.transpose();\n n_good++;\n }\n else {\n // cout << TermColor::RED() << \"delta=\" << delta.transpose() << TermColor::RESET();\n }\n // cout << endl;\n }\n cout << \"n_good=\" <get_K() * _X_i; //< scaling with camera-intrinsic matrix\n }\n\n #endif // PI( odom_b_T_a * 3dpts )\n\n\n #if 1\n // plot( B, ud )\n cv::Mat __dst;\n MiscUtils::plot_point_sets( b_imleft_srectified, _detectedpts_of_B, __dst, cv::Scalar( 0,0,255), false, \"_detectedpts_of_B (red)\" );\n // cv::imshow( \"plot( B, ud )\", __dst );\n\n // plot( B, _3dpts_of_A_projectedonB )\n MiscUtils::plot_point_sets( __dst, _3dpts_of_A_projectedonB, cv::Scalar( 0,255,255), false, \";_3dpts_of_A_projectedonB, PI( b_T_a * X),(yellow)\" );\n // MiscUtils::plot_point_sets( b_imleft_srectified, _3dpts_of_A_projectedonB, __dst, cv::Scalar( 0,255,255), false, \"_3dpts_of_A_projectedonB\" );\n\n MiscUtils::plot_point_sets( __dst, _detectedpts_of_A, cv::Scalar( 255,255,255), false, \";;_detectedpts_of_A(white)\" );\n\n MiscUtils::plot_point_sets( __dst, _3dpts_of_A_projectedonB_with_odom_rel_pose, cv::Scalar( 255,0,0), false, \";;;_3dpts_of_A_projectedonB_with_odom_rel_pose, PI( odom_b_T_a * X),(blue)\" );\n\n string status_msg = \"\";\n status_msg += \"_detectedpts_of_B (red);\";\n status_msg += \"_3dpts_of_A_projectedonB, PI( b_T_a * X),(yellow);\";\n status_msg += \"_detectedpts_of_A(white);\";\n status_msg += \"_3dpts_of_A_projectedonB_with_odom_rel_pose, PI( odom_b_T_a * X),(blue)\";\n MiscUtils::append_status_image( __dst, status_msg );\n\n cv::imshow( \"b_imleft_srectified\", __dst );\n #endif //plotting\n\n #endif //verification\n\n #endif //option-1\n\n\n\n #if 1 // p3p( 3d_pts_of_a, 3d_pts_of_b )\n cout << TermColor::iBLUE() << \" p3p( 3d_pts_of_a, 3d_pts_of_b )\" << TermColor::RESET() << endl;\n\n //---------prep data\n vector< Vector3d> uv_X;\n vector< Vector3d> uvd_Y;\n StaticPointFeatureMatching::make_3d_3d_collection__using__pfmatches_and_disparity( uv, a_3dImage, uv_d, b_3dImage,\n uv_X, uvd_Y );\n\n //-------------initial guess\n Matrix4d icp_b_T_a;\n icp_b_T_a = wTB.inverse() * wTA ;\n cout << TermColor::YELLOW() << \"initial icp_b_T_a(ceres) : \" << PoseManipUtils::prettyprintMatrix4d( icp_b_T_a ) << TermColor::RESET() << endl;\n\n\n //------------------p3p (icp)\n string p3p__msg;\n // float p3p_goodness = StaticTheiaPoseCompute::P3P_ICP( uv_X, uvd_Y, icp_b_T_a, p3p__msg );\n float p3p_goodness = StaticCeresPoseCompute::P3P_ICP( uv_X, uvd_Y, icp_b_T_a, p3p__msg );\n\n\n\n //--------final solution\n cout << TermColor::YELLOW() << \"final icp_b_T_a(ceres) : \" << PoseManipUtils::prettyprintMatrix4d( icp_b_T_a ) << TermColor::RESET() << endl;\n\n #endif //option-3\n\n}\n\nint main(int argc, char ** argv )\n{\n //---\n cout << TermColor::iGREEN() << \"StereoGeometry\\n\" << TermColor::RESET() ;\n auto stereogeom = make_stereo_geom();\n\n\n //--- Cmdline\n if( argc != 3 ) {\n cout << \"[MAIN::INVALID USAGE] I am expecting you to give two numbers in the command line between which you want to compute the pose\\n\";\n exit(1);\n }\n Matrix4d out_b_T_a = Matrix4d::Identity();\n int a = stoi( argv[1] );\n int b = stoi( argv[2] );\n\n\n //--\n compute_rel_pose( stereogeom, a, b );\n cv::waitKey(0);\n\n\n\n}\n\n\n#include \"../utils/nlohmann/json.hpp\"\nusing json = nlohmann::json;\nint main1( int argc, char ** argv )\n{\n std::shared_ptr stereogeom = make_stereo_geom();\n\n // load ProcessedLoopCandidate.json\n cout << \"Open JSON : \" << BASE+\"/matching/ProcessedLoopCandidate.json\" << endl;\n std::ifstream fp(BASE+\"/matching/ProcessedLoopCandidate.json\");\n json proc_candi_json;\n fp >> proc_candi_json;\n for( int i =0 ; i\n#include \n#include \n\n#include \n\n//opencv\n#include \n#include \n#include \n#include \n\n\nint main( int argc, char ** argv )\n{\n // Ros INIT and Make connection to Server\n ros::init( argc, argv, \"unittest_rosservice_client_cpp\" );\n ros::NodeHandle n;\n ros::ServiceClient client = n.serviceClient( \"whole_image_descriptor_compute\" );\n client.waitForExistence();\n if( !client.exists() ) {\n ROS_ERROR( \"Connection to server NOT successful\" );\n return 0;\n }\n else std::cout << \"Connection to server established\\n\";\n\n // Make service message\n cerebro::WholeImageDescriptorCompute srv; //service message\n cv::Mat im = cv::imread( \"/app/lena.jpg\") ;\n cv::Mat im_resized;\n cv::resize( im, im_resized, cv::Size(640, 512) );\n cv::imshow( \"lena\", im );\n cv::waitKey(0);\n\n // call\n sensor_msgs::ImagePtr image_msg = cv_bridge::CvImage(std_msgs::Header(), \"bgr8\", im_resized).toImageMsg();\n srv.request.ima = *image_msg;\n srv.request.a = 678;\n if( client.call( srv ) ) {\n ROS_INFO( \"Received response from server\" );\n std::cout << \"desc.size=\" << srv.response.desc.size() << std::endl;\n }\n else {\n ROS_ERROR( \"Failed to call ros service\" );\n }\n\n return 0;\n\n}\n" }, { "path": "src/unittest/unittest_termcolor.cpp", "content": "#include \n\n#include \"../utils/TermColor.h\"\n// #include \"../utils/PoseManipUtils.h\"\n\nusing namespace std;\nint main()\n{\n cout << \"Hello World\\n\";\n cout << TermColor::RED() << \"Hello World in red\" << TermColor::RESET() << endl;\n cout << TermColor::iRED() << \"Hello World in red\" << TermColor::RESET() << endl;\n\n\n cout << TermColor::compose( TermColor::CTRL_BOLD, TermColor::FG_RED, true ) << \"Hello\" << TermColor::RESET() << endl;\n cout << TermColor::compose( TermColor::CTRL_BOLD, TermColor::FG_RED, false ) << \"Hello\" << TermColor::RESET() << endl;\n cout << TermColor::compose( TermColor::FG_BLUE, true ) << \"Hello\" << TermColor::RESET() << endl;\n cout << TermColor::compose( TermColor::FG_BLUE, false ) << \"Hello\" << TermColor::RESET() << endl;\n\n cout << TermColor::compose( TermColor::BG_CYAN ) << \"Hello\" << TermColor::RESET() << endl;\n cout << TermColor::compose( TermColor::CTRL_UNDERLINE, TermColor::BG_GREEN ) << \"Hello\" << TermColor::RESET() << endl;\n return 0;\n}\n" }, { "path": "src/unittest/unittest_theia.cpp", "content": "// Opens the log.json file :\n// A) Plots the vio poses\n// B) plot loop candidates\n// c) at loop candidate compute relative pose using pnp\n// d) plot new camera and old camera.\n\n\n#include \n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n//opencv\n#include \n#include \n#include \n#include \n\n#include \"../utils/CameraGeometry.h\"\n\n\n#include \"../utils/MiscUtils.h\"\n#include \"../utils/ElapsedTime.h\"\n#include \"../utils/PoseManipUtils.h\"\n#include \"../utils/TermColor.h\"\n#include \"../utils/RawFileIO.h\"\n#include \"../utils/RosMarkerUtils.h\"\n\n#include \"../utils/nlohmann/json.hpp\"\nusing json = nlohmann::json;\n\n#include \"../utils/GMSMatcher/gms_matcher.h\"\n#include \n\n\nconst std::string BASE = \"/Bulk_Data/_tmp/\";\n\n\n\n\n/////////// DlsPnp-RANSAC ///////////////////\n// Data\nstruct CorrespondencePair_3d2d {\n Vector3d a_X; // 3d point expressed in co-ordinate system of `image-a`\n Vector2d uv_d; //feature point in normalized-image co-ordinates. of the other view (b)\n};\n\n// Model\nstruct RelativePose {\n Matrix4d b_T_a;\n};\n\n\nclass DlsPnpWithRansac: public theia::Estimator {\npublic:\n // Number of points needed to estimate a line.\n double SampleSize() const { return 15; }\n\n // Estimate a pose from N points\n bool EstimateModel( const std::vector& data,\n std::vector* models) const {\n\n // cout << \"EstimateModel: data.size=\"<< data.size() << \" \" << data[0].uv_d.transpose() << endl;\n std::vector feature_position;\n std::vector world_point;\n for( int i=0 ; i solution_rotations;\n std::vector solution_translations;\n\n theia::DlsPnp( feature_position, world_point, &solution_rotations, &solution_translations );\n if( solution_rotations.size() == 1 ) {\n RelativePose r;\n r.b_T_a = Matrix4d::Identity();\n r.b_T_a.topLeftCorner(3,3) = solution_rotations[0].toRotationMatrix();\n r.b_T_a.col(3).topRows(3) = solution_translations[0];\n models->push_back( r );\n return true;\n }\n else {\n return false;\n }\n }\n\n\n double Error( const CorrespondencePair_3d2d& point, const RelativePose& r ) const {\n Vector3d b_X = r.b_T_a.topLeftCorner(3,3) * point.a_X + r.b_T_a.col(3).topRows(3);\n double depth = b_X(2);\n b_X /= depth;\n double reproj_error = abs(b_X(0) - point.uv_d(0)) + abs(b_X(1) - point.uv_d(1));\n\n // I want nearby points to have smaller Error than far off points.\n // So artificially increase the error when depth is higher for the point.\n double f = 1.;\n #if 0\n if( depth < 1. )\n f = .5;\n if( depth >=1 && depth < 3 )\n f = 5.0;\n if( depth >=3 && depth < 8 )\n f = 2.;\n if( depth >= 8. && depth < 15 )\n f = 1.7;\n if( depth >= 15 )\n f = 0.1;\n #endif\n\n return reproj_error*f;\n\n }\n};\n/////////// END DlsPnp-RANSAC ///////////////////\n\n\n\n//////////////////// AlignPointCloudsUmeyama with Ransac ///////////////////////\n// Data\nstruct CorrespondencePair_3d3d {\n Vector3d a_X; // 3d point expressed in co-ordinate system of `image-a`\n Vector3d b_X; // 3d point expressed in co-ordinate system of `image-b`\n};\n\n// Model\n// struct RelativePose {\n // Matrix4d b_T_a;\n// };\n\nclass AlignPointCloudsUmeyamaWithRansac: public theia::Estimator {\npublic:\n // Number of points needed to estimate a line.\n double SampleSize() const { return 10; }\n\n bool EstimateModel( const std::vector& data,\n std::vector* models) const {\n //TODO\n\n std::vector a_X;\n std::vector b_X;\n for( int i=0 ; i 0.9 ) {\n RelativePose r;\n r.b_T_a = Matrix4d::Identity();\n r.b_T_a.topLeftCorner(3,3) = ____R;\n r.b_T_a.col(3).topRows(3) = ____t;\n models->push_back( r );\n // cout << \"Estimate s=\" << ___s << \" \" << PoseManipUtils::prettyprintMatrix4d(r.b_T_a)<< endl;\n return true;\n } else {\n return false;\n }\n\n }\n\n double Error( const CorrespondencePair_3d3d& point, const RelativePose& r ) const {\n Vector3d b_X_cap = r.b_T_a.topLeftCorner(3,3) * point.a_X + r.b_T_a.col(3).topRows(3);\n\n double err = (b_X_cap - point.b_X).norm();\n\n double f=1.0;\n if( point.a_X(2) < 1. && point.a_X(2) > 8. )\n f = 1.2;\n\n // cout << \"ICP RAnsac error=\" << err << endl;\n\n return f*err;\n\n }\n\n};\n\n//////////////////// END AlignPointCloudsUmeyama with Ransac ///////////////////////\n\nvoid point_feature_matches( const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n MatrixXd& u, MatrixXd& ud )\n{\n ElapsedTime timer;\n\n\n //\n // Point feature and descriptors extract\n std::vector kp1, kp2;\n cv::Mat d1, d2; //< descriptors\n\n cv::Ptr orb = cv::ORB::create(5000);\n orb->setFastThreshold(0);\n\n timer.tic();\n orb->detectAndCompute(imleft_undistorted, cv::Mat(), kp1, d1);\n orb->detectAndCompute(imright_undistorted, cv::Mat(), kp2, d2);\n cout << timer.toc_milli() << \" (ms) 2X detectAndCompute(ms) : \"<< endl;\n // std::cout << \"d1 \" << MiscUtils::cvmat_info( d1 ) << std::endl;\n // std::cout << \"d2 \" << MiscUtils::cvmat_info( d2 ) << std::endl;\n\n //plot\n // cv::Mat dst_left, dst_right;\n // MatrixXd e_kp1, e_kp2;\n // MiscUtils::keypoint_2_eigen( kp1, e_kp1 );\n // MiscUtils::keypoint_2_eigen( kp2, e_kp2 );\n // MiscUtils::plot_point_sets( imleft_undistorted, e_kp1, dst_left, cv::Scalar(0,0,255), false );\n // MiscUtils::plot_point_sets( imright_undistorted, e_kp2, dst_right, cv::Scalar(0,0,255), false );\n // cv::imshow( \"dst_left\", dst_left );\n // cv::imshow( \"dst_right\", dst_right );\n\n //\n // Point feature matching\n cv::BFMatcher matcher(cv::NORM_HAMMING); // TODO try FLANN matcher here.\n vector matches_all, matches_gms;\n timer.tic();\n matcher.match(d1, d2, matches_all);\n std::cout << timer.toc_milli() << \" : (ms) BFMatcher took (ms)\\t\";\n std::cout << \"BFMatcher : npts = \" << matches_all.size() << std::endl;\n\n\n // gms_matcher\n timer.tic();\n std::vector vbInliers;\n gms_matcher gms(kp1, imleft_undistorted.size(), kp2, imright_undistorted.size(), matches_all);\n int num_inliers = gms.GetInlierMask(vbInliers, false, false);\n cout << timer.toc_milli() << \" : (ms) GMSMatcher took.\\t\" ;\n cout << \"Got total gms matches \" << num_inliers << \" matches.\" << endl;\n\n // collect matches\n for (size_t i = 0; i < vbInliers.size(); ++i)\n {\n if (vbInliers[i] == true)\n {\n matches_gms.push_back(matches_all[i]);\n }\n }\n // MatrixXd M1, M2;\n MiscUtils::dmatch_2_eigen( kp1, kp2, matches_gms, u, ud, true );\n\n\n}\n\n\n\n\nstd::shared_ptr make_stereo_geom()\n{\n //--------- Intrinsics load\n camodocal::CameraPtr left_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.0.yaml\");\n camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.1.yaml\");\n // camodocal::CameraPtr left_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(string(BASE+\"../leveled_cam_pinhole/\")+\"/camera_left.yaml\");\n // camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(string(BASE+\"../leveled_cam_pinhole/\")+\"/camera_right.yaml\");\n\n cout << left_camera->parametersToString() << endl;\n cout << right_camera->parametersToString() << endl;\n\n if( !left_camera || !right_camera )\n {\n cout << \"\\nERROR : Abstract stereo Camera is not set. You need to init camera before setting it to geometry clases\\n\";\n exit(10);\n }\n\n\n //----------- Stereo Base line load (alsoed called extrinsic calibration)\n // mynt eye\n Vector4d q_xyzw = Vector4d( -7.0955103716253032e-04,-1.5775578725333590e-03,-1.2732644461763854e-03,9.9999769332040711e-01 );\n Vector3d tr_xyz = Vector3d( -1.2006984141573309e+02/1000.,3.3956264524978619e-01/1000.,-1.6784055634087214e-01/1000. );\n Matrix4d right_T_left;\n PoseManipUtils::raw_xyzw_to_eigenmat( q_xyzw, tr_xyz, right_T_left );\n cout << \"XXright_T_left: \" << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;\n\n // StereoGeometry* sthm = new StereoGeometry( left_camera, right_camera, right_T_left );\n // cout << \"sthn->get_K() \" << sthm->get_K() << endl;\n\n\n // StereoGeometry NNN = StereoGeometry( left_camera, right_camera, right_T_left );\n // cout << \"NNN->get_K() \" << NNN.get_K() << endl;\n\n //-------------------- init stereogeom\n std::shared_ptr stereogeom;\n stereogeom = std::make_shared( left_camera,right_camera, right_T_left );\n // stereogeom->set_K( 375.0, 375.0, 376.0, 240.0 );\n\n cout << \"[in make_stereo_geom]\" << stereogeom->get_K() << endl;\n\n return stereogeom;\n\n}\n\n\n// will return false if cannot compute pose\nbool relative_pose_compute_with_theia( std::shared_ptr stereogeom,\n int frame_a, int frame_b,\n Matrix4d& out_b_T_a )\n{\n ElapsedTime timer;\n\n //------------------------------------------------\n //------ 3d points from frame_a\n //------------------------------------------------\n cout << TermColor::iGREEN() << \"LOAD: BASE/\"+std::to_string(frame_a)+\".jpg\" << TermColor::RESET() << endl;\n cv::Mat a_imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_a)+\".jpg\", 0 );\n cv::Mat a_imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_a)+\"_1.jpg\", 0 );\n if( a_imright_raw.empty() || a_imright_raw.empty() )\n {\n cout << TermColor::RED() << \"Cannot read image with idx=\" << frame_a << \" perhaps filenotfound\" << TermColor::RESET() << endl;\n return false;\n }\n cv::Mat a_imleft_srectified, a_imright_srectified;\n cv::Mat _3dImage;\n MatrixXd _3dpts;\n // stereogeom->get3dpoints_and_3dmap_from_raw_images( a_imleft_raw, a_imright_raw,\n // _3dpts, _3dImage,\n // a_imleft_srectified, a_imright_srectified );\n\n cv::Mat disp_viz;\n timer.tic();\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images( a_imleft_raw, a_imright_raw,\n a_imleft_srectified,a_imright_srectified, _3dpts, _3dImage, disp_viz );\n cout << timer.toc_milli() << \" : (ms) get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images\\n\";\n\n\n #if 1\n cv::imshow( \"a_imleft_raw\", a_imleft_raw );\n cv::imshow( \"a_imright_raw\", a_imright_raw );\n // cv::imshow( \"a_imleft_srectified\", a_imleft_srectified );\n // cv::imshow( \"a_imright_srectified\", a_imright_srectified );\n cv::imshow( \"a_disp_viz\", disp_viz );\n #endif\n\n //-----------------------------------------------------\n //--------------- stereo rectify b\n //-----------------------------------------------------\n cout << TermColor::iGREEN() << \"LOAD: BASE/\"+std::to_string(frame_b)+\".jpg\" << TermColor::RESET() << endl;\n cv::Mat b_imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_b)+\".jpg\", 0 );\n cv::Mat b_imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_b)+\"_1.jpg\", 0 );\n if( b_imleft_raw.empty() || b_imright_raw.empty() )\n {\n cout << TermColor::RED() << \"Cannot read image with idx=\" << frame_b << \" perhaps filenotfound\" << TermColor::RESET() << endl;\n return false;\n }\n cv::Mat b_imleft_srectified, b_imright_srectified;\n stereogeom->do_stereo_rectification_of_raw_images( b_imleft_raw, b_imright_raw,\n b_imleft_srectified, b_imright_srectified );\n\n\n\n //---------------------------------------------------------------------\n //------------ point matches between a_left, b_left\n //---------------------------------------------------------------------\n MatrixXd u, ud; // u is from frame_a; ud is from frame_b\n point_feature_matches(a_imleft_srectified, b_imleft_srectified, u, ud );\n if( u.cols() < 30 ) {\n cout << TermColor::RED() << \"too few gms matches between \" << frame_a << \" and \" << frame_b << TermColor::RESET() << endl;\n return false;\n }\n\n\n\n\n\n //---------------- make collection of 3d 2d points\n int c = 0;\n MatrixXd u_normalized = stereogeom->get_K().inverse() * u;\n MatrixXd ud_normalized = stereogeom->get_K().inverse() * ud;\n std::vector world_point;\n std::vector feature_position_ud;\n std::vector feature_position_u;\n for( int k=0 ; k( (int)u(1,k), (int)u(0,k) );\n if( _3dpt[2] < 0.1 || _3dpt[2] > 25. )\n continue;\n\n // the 3d point should also project to same 2d points.\n\n\n c++;\n #if 0\n cout << TermColor::RED() << \"---\" << k << \"---\" << TermColor::RESET() << endl;\n cout << \"ud=\" << ud.col(k).transpose() ;\n cout << \" <--> \";\n cout << \"u=\" << u.col(k).transpose() ;\n cout << \" 3dpt of u=\";\n cout << TermColor::GREEN() << _3dpt[0] << \" \" << _3dpt[1] << \" \" << _3dpt[2] << \" \" << TermColor::RESET();\n\n // project 3d point with Identity\n Vector3d X_i = Vector3d( (double) _3dpt[0],(double) _3dpt[1],(double) _3dpt[2] );\n X_i(0) /= X_i(2);\n X_i(1) /= X_i(2);\n X_i(2) /= X_i(2);\n Vector3d u_cap = stereogeom->get_K() * X_i;\n cout << \"\\nPI(3dpt)=\" << u_cap.transpose() ;\n\n auto delta = u_cap - u.col(k);\n if( abs(delta(0)) < 2. && abs(delta(1)) < 2. ) {\n cout << TermColor::GREEN() << \"\\tdelta=\" << delta.transpose() << TermColor::RESET();\n make_n_good++;\n }\n else\n cout << \"\\tdelta=\" << delta.transpose() ;\n\n cout << endl;\n #endif\n\n feature_position_ud.push_back( Vector2d( ud_normalized(0,k), ud_normalized(1,k) ) );\n feature_position_u.push_back( Vector2d( u_normalized(0,k), u_normalized(1,k) ) );\n\n world_point.push_back( Vector3d( _3dpt[0], _3dpt[1], _3dpt[2] ) );\n // world_point.push_back( X_i );\n }\n cout << TermColor::GREEN() << \"of the total \" << u.cols() << \" point feature correspondences \" << c << \" had valid depths\" << TermColor::RESET() << endl;\n if( c < 30 ) {\n cout << TermColor::RED() << \"too few valid 3d points between \" << frame_a << \" and \" << frame_b << TermColor::RESET() << endl;\n return false;\n }\n\n\n #if 1\n cv::Mat dst_feat_matches;\n string msg = string(\"gms_matcher;\")+\"of the total \"+std::to_string(u.cols())+\" point feature correspondences \" +std::to_string(c)+ \" had valid depths\";\n MiscUtils::plot_point_pair( a_imleft_srectified, u, frame_a,\n b_imleft_srectified, ud, frame_b,\n dst_feat_matches, 3, msg\n );\n cv::Mat dst_feat_matches_resized;\n cv::resize(dst_feat_matches, dst_feat_matches_resized, cv::Size(), 0.5, 0.5);\n\n cv::imshow( \"dst_feat_matches\", dst_feat_matches_resized);\n #endif\n\n //------------- TODO assess u, ud, world_point for histogram spreads. a more spread in measurements will give more precise more. Also will be helpful to weedout bad poses\n\n\n //---------------------------------------------------------------------\n //------------------ theia pnp\n //---------------------------------------------------------------------\n\n\n #if 1\n // simple DlsPnp()\n std::vector solution_rotations;\n std::vector solution_translations;\n timer.tic();\n theia::DlsPnp( feature_position_ud, world_point, &solution_rotations, &solution_translations );\n cout << timer.toc_milli() << \" : (ms) : theia::DlsPnp done in\\n\";\n cout << \"solutions count = \" << solution_rotations.size() << \" \" << solution_translations.size() << endl;\n if( solution_rotations.size() == 0 ) {\n cout << TermColor::RED() << \" theia::DlsPnp returns no solution\" << TermColor::RESET() << endl;\n return false;\n }\n Matrix4d b_T_a = Matrix4d::Identity();\n b_T_a.topLeftCorner(3,3) = solution_rotations[0].toRotationMatrix();\n b_T_a.col(3).topRows(3) = solution_translations[0];\n // cout << \"solution_T \" << b_T_a << endl;\n cout << \"solution_T (b_T_a): \" << PoseManipUtils::prettyprintMatrix4d( b_T_a ) << endl;\n out_b_T_a = b_T_a;\n // return true;\n #endif\n\n\n #if 1\n // DlsPnp with ransac\n\n timer.tic();\n // prep data\n vector data_r;\n for( int i=0 ; i ransac_estimator(params, dlspnp_estimator);\n // theia::LMed ransac_estimator(params, dlspnp_estimator);\n // Initialize must always be called!\n ransac_estimator.Initialize();\n\n theia::RansacSummary summary;\n ransac_estimator.Estimate(data_r, &best_rel_pose, &summary);\n // cout << \"Line y = \" << best_line.m << \"*x + \" << best_line.b;\n cout << timer.toc_milli() << \"(ms)!! DlsPnpWithRansac ElapsedTime includes data prep\\n\";\n cout << \"Ransac:\";\n // for( int i=0; iget_K() * _X_i; //< scaling with camera-intrinsic matrix\n\n\n\n Vector3d _tmp;\n _tmp << feature_position_u[i], 1.0 ;\n _detectedpts_of_A.col(i) = stereogeom->get_K() * _tmp;\n\n\n // cout << \"---\\n\";\n cout << i << \" ) \";\n cout << \"_3dpts_of_A_projectedonA.col(i)=\"<< _3dpts_of_A_projectedonA.col(i).transpose() << \"\\t\";\n cout << \"_detectedpts_of_A.col(i)=\" << _detectedpts_of_A.col(i).transpose() ;\n\n Vector3d delta = _3dpts_of_A_projectedonA.col(i) - _detectedpts_of_A.col(i);\n if( abs(delta(0)) < 2. && abs(delta(1)) < 2. ) {\n cout << \" delta=\" << delta.transpose();\n n_good++;\n }\n else {\n cout << TermColor::RED() << \"delta=\" << delta.transpose() << TermColor::RESET();\n }\n cout << endl;\n }\n cout << \"# of points which with ok delta=\" << n_good << \" out of total\" << world_point.size() << endl;\n\n\n // plot( A, u );\n cv::Mat __dst;\n MiscUtils::plot_point_sets( a_imleft_srectified, _detectedpts_of_A, __dst, cv::Scalar( 0,0,255), false );\n cv::imshow( \"plot( A, u )\", __dst );\n\n\n // plot( A, _3dpts_of_A_projectedonA )\n MiscUtils::plot_point_sets( a_imleft_srectified, _3dpts_of_A_projectedonA, __dst, cv::Scalar( 0,255,255), false );\n cv::imshow( \"plot( A, _3dpts_of_A_projectedonA )\", __dst );\n #endif\n\n\n //---------------------------------------------\n //--- Use the theia's estimated pose do PI( b_T_a * 3dpts ) and plot these\n //--- points on B. Also plot detected points of B\n //---------------------------------------------\n #if 1\n MatrixXd _3dpts_of_A_projectedonB = MatrixXd::Zero(3, world_point.size() );\n MatrixXd _detectedpts_of_B = MatrixXd::Zero(3, world_point.size() );\n MatrixXd _detectedpts_of_A = MatrixXd::Zero(3, world_point.size() );\n\n int n_good = 0;\n for( int i=0 ; iget_K() * _X_i; //< scaling with camera-intrinsic matrix\n\n\n Vector3d _tmp;\n _tmp << feature_position_ud[i], 1.0 ;\n _detectedpts_of_B.col(i) = stereogeom->get_K() * _tmp;\n\n _tmp << feature_position_u[i], 1.0 ;\n _detectedpts_of_A.col(i) = stereogeom->get_K() * _tmp;\n\n\n Vector3d delta = _3dpts_of_A_projectedonB.col(i) - _detectedpts_of_B.col(i);\n if( abs(delta(0)) < 2. && abs(delta(1)) < 2. ) {\n // cout << \" delta=\" << delta.transpose();\n n_good++;\n }\n else {\n // cout << TermColor::RED() << \"delta=\" << delta.transpose() << TermColor::RESET();\n }\n // cout << endl;\n }\n cout << \"n_good=\" <get_K() * _X_i; //< scaling with camera-intrinsic matrix\n }\n\n #endif\n\n\n // plot( B, ud )\n cv::Mat __dst;\n MiscUtils::plot_point_sets( b_imleft_srectified, _detectedpts_of_B, __dst, cv::Scalar( 0,0,255), false, \"_detectedpts_of_B (red)\" );\n // cv::imshow( \"plot( B, ud )\", __dst );\n\n // plot( B, _3dpts_of_A_projectedonB )\n MiscUtils::plot_point_sets( __dst, _3dpts_of_A_projectedonB, cv::Scalar( 0,255,255), false, \";_3dpts_of_A_projectedonB, PI( b_T_a * X),(yellow)\" );\n // MiscUtils::plot_point_sets( b_imleft_srectified, _3dpts_of_A_projectedonB, __dst, cv::Scalar( 0,255,255), false, \"_3dpts_of_A_projectedonB\" );\n\n MiscUtils::plot_point_sets( __dst, _detectedpts_of_A, cv::Scalar( 255,255,255), false, \";;_detectedpts_of_A(white)\" );\n\n MiscUtils::plot_point_sets( __dst, _3dpts_of_A_projectedonB_with_odom_rel_pose, cv::Scalar( 255,0,0), false, \";;;_3dpts_of_A_projectedonB_with_odom_rel_pose, PI( odom_b_T_a * X),(blue)\" );\n\n string status_msg = \"\";\n status_msg += \"_detectedpts_of_B (red);\";\n status_msg += \"_3dpts_of_A_projectedonB, PI( b_T_a * X),(yellow);\";\n status_msg += \"_detectedpts_of_A(white);\";\n status_msg += \"_3dpts_of_A_projectedonB_with_odom_rel_pose, PI( odom_b_T_a * X),(blue)\";\n MiscUtils::append_status_image( __dst, status_msg );\n\n cv::imshow( \"b_imleft_srectified\", __dst );\n}\n\nvoid plot_vio_poses( json& json_obj, ros::Publisher& pub )\n{\n int n_max = json_obj[\"DataNodes\"].size();\n visualization_msgs::Marker cam, cam_text;\n ros::Rate loop_rate(100);\n for( int i=0 ; i raa = json_obj[\"DataNodes\"][i][\"w_T_c\"][\"data\"];\n RawFileIO::read_eigen_matrix( raa, w_T_c );//loads as row-major\n // cout << \"w_T_c \"<< w_T_c << endl;\n cout << \"wTc : \" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl;\n\n RosMarkerUtils::init_text_marker( cam_text );\n cam_text.ns = \"vio_pose_text\";\n cam_text.id = i;\n cam_text.text = std::to_string(i);\n cam_text.scale.z = 0.08;\n RosMarkerUtils::setcolor_to_marker( 1.0,1.0,1.0, cam_text );\n RosMarkerUtils::setpose_to_marker( w_T_c, cam_text );\n pub.publish( cam_text );\n\n\n RosMarkerUtils::init_camera_marker( cam, 0.7 );\n cam.ns = \"vio_pose\";\n cam.id = i;\n RosMarkerUtils::setpose_to_marker( w_T_c, cam );\n RosMarkerUtils::setcolor_to_marker( 0.0, 1.0, 0.0, cam );\n pub.publish( cam );\n ros::spinOnce();\n loop_rate.sleep();\n\n }\n }\n\n}\n\n\nvoid plot_loop_candidates_as_lines( json& log, json& loopcandidates, ros::Publisher&pub )\n{\n\n visualization_msgs::Marker loop_line_marker;\n visualization_msgs::Marker loop_line_marker_new;\n std::shared_ptr stereogeom = make_stereo_geom();\n\n ros::Rate loop_rate(100);\n for( int i=0 ; i r_wta = log[\"DataNodes\"][global_a][\"w_T_c\"][\"data\"];\n vector r_wtb = log[\"DataNodes\"][global_b][\"w_T_c\"][\"data\"];\n Matrix4d w_T_a, w_T_b;\n RawFileIO::read_eigen_matrix( r_wta, w_T_a );\n RawFileIO::read_eigen_matrix( r_wtb, w_T_b );\n\n\n RosMarkerUtils::init_line_marker( loop_line_marker, w_T_a.col(3).topRows(3), w_T_b.col(3).topRows(3) );\n RosMarkerUtils::setcolor_to_marker( 1.0, 0.0, 0.0, loop_line_marker );\n loop_line_marker.ns = \"loopcandidates\";\n loop_line_marker.id = i;\n pub.publish( loop_line_marker );\n\n\n\n // PnP\n Matrix4d b_T_a;\n bool status = relative_pose_compute_with_theia( stereogeom, global_a,global_b, b_T_a );\n if( status ) {\n Matrix4d w_Tcap_b = w_T_a * b_T_a.inverse(); //< new pose\n\n cout << \"w_T_\"<< global_a << \": \" << PoseManipUtils::prettyprintMatrix4d( w_T_a ) << endl;\n cout << \"w_T_\"<< global_b << \": \" << PoseManipUtils::prettyprintMatrix4d( w_T_b ) << endl;\n cout << \"w_Tcap_\"<< global_b << \": \" << PoseManipUtils::prettyprintMatrix4d( w_Tcap_b ) << endl; //new pose of b.\n\n RosMarkerUtils::init_line_marker( loop_line_marker_new, w_T_a.col(3).topRows(3), w_Tcap_b.col(3).topRows(3) );\n RosMarkerUtils::setcolor_to_marker( 1.0, 1.0, 1.0, loop_line_marker_new );\n loop_line_marker_new.ns = \"loopcandidates_corrected\";\n loop_line_marker_new.id = i;\n pub.publish( loop_line_marker_new );\n char key = cv::waitKey(0);\n if( key == 'q' ) {\n cout << \"q pressed\\n\";\n return;\n }\n }\n else {\n // blue-out the edge which failed geometric verification\n RosMarkerUtils::setcolor_to_marker( 0.0, 0.0, 1.0, loop_line_marker );\n pub.publish( loop_line_marker );\n }\n\n\n\n\n ros::spinOnce();\n loop_rate.sleep();\n\n }\n}\n\n\nbool self_projection_test( std::shared_ptr stereogeom,\n int frame_a, int frame_b )\n{\n // I hacve a doubt that the 3dImage which is created from stereo geometry\n // gives out the 3d points in co-ordinate system of right camera-center.\n // I am wanting it in left camera center co-ordinates. Just reproject those\n // points and ensure my premise.\n\n //------------------------------------------------\n //------ 3d points from frame_a\n //------------------------------------------------\n cout << TermColor::iGREEN() << \"LOAD: BASE/\"+std::to_string(frame_a)+\".jpg\" << TermColor::RESET() << endl;\n cv::Mat a_imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_a)+\".jpg\", 0 );\n cv::Mat a_imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_a)+\"_1.jpg\", 0 );\n Matrix4d wTA;\n RawFileIO::read_eigen_matrix( BASE+\"/\"+std::to_string(frame_a)+\".wTc\", wTA );\n if( a_imright_raw.empty() || a_imright_raw.empty() )\n {\n cout << TermColor::RED() << \"Cannot read image with idx=\" << frame_a << \" perhaps filenotfound\" << TermColor::RESET() << endl;\n return false;\n }\n\n\n cv::Mat a_imleft_srectified, a_imright_srectified;\n cv::Mat _3dImage;\n MatrixXd _3dpts;\n cv::Mat disp_viz;\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images( a_imleft_raw, a_imright_raw,\n a_imleft_srectified,a_imright_srectified, _3dpts, _3dImage, disp_viz );\n\n\n\n //-----------------------------------------------------\n //--------------- stereo rectify b\n //-----------------------------------------------------\n cout << TermColor::iGREEN() << \"LOAD: BASE/\"+std::to_string(frame_b)+\".jpg\" << TermColor::RESET() << endl;\n cv::Mat b_imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_b)+\".jpg\", 0 );\n cv::Mat b_imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_b)+\"_1.jpg\", 0 );\n Matrix4d wTB;\n RawFileIO::read_eigen_matrix( BASE+\"/\"+std::to_string(frame_b)+\".wTc\", wTB );\n if( b_imleft_raw.empty() || b_imright_raw.empty() )\n {\n cout << TermColor::RED() << \"Cannot read image with idx=\" << frame_b << \" perhaps filenotfound\" << TermColor::RESET() << endl;\n return false;\n }\n cv::Mat b_imleft_srectified, b_imright_srectified;\n stereogeom->do_stereo_rectification_of_raw_images( b_imleft_raw, b_imright_raw,\n b_imleft_srectified, b_imright_srectified );\n\n\n //---------------------------------------------------------------------\n //------------ point matches between a_left, b_left\n //---------------------------------------------------------------------\n MatrixXd u, ud; // u is from frame_a; ud is from frame_b\n point_feature_matches(a_imleft_srectified, b_imleft_srectified, u, ud );\n if( u.cols() < 30 ) {\n cout << TermColor::RED() << \"too few gms matches between \" << frame_a << \" and \" << frame_b << TermColor::RESET() << endl;\n return false;\n }\n\n\n //----------------------------------------------------------------------\n //---------------- make collection of 3d 2d points\n //----------------------------------------------------------------------\n int c = 0;\n MatrixXd u_normalized = stereogeom->get_K().inverse() * u;\n MatrixXd ud_normalized = stereogeom->get_K().inverse() * ud;\n std::vector world_point;\n std::vector feature_position_ud;\n std::vector feature_position_u;\n for( int k=0 ; k( (int)u(1,k), (int)u(0,k) );\n if( _3dpt[2] < 0.1 || _3dpt[2] > 25. )\n continue;\n\n // the 3d point should also project to same 2d points.\n\n\n c++;\n #if 0\n cout << TermColor::RED() << \"---\" << k << \"---\" << TermColor::RESET() << endl;\n cout << \"ud=\" << ud.col(k).transpose() ;\n cout << \" <--> \";\n cout << \"u=\" << u.col(k).transpose() ;\n cout << \" 3dpt of u=\";\n cout << TermColor::GREEN() << _3dpt[0] << \" \" << _3dpt[1] << \" \" << _3dpt[2] << \" \" << TermColor::RESET();\n\n // project 3d point with Identity\n Vector3d X_i = Vector3d( (double) _3dpt[0],(double) _3dpt[1],(double) _3dpt[2] );\n X_i(0) /= X_i(2);\n X_i(1) /= X_i(2);\n X_i(2) /= X_i(2);\n Vector3d u_cap = stereogeom->get_K() * X_i;\n cout << \"\\nPI(3dpt)=\" << u_cap.transpose() ;\n\n auto delta = u_cap - u.col(k);\n if( abs(delta(0)) < 2. && abs(delta(1)) < 2. ) {\n cout << TermColor::GREEN() << \"\\tdelta=\" << delta.transpose() << TermColor::RESET();\n make_n_good++;\n }\n else\n cout << \"\\tdelta=\" << delta.transpose() ;\n\n cout << endl;\n #endif\n\n feature_position_ud.push_back( Vector2d( ud_normalized(0,k), ud_normalized(1,k) ) );\n feature_position_u.push_back( Vector2d( u_normalized(0,k), u_normalized(1,k) ) );\n\n world_point.push_back( Vector3d( _3dpt[0], _3dpt[1], _3dpt[2] ) );\n // world_point.push_back( X_i );\n }\n cout << TermColor::GREEN() << \"of the total \" << u.cols() << \" point feature correspondences \" << c << \" had valid depths\" << TermColor::RESET() << endl;\n\n\n #if 0\n // PI( _3dpts )\n cout << \"_3dpts.shape=\" << _3dpts.rows() << \",\" << _3dpts.cols() << endl;\n MatrixXd reporj = MatrixXd::Zero( 3, _3dpts.cols() );\n vector reporj_falsedepthcolors;\n auto fp_map = FalseColors();\n for( int i=0 ; i<_3dpts.cols() ; i++ ) {\n Vector4d a_X = _3dpts.col(i); //< 3d pt\n reporj_falsedepthcolors.push_back( fp_map.getFalseColor(a_X(2)/10.) );\n reporj.col(i) = stereogeom->get_K() * (a_X.topRows(3) / a_X(2)); // u <== PI( a_X )\n }\n\n cv::Mat __dst;\n // MiscUtils::plot_point_sets( a_imleft_raw, reporj, __dst, reporj_falsedepthcolors, 0.5, \"PI(_3dpts) on a_imleft_raw\" );\n MiscUtils::plot_point_sets( a_imright_raw, reporj, __dst, reporj_falsedepthcolors, 0.5, \"PI(_3dpts) on a_imright_raw\" );\n cv::imshow( \"__dst\", __dst );\n\n cv::imshow( \"strip\", fp_map.getStrip(30, 300) );\n cv::waitKey(0);\n #endif\n\n\n\n #if 0\n // PI(world_point) - OK\n MatrixXd reproj_of_worldpts_on_left_of_A = MatrixXd::Zero(3,world_point.size());\n for( int i=0 ; iget_K() * (world_point[i] / world_point[i](2) );\n }\n\n cv::Mat __dst;\n MiscUtils::plot_point_sets( a_imleft_raw, reproj_of_worldpts_on_left_of_A, __dst, cv::Scalar(0,0,255), false, \"reproj_of_worldpts_on_left_of_A(red)\" );\n MiscUtils::plot_point_sets( __dst, u, cv::Scalar(0,255,0), false, \";u plotted on A (in green)\" );\n cv::imshow( \"__dst\", __dst );\n cv::waitKey(0);\n #endif\n\n\n Matrix4d odom_b_T_a = wTB.inverse() * wTA ;\n\n Matrix3d rm_R1, rm_R2;\n cv::cv2eigen( stereogeom->get_rm_R1(), rm_R1 );\n cv::cv2eigen( stereogeom->get_rm_R2(), rm_R2 );\n Matrix4d rm_T1, rm_T2;\n rm_T1 = Matrix4d::Identity();\n rm_T2 = Matrix4d::Identity();\n rm_T1.topLeftCorner<3,3>() = rm_R1;\n rm_T2.topLeftCorner<3,3>() = rm_R2;\n cout << \"odom_b_T_a\" << PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) << endl;\n cout << \"rm_T1\" << PoseManipUtils::prettyprintMatrix4d( rm_T1 ) << endl;\n cout << \"rm_T2\" << PoseManipUtils::prettyprintMatrix4d( rm_T2 ) << endl;\n\n cout << \"get_rm_R1\\n\" << stereogeom->get_rm_R1() << endl;\n cout << \"get_rm_R1\\n\" << rm_R1 << endl;\n cout << \"---\\n\";\n cout << \"get_rm_R2\\n\" << stereogeom->get_rm_R2() << endl;\n cout << \"get_rm_R2\\n\" << rm_R2 << endl;\n\n\n MatrixXd reproj_of_worldpts_on_srectifiedleft_of_B = MatrixXd::Zero(3,world_point.size());\n MatrixXd reproj_of_worldpts_on_srectifiedleft_of_B__1 = MatrixXd::Zero(3,world_point.size());\n\n for( int i=0 ; iget_K() * (bd_X.topRows(3) / bd_X(2) );\n\n\n Vector4d bd_X_1 = (rm_T2.inverse() * (odom_b_T_a * rm_T1)) * ad_X;\n reproj_of_worldpts_on_srectifiedleft_of_B__1.col(i) =\n stereogeom->get_K() * (bd_X_1.topRows(3) / bd_X_1(2) );\n }\n cv::Mat __dst;\n MiscUtils::plot_point_sets( b_imleft_srectified, ud, __dst, cv::Scalar(0,255,0), false, \"ud plotted on B srectified_left (in green)\" );\n MiscUtils::plot_point_sets( __dst, reproj_of_worldpts_on_srectifiedleft_of_B, cv::Scalar(0,0,255), false, \";reproj_of_worldpts_on_srectifiedleft_of_B with odom_b_T_a(red)\" );\n MiscUtils::plot_point_sets( __dst, reproj_of_worldpts_on_srectifiedleft_of_B__1, cv::Scalar(0,128,255), false, \";;reproj_of_worldpts_on_srectifiedleft_of_B__1 with odom_b_T_a(yellow)\" );\n cv::imshow( \"__dst\", __dst );\n cv::waitKey(0);\n\n}\n\n//-----------------------------------------------\n//-------- NEW ----------------------------------\n//-----------------------------------------------\n// Given the point feature matches and the 3d image (from disparity map) will return\n// the valid world points and corresponding points.\n// [Input]\n// uv: 2xN matrix of point-feature in image-a. In image co-ordinates (not normalized image cords)\n// _3dImage_uv : 3d image from disparity map of image-a. sizeof( _3dImage_uv) === WxHx3\n// uv_d: 2xN matrix of point-feature in image-b. Note that uv<-->uv_d are correspondences so should of equal sizes\n// [Output]\n// feature_position_uv : a subset of uv but normalized_image_cordinates\n// feature_position_uv_d : a subset of uv_d. results in normalized_image_cordinates\n// world_point : 3d points of uv.\n// [Note]\n// feature_position_uv \\subset uv. Selects points which have valid depths.\n// size of output is same for all 3\n// world points are of uv and in co-ordinate system of camera center of uv (or image-a).\n\nbool make_3d_2d_collection__using__pfmatches_and_disparity( std::shared_ptr stereogeom,\n const MatrixXd& uv, const cv::Mat& _3dImage_uv, const MatrixXd& uv_d,\n std::vector& feature_position_uv, std::vector& feature_position_uv_d,\n std::vector& world_point )\n{\n assert( (uv.cols() == uv_d.cols() ) && \"[Cerebro::make_3d_2d_collection__using__pfmatches_and_disparity] pf-matches need to be of same length. You provided of different lengths\\n\" );\n assert( _3dImage_uv.type() == CV_32FC3 );\n\n if( uv.cols() != uv_d.cols() ) {\n cout << TermColor::RED() << \"[Cerebro::make_3d_2d_collection__using__pfmatches_and_disparity] pf-matches need to be of same length. You provided of different lengths\\n\" << TermColor::RESET();\n return false;\n }\n\n if( _3dImage_uv.type() != CV_32FC3 && _3dImage_uv.rows <= 0 && _3dImage_uv.cols <= 0 ) {\n cout << TermColor::RED() << \"[Cerebro::make_3d_2d_collection__using__pfmatches_and_disparity] _3dImage is expected to be of size CV_32FC3\\n\" << TermColor::RESET();\n return false;\n }\n\n\n\n int c = 0;\n MatrixXd ud_normalized = stereogeom->get_K().inverse() * uv_d;\n MatrixXd u_normalized = stereogeom->get_K().inverse() * uv;\n feature_position_uv.clear();\n feature_position_uv_d.clear();\n world_point.clear();\n\n for( int k=0 ; k( (int)uv(1,k), (int)uv(0,k) );\n if( _3dpt[2] < 0.1 || _3dpt[2] > 25. )\n continue;\n\n c++;\n #if 0\n cout << TermColor::RED() << \"---\" << k << \"---\" << TermColor::RESET() << endl;\n cout << \"ud=\" << ud.col(k).transpose() ;\n cout << \" <--> \";\n cout << \"u=\" << u.col(k).transpose() ;\n cout << \" 3dpt of u=\";\n cout << TermColor::GREEN() << _3dpt[0] << \" \" << _3dpt[1] << \" \" << _3dpt[2] << \" \" << TermColor::RESET();\n cout << endl;\n #endif\n\n feature_position_uv.push_back( Vector2d( u_normalized(0,k), u_normalized(1,k) ) );\n feature_position_uv_d.push_back( Vector2d( ud_normalized(0,k), ud_normalized(1,k) ) );\n world_point.push_back( Vector3d( _3dpt[0], _3dpt[1], _3dpt[2] ) );\n }\n\n cout << \"[make_3d_2d_collection__using__pfmatches_and_disparity]of the total \" << uv.cols() << \" point feature correspondences \" << c << \" had valid depths\\n\";\n\n return true;\n\n if( c < 30 ) {\n cout << TermColor::RED() << \"too few valid 3d points between frames\" << TermColor::RESET() << endl;\n return false;\n }\n\n}\n\n// given pf-matches uv<-->ud_d and their _3dImages. returns the 3d point correspondences at points where it is valid\n// uv_X: the 3d points are in frame of ref of camera-uv\n// uvd_Y: these 3d points are in frame of ref of camera-uvd\nbool make_3d_3d_collection__using__pfmatches_and_disparity(\n const MatrixXd& uv, const cv::Mat& _3dImage_uv,\n const MatrixXd& uv_d, const cv::Mat& _3dImage_uv_d,\n vector& uv_X, vector& uvd_Y\n)\n{\n assert( uv.cols() > 0 && uv.cols() == uv_d.cols() );\n uv_X.clear();\n uvd_Y.clear();\n\n // similar to above but should return world_point__uv and world_point__uv_d\n int c=0;\n for( int k=0 ; k( (int)uv(1,k), (int)uv(0,k) );\n cv::Vec3f uvd_3dpt = _3dImage_uv_d.at( (int)uv_d(1,k), (int)uv_d(0,k) );\n\n if( uv_3dpt[2] < 0.1 || uv_3dpt[2] > 25. || uvd_3dpt[2] < 0.1 || uvd_3dpt[2] > 25. )\n continue;\n\n uv_X.push_back( Vector3d( uv_3dpt[0], uv_3dpt[1], uv_3dpt[2] ) );\n uvd_Y.push_back( Vector3d( uvd_3dpt[0], uvd_3dpt[1], uvd_3dpt[2] ) );\n c++;\n\n }\n cout << \"[make_3d_3d_collection__using__pfmatches_and_disparity] of the total \" << uv.cols() << \" point feature correspondences \" << c << \" had valid depths\\n\";\n\n}\n\n\n// Theia's ICP\n// [Input]\n// uv_X: a 3d point cloud expressed in some frame-of-ref, call it frame-of-ref of `uv`\n// uvd_Y: a 3d point cloud expressed in another frame-of-ref, call it frame-of-ref of `uvd`\n// [Output]\n// uvd_T_uv: Relative pose between the point clouds\n// [Note]\n// uv_X <---> uvd_Y\n#define ____P3P_ICP_( msg ) msg;\n// #define ____P3P_ICP_( msg ) ;\nfloat P3P_ICP( const vector& uv_X, const vector& uvd_Y,\n Matrix4d& uvd_T_uv, string & p3p__msg )\n{\n\n // call this theia::AlignPointCloudsUmeyamaWithWeights\n // void AlignPointCloudsUmeyamaWithWeights(\n // const std::vector& left,\n // const std::vector& right,\n // const std::vector& weights, Eigen::Matrix3d* rotation,\n // Eigen::Vector3d* translation, double* scale)\n p3p__msg = \"\";\n Matrix3d ____R;\n Vector3d ____t; double ___s=1.0;\n\n ElapsedTime timer;\n timer.tic();\n\n #if 0\n theia::AlignPointCloudsUmeyama( uv_X, uvd_Y, &____R, &____t, &___s ); // all weights = 1. TODO: ideally weights could be proportion to point's Z.\n // theia::AlignPointCloudsICP( uv_X, uvd_Y, &____R, &____t ); // all weights = 1. TODO: ideally weights could be proportion to point's Z.\n\n double elapsed_time_p3p = timer.toc_milli();\n\n ____P3P_ICP_(\n cout << \"___R:\\n\" << ____R << endl;\n cout << \"___t: \" << ____t.transpose() << endl;\n cout << \"___s: \" << ___s << endl;\n )\n\n uvd_T_uv = Matrix4d::Identity();\n uvd_T_uv.topLeftCorner<3,3>() = ____R;\n uvd_T_uv.col(3).topRows(3) = ____t;\n\n ____P3P_ICP_(\n cout << TermColor::GREEN() << \"p3p done in (ms) \" << elapsed_time_p3p << \" p3p_ICP: {uvd}_T_{uv} : \" << PoseManipUtils::prettyprintMatrix4d( uvd_T_uv ) << TermColor::RESET() << endl;\n )\n\n\n\n if( min( ___s, 1.0/___s ) < 0.9 ) {\n ____P3P_ICP_( cout << TermColor::RED() << \"theia::AlignPointCloudsUmeyama scales doesn't look good;this usually implies that estimation is bad. scale= \" << ___s << TermColor::RESET() << endl; )\n p3p__msg += \"p3p_ICP: scale=\" +to_string( ___s )+\" {uvd}_T_{uv} : \" + PoseManipUtils::prettyprintMatrix4d( uvd_T_uv );\n p3p__msg += \"p3p done in (ms)\" + to_string(elapsed_time_p3p)+\"; theia::AlignPointCloudsUmeyama scales doesn't look good, this usually implies that estimation is bad. scale= \" + to_string(___s);\n // return -1;\n }\n\n p3p__msg += \"p3p done in (ms)\" + to_string(elapsed_time_p3p)+\"; p3p_ICP: {uvd}_T_{uv} : \" + PoseManipUtils::prettyprintMatrix4d( uvd_T_uv );\n p3p__msg += \";weight=\"+to_string( min( ___s, 1.0/___s ) );\n // return min( ___s, 1.0/___s );\n #endif\n\n\n // with ransac\n timer.tic();\n vector data_r;\n for( int i=0 ; i ransac_estimator( params, icp_estimator);\n ransac_estimator.Initialize();\n\n theia::RansacSummary summary;\n ransac_estimator.Estimate(data_r, &best_rel_pose, &summary);\n auto elapsed_time_p3p_ransac = timer.toc_milli();\n\n uvd_T_uv = Matrix4d::Identity();\n uvd_T_uv = best_rel_pose.b_T_a ;\n\n\n ____P3P_ICP_(\n cout << TermColor::iGREEN() << \"ICP Ransac:\";\n // for( int i=0; i& w_X, const std::vector& c_uv_normalized,\n Matrix4d& c_T_w,\n string& pnp__msg )\n{\n pnp__msg = \"\";\n ElapsedTime timer;\n #if 0\n //--- DlsPnp\n std::vector solution_rotations;\n std::vector solution_translations;\n timer.tic();\n theia::DlsPnp( c_uv_normalized, w_X, &solution_rotations, &solution_translations );\n auto elapsed_dls_pnp = timer.toc_milli() ;\n ___P_N_P__(\n cout << elapsed_dls_pnp << \" : (ms) : theia::DlsPnp done in\\n\";\n cout << \"solutions count = \" << solution_rotations.size() << \" \" << solution_translations.size() << endl;\n )\n\n if( solution_rotations.size() == 0 ) {\n ___P_N_P__(\n cout << TermColor::RED() << \" theia::DlsPnp returns no solution\" << TermColor::RESET() << endl;\n )\n pnp__msg = \" theia::DlsPnp returns no solution\";\n return -1.;\n }\n\n if( solution_rotations.size() > 1 ) {\n ___P_N_P__(\n cout << TermColor::RED() << \" theia::DlsPnp returns multiple solution\" << TermColor::RESET() << endl;\n )\n pnp__msg = \" theia::DlsPnp returns multiple solution\";\n return -1.;\n }\n\n // retrive solution\n c_T_w = Matrix4d::Identity();\n c_T_w.topLeftCorner(3,3) = solution_rotations[0].toRotationMatrix();\n c_T_w.col(3).topRows(3) = solution_translations[0];\n\n ___P_N_P__(\n // cout << \"solution_T \" << b_T_a << endl;\n cout << TermColor::GREEN() << \"DlsPnp (c_T_w): \" << PoseManipUtils::prettyprintMatrix4d( c_T_w ) << TermColor::RESET() << endl;\n // out_b_T_a = b_T_a;\n )\n\n pnp__msg += \"DlsPnp (c_T_w): \" + PoseManipUtils::prettyprintMatrix4d( c_T_w ) + \";\";\n pnp__msg += \" elapsed_dls_pnp_ms=\"+to_string(elapsed_dls_pnp)+\";\";\n\n // return 1.0;\n #endif\n\n\n #if 1\n //--- DlsPnpWithRansac\n timer.tic();\n // prep data\n vector data_r;\n for( int i=0 ; i ransac_estimator(params, dlspnp_estimator);\n // Initialize must always be called!\n ransac_estimator.Initialize();\n\n theia::RansacSummary summary;\n ransac_estimator.Estimate(data_r, &best_rel_pose, &summary);\n\n auto elapsed_dls_pnp_ransac=timer.toc_milli();\n ___P_N_P__(\n cout << elapsed_dls_pnp_ransac << \"(ms)!! DlsPnpWithRansac ElapsedTime includes data prep\\n\";\n cout << \"Ransac:\";\n // for( int i=0; i stereogeom,\n int frame_a, int frame_b )\n{\n // I hacve a doubt that the 3dImage which is created from stereo geometry\n // gives out the 3d points in co-ordinate system of right camera-center.\n // I am wanting it in left camera center co-ordinates. Just reproject those\n // points and ensure my premise.\n\n //------------------------------------------------\n //------ 3d points from frame_a\n //------------------------------------------------\n cout << TermColor::iGREEN() << \"LOAD: BASE/\"+std::to_string(frame_a)+\".jpg\" << TermColor::RESET() << endl;\n cv::Mat a_imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_a)+\".jpg\", 0 );\n cv::Mat a_imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_a)+\"_1.jpg\", 0 );\n Matrix4d wTA;\n RawFileIO::read_eigen_matrix( BASE+\"/\"+std::to_string(frame_a)+\".wTc\", wTA );\n if( a_imright_raw.empty() || a_imright_raw.empty() )\n {\n cout << TermColor::RED() << \"Cannot read image with idx=\" << frame_a << \" perhaps filenotfound\" << TermColor::RESET() << endl;\n return false;\n }\n\n\n cv::Mat a_imleft_srectified, a_imright_srectified;\n cv::Mat a_3dImage;\n MatrixXd a_3dpts;\n cv::Mat a_disp_viz;\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images( a_imleft_raw, a_imright_raw,\n a_imleft_srectified,a_imright_srectified, a_3dpts, a_3dImage, a_disp_viz );\n\n\n\n //-----------------------------------------------------\n //--------------- stereo rectify b\n //-----------------------------------------------------\n cout << TermColor::iGREEN() << \"LOAD: BASE/\"+std::to_string(frame_b)+\".jpg\" << TermColor::RESET() << endl;\n cv::Mat b_imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_b)+\".jpg\", 0 );\n cv::Mat b_imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_b)+\"_1.jpg\", 0 );\n Matrix4d wTB;\n RawFileIO::read_eigen_matrix( BASE+\"/\"+std::to_string(frame_b)+\".wTc\", wTB );\n if( b_imleft_raw.empty() || b_imright_raw.empty() )\n {\n cout << TermColor::RED() << \"Cannot read image with idx=\" << frame_b << \" perhaps filenotfound\" << TermColor::RESET() << endl;\n return false;\n }\n cv::Mat b_imleft_srectified, b_imright_srectified;\n cv::Mat b_3dImage;\n MatrixXd b_3dpts;\n cv::Mat b_disp_viz;\n stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images( b_imleft_raw, b_imright_raw,\n b_imleft_srectified, b_imright_srectified, b_3dpts, b_3dImage, b_disp_viz );\n\n\n //---------------------------------------------------------------------\n //------------ point matches between a_left, b_left\n //---------------------------------------------------------------------\n MatrixXd uv, uv_d; // u is from frame_a; ud is from frame_b\n ElapsedTime timer;\n timer.tic();\n point_feature_matches(a_imleft_srectified, b_imleft_srectified, uv, uv_d );\n string msg_pf_matches = to_string( timer.toc_milli() )+\" (ms) elapsed time for point_feature_matches computation\";\n if( uv.cols() < 30 ) {\n cout << TermColor::RED() << \"too few gms matches between \" << frame_a << \" and \" << frame_b << TermColor::RESET() << endl;\n return false;\n }\n\n\n #if 1 // plot ppoint-feature matches\n cv::Mat dst_feat_matches;\n MiscUtils::plot_point_pair( a_imleft_srectified, uv, frame_a,\n b_imleft_srectified, uv_d, frame_b,\n dst_feat_matches, 3, msg_pf_matches+\";#pf-matches: \"+to_string( uv.cols() ) );\n cv::resize(dst_feat_matches, dst_feat_matches, cv::Size(), 0.5, 0.5 );\n cv::imshow( \"dst_feat_matches\", dst_feat_matches );\n #endif\n\n #if 1 // disparty maps of both images\n cv::Mat dst_disp;\n MiscUtils::side_by_side( a_disp_viz, b_disp_viz, dst_disp );\n MiscUtils::append_status_image( dst_disp, \"a=\"+ to_string(frame_a)+\" b=\"+to_string(frame_b), .8 );\n cv::resize(dst_disp, dst_disp, cv::Size(), 0.5, 0.5 );\n #endif\n\n //\n // Collect 3d points\n //\n\n // Option-A:\n cout << TermColor::BLUE() << \"Option-A\" << TermColor::RESET() << endl;\n std::vector feature_position_uv;\n std::vector feature_position_uv_d;\n std::vector world_point_uv;\n make_3d_2d_collection__using__pfmatches_and_disparity( stereogeom, uv, a_3dImage, uv_d,\n feature_position_uv, feature_position_uv_d, world_point_uv);\n Matrix4d op1__b_T_a; string pnp__msg;\n float pnp_goodness = PNP( world_point_uv, feature_position_uv_d, op1__b_T_a, pnp__msg );\n cout << TermColor::YELLOW() << pnp_goodness << \" op1__b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a ) << TermColor::RESET() << endl;\n cout << pnp__msg << endl;\n\n // Option-B:\n cout << TermColor::BLUE() << \"Option-B\" << TermColor::RESET() << endl;\n std::vector world_point_uv_d;\n make_3d_2d_collection__using__pfmatches_and_disparity( stereogeom, uv_d, b_3dImage, uv,\n feature_position_uv_d, feature_position_uv, world_point_uv_d);\n Matrix4d op2__a_T_b, op2__b_T_a; string pnp__msg_option_B;\n float pnp_goodness_optioN_B = PNP( world_point_uv_d, feature_position_uv, op2__a_T_b, pnp__msg_option_B );\n op2__b_T_a = op2__a_T_b.inverse();\n cout << TermColor::YELLOW() << pnp_goodness_optioN_B << \" op2__a_T_b = \" << PoseManipUtils::prettyprintMatrix4d( op2__a_T_b ) << TermColor::RESET() << endl;\n cout << TermColor::YELLOW() << pnp_goodness_optioN_B << \" op2__b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( op2__b_T_a ) << TermColor::RESET() << endl;\n cout << pnp__msg_option_B << endl;\n\n // Option-C\n cout << TermColor::BLUE() << \"Option-C\" << TermColor::RESET() << endl;\n vector< Vector3d> uv_X;\n vector< Vector3d> uvd_Y;\n make_3d_3d_collection__using__pfmatches_and_disparity( uv, a_3dImage, uv_d, b_3dImage,\n uv_X, uvd_Y );\n Matrix4d icp_b_T_a; string p3p__msg;\n float p3p_goodness = P3P_ICP( uv_X, uvd_Y, icp_b_T_a, p3p__msg );\n cout << TermColor::YELLOW() << p3p_goodness << \" icp_b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( icp_b_T_a ) << TermColor::RESET() << endl;\n cout << p3p__msg << endl;\n\n\n #if 1\n MiscUtils::append_status_image( dst_disp, \"[b_T_a <-- PNP( 3d(a), uv(b))];\"+pnp__msg+\";[a_T_b <-- PNP( 3d(b), uv(a))];\"+pnp__msg_option_B+\";[icp_b_T_a <-- ICP(3d(a), 3d(b))];\"+p3p__msg );\n cv::imshow( \"dst_disp\", dst_disp );\n #endif\n\n\n\n Matrix4d odom_b_T_a = wTB.inverse() * wTA;\n cout << TermColor::YELLOW() << \"odom_b_T_a = \" << PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) << TermColor::RESET() << endl;\n\n\n cout << \"|op1 - op2|\" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a.inverse() * op2__b_T_a ) << endl;\n cout << \"|op1 - icp|\" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a.inverse() * icp_b_T_a ) << endl;\n cout << \"|op2 - icp|\" << PoseManipUtils::prettyprintMatrix4d( op2__b_T_a.inverse() * icp_b_T_a ) << endl;\n\n\n}\n\nint main0( int argc, char ** argv )\n{\n std::shared_ptr stereogeom = make_stereo_geom();\n\n // load ProcessedLoopCandidate.json\n cout << \"Open JSON : \" << BASE+\"/matching/ProcessedLoopCandidate.json\" << endl;\n std::ifstream fp(BASE+\"/matching/ProcessedLoopCandidate.json\");\n json proc_candi_json;\n fp >> proc_candi_json;\n for( int i =0 ; i stereogeom = make_stereo_geom();\n // cout << \"exit main\\n\";\n // return 0;\n\n // will return false if cannot compute pose\n\n if( argc != 3 ) {\n cout << \"[MAIN::INVALID USAGE] I am expecting you to give two numbers in the command line between which you want to compute the pose\\n\";\n exit(1);\n }\n Matrix4d out_b_T_a = Matrix4d::Identity();\n int a = stoi( argv[1] );\n int b = stoi( argv[2] );\n // bool status = relative_pose_compute_with_theia(stereogeom, a, b, out_b_T_a );\n // bool status = self_projection_test( stereogeom, a, b );\n bool status = verified_alignment( stereogeom, a, b );\n\n\n // bool status = relative_pose_compute_with_theia(stereogeom, 195, 3, out_b_T_a );\n cv::waitKey(0);\n\n cout << \"####RESULT####\\n\";\n cout << \"status: \" << status << endl;\n cout << PoseManipUtils::prettyprintMatrix4d( out_b_T_a ) << endl;\n\n\n}\n\nint main1( int argc, char ** argv )\n{\n\n ///////// ROS INIT\n ros::init(argc, argv, \"camera_geometry_inspect_ptcld\" );\n ros::NodeHandle n;\n ros::Publisher chatter_pub = n.advertise(\"chatter\", 1000);\n ros::Rate loop_rate(10);\n\n\n //////////// Open `log.json` and `loopcandidates_liverun.json`\n string json_fname = BASE+\"/log.json\";\n cout << TermColor::GREEN() << \"Open file: \" << json_fname << TermColor::RESET() << endl;\n std::ifstream json_fileptr(json_fname);\n json json_obj;\n json_fileptr >> json_obj;\n cout << \"Successfully opened file \"<< json_fname << endl;\n\n\n string json_loops_fname = BASE+\"/loopcandidates_liverun.json\";\n cout << TermColor::GREEN() << \"Open file: \" << json_loops_fname << TermColor::RESET() << endl;\n std::ifstream json_fileptr_loops(json_loops_fname);\n json json_obj_loopcandidates;\n json_fileptr_loops >> json_obj_loopcandidates;\n cout << \"Successfully opened file \"<< json_loops_fname << endl;\n\n\n ///////////// plot_vio_poses\n plot_vio_poses( json_obj, chatter_pub );\n\n //////////// plot loop candidates\n plot_loop_candidates_as_lines( json_obj, json_obj_loopcandidates, chatter_pub );\n\n\n\n cout << \"ros::spin()\\n. Press CTRL+C to quit\\n\";\n ros::spin();\n\n\n ///// stereo geometry\n/*\n std::shared_ptr stereogeom = make_stereo_geom();\n int frame_a = 840;\n int frame_b = 1344;\n Matrix4d b_T_a;\n relative_pose_compute_with_theia( stereogeom, frame_a,frame_b, b_T_a );\n cv::waitKey(0);\n*/\n\n\n\n}\n" }, { "path": "src/unittest/unittest_theia_ransac.cpp", "content": "#include \n#include \n//opencv\n#include \n#include \n#include \n#include \n\n#include \n\nint main()\n{\n cout << \"Hello\\n\";\n // it was convient to edit unittest_theia instead....TODO: remove this file\n}\n" }, { "path": "src/utils/CameraGeometry.cpp", "content": "#include \"CameraGeometry.h\"\n\nMonoGeometry::MonoGeometry(camodocal::CameraPtr _camera)\n{\n assert( _camera && \"Abstract Camera is not set. You need to init camera before setting it to geometry clases\" );\n if( !_camera ) {\n cout << \"Abstract Camera is not set. You need to init camera before setting it to geometry clases\\n\";\n exit(10);\n }\n this->camera = _camera;\n\n // set K_new as default\n GeometryUtils::getK( this->camera, this->K_new );\n this->new_fx = K_new(0,0); // 375.;\n this->new_fy = K_new(1,1); //375.;\n this->new_cx = K_new(0,2); //376.;\n this->new_cy = K_new(1,2); //240.;\n\n // make rectification maps. Remember to remake the maps when set_K is called\n this->camera->initUndistortRectifyMap( map_x, map_y, new_fx, new_fy, cv::Size(0,0), new_cx, new_cy );\n}\n\n\nvoid MonoGeometry::set_K( Matrix3d K )\n{\n std::lock_guard lk(m);\n this->K_new = K;\n this->new_fx = K_new(0,0); // 375.;\n this->new_fy = K_new(1,1); //375.;\n this->new_cx = K_new(0,2); //376.;\n this->new_cy = K_new(1,2); //240.;\n\n // once a new K is set will have to recompute maps.\n this->camera->initUndistortRectifyMap( map_x, map_y,\n new_fx, new_fy, cv::Size(0,0),\n new_cx, new_cy );\n}\n\nvoid MonoGeometry::do_image_undistortion( const cv::Mat& im_raw, cv::Mat & im_undistorted )\n{\n std::lock_guard lk(m);\n cv::remap( im_raw, im_undistorted, map_x, map_y, CV_INTER_LINEAR );\n}\n\n//-------------------------------------------------------------------------------------//\n\nStereoGeometry::StereoGeometry( camodocal::CameraPtr _left_camera,\n camodocal::CameraPtr _right_camera,\n const Matrix4d& __right_T_left )\n{\n if( !_left_camera || !_right_camera )\n {\n cout << \"\\nERROR : Abstract stereo Camera is not set. You need to init camera before setting it to geometry clases\\n\";\n exit(10);\n }\n assert( _left_camera && _right_camera && \"Abstract stereo Camera is not set. You need to init camera before setting it to geometry clases\" );\n\n this->camera_left = _left_camera;\n this->camera_right = _right_camera;\n // this->right_T_left = Matrix4d( __right_T_left ); //stereo extrinsic. relative pose between two pairs.\n this->set_stereoextrinsic( __right_T_left ); //stereo extrinsic. relative pose between two pairs.\n\n // set K_new from left_camera's intrinsic\n Matrix3d __K_new;\n GeometryUtils::getK( this->camera_left, __K_new );\n this->set_K( __K_new );\n\n\n left_geom = std::make_shared( this->camera_left );\n right_geom = std::make_shared( this->camera_right );\n left_geom->set_K( this->K_new );\n right_geom->set_K( this->K_new ); // it is really important that you have same K_new for each of the images in the stereo-pair, this is not a mistake.\n\n // stereo rectification maps\n // theory : http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/FUSIELLO/tutorial.html\n // make_stereo_rectification_maps() // no need to call this again as it is already called when u either do set_K() or set_stereoextrinsic().\n\n // init SGBM\n // TODO Can put in options to the BM algorithm, look at opencv docs to set the options\n // like blocksize, numDisparities etc etc.\n bm = cv::StereoBM::create(64,21);\n sgbm = cv::StereoSGBM::create();\n\n}\n\nvoid StereoGeometry::print_blockmatcher_algo_info()\n{\n cout << TermColor::RED();\n cout << \"name = \" << bm->getDefaultName() << endl;\n cout << \"getNumDisparities=\" << bm->getNumDisparities() << endl;\n cout << \"getMinDisparity=\" << bm->getMinDisparity() << endl;\n cout << \"getBlockSize=\" << bm->getBlockSize() << endl;\n\n\n cout << TermColor::RESET() << endl;\n}\n\n\n\nvoid StereoGeometry::set_stereoextrinsic( Matrix4d __right_T_left )\n{\n std::lock_guard lk(m_extrinsics);\n this->right_T_left = Matrix4d( __right_T_left );\n make_stereo_rectification_maps();\n}\n\nvoid StereoGeometry::set_stereoextrinsic(Vector4d quat_xyzw, Vector3d tr_xyz )\n{\n std::lock_guard lk(m_extrinsics);\n Matrix4d transform ; //right_T_left\n PoseManipUtils::raw_xyzw_to_eigenmat( quat_xyzw, tr_xyz, transform );\n this->right_T_left = Matrix4d( transform );\n make_stereo_rectification_maps();\n}\n\nconst Matrix4d& StereoGeometry::get_stereoextrinsic()\n{\n std::lock_guard lk(m_extrinsics);\n return this->right_T_left;\n}\n\nvoid StereoGeometry::fundamentalmatrix_from_stereoextrinsic( Matrix3d& F )\n{\n Matrix3d Tx, _Rot;\n {\n std::lock_guard lk2(m_extrinsics);\n PoseManipUtils::vec_to_cross_matrix( right_T_left.col(3).topRows(3), Tx );\n _Rot = right_T_left.topLeftCorner(3,3);\n }\n\n // The actual formula is : inverse(K_right)' * Tx * R * inverse(K_left)\n // but we use the same K_new for both. This is a subtle point here.\n F = this->get_K().transpose().inverse() * Tx * _Rot * this->get_K().inverse(); //< Fundamental Matrix\n}\n\n\nvoid StereoGeometry::draw_epipolarlines( cv::Mat& imleft_undistorted, cv::Mat& imright_undistorted )\n{\n IOFormat numpyFmt(FullPrecision, 0, \", \", \",\\n\", \"[\", \"]\", \"[\", \"]\");\n\n // if image is 1 channel make it to 3 channel so that I can plot colored lines and points.\n cv::Mat imleft_undistorted_3chnl, imright_undistorted_3chnl;\n if( imleft_undistorted.channels() == 1 )\n cv::cvtColor(imleft_undistorted, imleft_undistorted_3chnl, CV_GRAY2BGR );\n else\n imleft_undistorted_3chnl = imleft_undistorted;\n\n if( imright_undistorted.channels() == 1 )\n cv::cvtColor(imright_undistorted, imright_undistorted_3chnl, CV_GRAY2BGR);\n else\n imright_undistorted_3chnl = imright_undistorted;\n\n imleft_undistorted = imleft_undistorted_3chnl;\n imright_undistorted = imright_undistorted_3chnl;\n\n\n // Fundamental Matrix\n Matrix3d F;\n this->fundamentalmatrix_from_stereoextrinsic( F );\n cout << \"[StereoGeometry::draw_epipolarlines]F=\" << F.format(numpyFmt) << endl;\n\n\n\n // tryout multiple points and get their corresponding epipolar line.\n for( int i=0 ; i<500; i+=20 ) {\n #if 1\n // take a sample point x on left image and find the corresponding line on the right image\n Vector3d x(1.5*i, i, 1.0);\n Vector3d ld = F * x;\n MiscUtils::draw_point( x, imleft_undistorted, cv::Scalar(255,0,0) );\n MiscUtils::draw_line( ld, imright_undistorted, cv::Scalar(255,0,0) );\n #endif\n\n #if 1\n // take a sample point on right image and find the corresponding line on the left image\n Vector3d xd(i, i, 1.0);\n Vector3d l = F.transpose() * xd;\n MiscUtils::draw_line( l, imleft_undistorted, cv::Scalar(0,0,255) );\n MiscUtils::draw_point( xd, imright_undistorted, cv::Scalar(0,0,255) );\n #endif\n }\n\n}\n\n\nvoid StereoGeometry::draw_srectified_epipolarlines( cv::Mat& imleft_srectified, cv::Mat& imright_srectified )\n{\n IOFormat numpyFmt(FullPrecision, 0, \", \", \",\\n\", \"[\", \"]\", \"[\", \"]\");\n\n // if image is 1 channel make it to 3 channel so that I can plot colored lines and points.\n cv::Mat imleft_srectified_3chnl, imright_srectified_3chnl;\n if( imleft_srectified.channels() == 1 )\n cv::cvtColor(imleft_srectified, imleft_srectified_3chnl, CV_GRAY2BGR );\n else\n imleft_srectified_3chnl = imleft_srectified;\n\n if( imright_srectified.channels() == 1 )\n cv::cvtColor(imright_srectified, imright_srectified_3chnl, CV_GRAY2BGR);\n else\n imright_srectified_3chnl = imright_srectified;\n\n imleft_srectified = imleft_srectified_3chnl;\n imright_srectified = imright_srectified_3chnl;\n\n\n\n\n Matrix3d F = rm_fundamental_matrix; //< This was computed by make_stereo_rectification_maps.\n // this->fundamentalmatrix_from_stereoextrinsic( F );\n cout << \"[StereoGeometry::draw_srectified_epipolarlines]F=\" << F.format(numpyFmt) << endl;\n\n\n\n // tryout multiple points and get their corresponding epipolar line.\n for( int i=0 ; i<500; i+=20 ) {\n #if 1\n // take a sample point x on left image and find the corresponding line on the right image\n Vector3d x(1.5*i, i, 1.0);\n Vector3d ld = F * x;\n MiscUtils::draw_point( x, imleft_srectified, cv::Scalar(255,0,0) );\n MiscUtils::draw_line( ld, imright_srectified, cv::Scalar(255,0,0) );\n #endif\n\n #if 1\n // take a sample point on right image and find the corresponding line on the left image\n Vector3d xd(i, i, 1.0);\n Vector3d l = F.transpose() * xd;\n MiscUtils::draw_line( l, imleft_srectified, cv::Scalar(0,0,255) );\n MiscUtils::draw_point( xd, imright_srectified, cv::Scalar(0,0,255) );\n #endif\n }\n\n}\n\n\nvoid StereoGeometry::set_K( Matrix3d K )\n{\n std::lock_guard lk(m_intrinsics);\n\n this->K_new = K;\n this->new_fx = K_new(0,0); // 375.;\n this->new_fy = K_new(1,1); //375.;\n this->new_cx = K_new(0,2); //376.;\n this->new_cy = K_new(1,2); //240.;\n make_stereo_rectification_maps();\n}\n\nvoid StereoGeometry::set_K( float _fx, float _fy, float _cx, float _cy )\n{\n std::lock_guard lk(m_intrinsics);\n this->K_new << _fx, 0.0, _cx,\n 0.0, _fy, _cy,\n 0.0, 0.0, 1.0;\n\n this->new_fx = K_new(0,0); // 375.;\n this->new_fy = K_new(1,1); //375.;\n this->new_cx = K_new(0,2); //376.;\n this->new_cy = K_new(1,2); //240.;\n make_stereo_rectification_maps();\n}\n\nconst Matrix3d& StereoGeometry::get_K()\n{\n std::lock_guard lk(m_intrinsics);\n return this->K_new;\n}\n\n\n// #define __StereoGeometry___make_stereo_rectification_maps(msg) msg;\n#define __StereoGeometry___make_stereo_rectification_maps(msg) ;\nvoid StereoGeometry::make_stereo_rectification_maps()\n{\n cv::Size imsize = cv::Size( camera_left->imageWidth(), camera_left->imageHeight());\n\n IOFormat numpyFmt(FullPrecision, 0, \", \", \",\\n\", \"[\", \"]\", \"[\", \"]\");\n\n\n // Adopted from : http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/FUSIELLO/node18.html\n __StereoGeometry___make_stereo_rectification_maps( cout << \"[compute_stereo_rectification_transform]\" << endl );\n\n\n\n\n\n Matrix3d right_R_left = this->right_T_left.topLeftCorner(3,3);\n Vector3d right_t_left = this->right_T_left.col(3).topRows(3);\n cv::Mat R, T;\n cv::eigen2cv( right_R_left, R );\n cv::eigen2cv( right_t_left, T );\n __StereoGeometry___make_stereo_rectification_maps(\n cout << \"R\"<< R << endl;\n cout << \"T\" << T << endl;\n cout << \"this->K_new: \"<< this->K_new << endl;\n )\n\n // Matrix3d K_new_eigen = get_K(); //< this causes some issues. Better not use it.\n Matrix3d K_new_eigen = this->K_new;//get_K();\n __StereoGeometry___make_stereo_rectification_maps(\n cout << \"K_new_eigen: \" << K_new_eigen << endl;\n )\n cv::Mat K_new_cvmat;\n cv::eigen2cv( K_new_eigen, K_new_cvmat );\n\n // TODO: If need be write getters for these matrices.\n // cv::Mat R1, R2;\n // cv::Mat P1, P2;\n // cv::Mat Q;\n\n cv::Mat D;\n // !! Main Call OpenCV !! //\n /// The below function does exactly this: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/FUSIELLO/node18.html\n // See OpenCVdoc: https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga617b1685d4059c6040827800e72ad2b6\n cv::stereoRectify( K_new_cvmat, D, K_new_cvmat, D, imsize, R, T,\n rm_R1, rm_R2, rm_P1, rm_P2, rm_Q\n );\n\n __StereoGeometry___make_stereo_rectification_maps(\n cout << \"\\tK_new=\" << cv::format(K_new_cvmat, cv::Formatter::FMT_NUMPY) << endl;\n cout << \"\\tR1=\" << cv::format(rm_R1, cv::Formatter::FMT_NUMPY) << endl;\n cout << \"\\tR2=\" << cv::format(rm_R2, cv::Formatter::FMT_NUMPY) << endl;\n cout << \"\\tP1=\" << cv::format(rm_P1, cv::Formatter::FMT_NUMPY) << endl;\n cout << \"\\tP2=\" << cv::format(rm_P2, cv::Formatter::FMT_NUMPY) << endl;\n cout << \"\\tQ=\" << cv::format(rm_Q, cv::Formatter::FMT_NUMPY) << endl;\n )\n // cout << TermColor::RESET();\n\n // For horizontal stereo :\n // P2(0,3) := Tx * f, where f = P2(0,0) and Tx is the position of left camera as seen by the right camera. right_T_left.\n double Tx = rm_P2.at(0,3) / rm_P2.at(0,0);\n rm_shift = Vector3d( Tx, 0, 0);\n __StereoGeometry___make_stereo_rectification_maps(cout << \"\\tTx = \" << Tx << \" Tx is the position of left camera as seen by the right camera. right_T_left\" << endl);\n Matrix3d TTx;\n PoseManipUtils::vec_to_cross_matrix( rm_shift, TTx );\n rm_fundamental_matrix = K_new.transpose().inverse() * TTx * K_new.inverse();\n __StereoGeometry___make_stereo_rectification_maps(cout << \"F_rect=\" << rm_fundamental_matrix.format(numpyFmt) << endl);\n\n\n // For vertical stereo : TODO\n // P2(1,3) := Ty * f, where f = P2(0,0) and Tx is the horizontal shift between cameras\n // double Ty = P2(1,3) / P2(0,0);\n\n __StereoGeometry___make_stereo_rectification_maps(cout << \"[/compute_stereo_rectification_transform]\" << endl);\n\n\n\n // !! Make Stereo Rectification Maps !! //\n __StereoGeometry___make_stereo_rectification_maps(cout << \"[make_rectification_maps]\\n\");\n cv::initUndistortRectifyMap( K_new_cvmat, D, rm_R1, rm_P1, imsize, CV_32FC1, map1_x, map1_y );\n cv::initUndistortRectifyMap( K_new_cvmat, D, rm_R2, rm_P2, imsize, CV_32FC1, map2_x, map2_y );\n __StereoGeometry___make_stereo_rectification_maps(\n cout << \"\\tmap1_x: \" << MiscUtils::cvmat_info( map1_x ) << endl;\n cout << \"\\tmap1_y: \" << MiscUtils::cvmat_info( map1_y ) << endl;\n cout << \"\\tmap2_x: \" << MiscUtils::cvmat_info( map2_x ) << endl;\n cout << \"\\tmap2_y: \" << MiscUtils::cvmat_info( map2_y ) << endl;\n cout << \"[/make_rectification_maps]\\n\");\n\n}\n\n//-------------------------- Undistort Raw Image ----------------------------------------//\n\nvoid StereoGeometry::do_image_undistortion( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& imleft_undistorted, cv::Mat& imright_undistorted\n )\n{\n std::lock_guard lk(m_intrinsics);\n // cout << \"imleft_raw \" << MiscUtils::cvmat_info( imleft_raw ) << endl;\n // cout << \"imright_raw \" << MiscUtils::cvmat_info( imright_raw ) << endl;\n\n // this doesn't need a lock as this is not dependent on\n left_geom->do_image_undistortion( imleft_raw, imleft_undistorted );\n right_geom->do_image_undistortion( imright_raw, imright_undistorted );\n}\n\n//--------------------------SRectify (Stereo Rectify)----------------------------------------//\n\n\nvoid StereoGeometry::do_stereo_rectification_of_undistorted_images(\n const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified )\n{\n cv::remap( imleft_undistorted, imleft_srectified, this->map1_x, this->map1_y, CV_INTER_LINEAR );\n cv::remap( imright_undistorted, imright_srectified, this->map2_x, this->map2_y, CV_INTER_LINEAR );\n}\n\n\n\nvoid StereoGeometry::do_stereo_rectification_of_raw_images(\n const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified )\n{\n cv::Mat imleft_undistorted, imright_undistorted;\n\n // raw --> undistorted\n do_image_undistortion(imleft_raw, imright_raw,\n imleft_undistorted, imright_undistorted\n );\n\n // undistorted --> stereo rectify\n do_stereo_rectification_of_undistorted_images(imleft_undistorted, imright_undistorted,\n imleft_srectified, imright_srectified\n );\n}\n\n\n//-----------------------------------DISPARITY Computation--------------------------------------//\n// stereo rectified --> disparity. Uses the stereo-block matching algorithm,\n// ie. cv::StereoBM\n// if you use this function, besure to call `do_stereo_rectification_of_undistorted_images`\n// or `do_stereo_rectification_of_raw_images` on your images before calling this function\nvoid StereoGeometry::do_stereoblockmatching_of_srectified_images(\n const cv::Mat& imleft_srectified, const cv::Mat& imright_srectified,\n cv::Mat& disparity\n)\n{\n // sgbm->compute( imleft_srectified, imright_srectified, disparity ); //disparity is CV_16UC1\n bm->compute( imleft_srectified, imright_srectified, disparity ); //disparity is CV_16UC1\n // cout << \"[StereoGeometry::do_stereoblockmatching_of_srectified_images] disparity\" << MiscUtils::cvmat_info( disparity ) << endl;\n}\n\n// raw--> disparity\n// internally does:\n// step-1: raw-->undistorted\n// step-2: undistorted--> srectify (stereo rectify)\n// step-3: srectify --> disparity\nvoid StereoGeometry::do_stereoblockmatching_of_raw_images(\n const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& disparity\n)\n{\n cv::Mat imleft_srectified, imright_srectified;\n do_stereo_rectification_of_raw_images( imleft_raw, imright_raw, imleft_srectified, imright_srectified );\n do_stereoblockmatching_of_srectified_images( imleft_srectified, imright_srectified, disparity );\n}\n\n\n// undistorted --> disparity\n// step-1: undistorted --> srectified\n// step-2 : srectified --> disparity\nvoid StereoGeometry::do_stereoblockmatching_of_undistorted_images(\n const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n cv::Mat& disparity\n)\n{\n cv::Mat imleft_srectified, imright_srectified;\n do_stereo_rectification_of_undistorted_images( imleft_undistorted, imright_undistorted, imleft_srectified, imright_srectified );\n do_stereoblockmatching_of_srectified_images( imleft_srectified, imright_srectified, disparity );\n}\n\n\n\n//----------------------------- DISPARITY and point cloud -------------------------//\n// # Inputs\n// disparity_raw : The disparity image\n// fill_eigen_matrix : _3dpts will be allocated and filled only if this flag is true. If this flag is false then eigen matrix is not filled in this function.\n// make_homogeneous : true will result in Eigen matrix being 4xN else will be 3xN. fill_eigen_matrix need to be true for this to matter.\n// # Outputs\n// out3d : 3 channel image. X,Y,Z --> ch0, ch1, ch2. Will also contain invalid 3d points.\n// _3dpts : 4xN matrix containing only valid points. N < disparity_raw.shape[0]*disparity_raw.shape[1].\nvoid StereoGeometry::disparity_to_3DPoints(const cv::Mat& disparity_raw,\n cv::Mat& out3D, MatrixXd& _3dpts,\n bool fill_eigen_matrix, bool make_homogeneous )\n{\n // Adopted from : https://github.com/bkornel/Reproject-Image-To-3D\n // OpenCV3.3 version has a bug not sure about other version. It produces inf.\n\n // /\\/\\/\\/\\/\\/\\/\\/\\/\\ ATTEMPT-1 /\\/\\/\\/\\/\\/\\/\\/\\/\\\n //---------------- assemble all the data needed for computation\n const cv::Mat Q = this->get_Q();\n CV_Assert( !disparity_raw.empty() );\n\n cv::Mat disparity;\n if( disparity_raw.type() != CV_32FC1 )\n disparity_raw.convertTo( disparity, CV_32FC1 );\n else\n disparity = disparity_raw;\n\n\tCV_Assert(disparity.type() == CV_32FC1 );\n\tCV_Assert( Q.cols == 4 && Q.rows == 4);\n CV_Assert( Q.type() == CV_32F || Q.type() == CV_64F );\n\n\t// 3-channel matrix for containing the reprojected 3D world coordinates\n\tout3D = cv::Mat::zeros(disparity.size(), CV_32FC3);\n\n\t// Getting the interesting parameters from Q, everything else is zero or one\n float Q03,Q13,Q23,Q32,Q33;\n\n if( Q.type() == CV_32F ) {\n \tQ03 = Q.at(0, 3);\n \tQ13 = Q.at(1, 3);\n \tQ23 = Q.at(2, 3);\n \tQ32 = Q.at(3, 2);\n \tQ33 = Q.at(3, 3);\n } else {\n Q03 = (float) Q.at(0, 3);\n Q13 = (float) Q.at(1, 3);\n Q23 = (float) Q.at(2, 3);\n Q32 = (float) Q.at(3, 2);\n Q33 = (float) Q.at(3, 3);\n }\n\n\n //--------------------- disparity to 3D points store as 3-channel image\n\t// Transforming a single-channel disparity map to a 3-channel image representing a 3D surface\n\tfor (int i = 0; i < disparity.rows; i++)\n\t{\n\t\tconst float* disp_ptr = disparity.ptr(i);\n\t\tcv::Vec3f* out3D_ptr = out3D.ptr(i);\n\n\t\tfor (int j = 0; j < disparity.cols; j++)\n\t\t{\n\t\t\tconst float pw = 1.0f / (disp_ptr[j]/16. * Q32 + Q33 + 1E-6); // 1E-6 added to avoid inf.\n\n // ---------------------------------^^^^^\n // it is crucial that you convert the raw_disparity to\n // CV_32F and divide by 16. This is because raw_disparity from StereoBM() is in CV_16SC1\n // (16bit) and a disparity value := NumDisparities*16. so if you have NumDisparities as 64\n // the maximum value in disparity will be 1024. This is a bit weird. So be careful.\n\n\t\t\tcv::Vec3f& point = out3D_ptr[j];\n\t\t\tpoint[0] = (static_cast(j)+Q03) * pw;\n\t\t\tpoint[1] = (static_cast(i)+Q13) * pw;\n\t\t\tpoint[2] = Q23 * pw;\n\t\t}\n\t}\n\n\n\n // /\\/\\/\\/\\/\\/\\/\\/\\/\\ ATTEMPT-2 /\\/\\/\\/\\/\\/\\/\\/\\/\n /*\n const cv::Mat Q = this->get_Q();\n cv::Mat Q_32; // opencv's reprojectImageTo3D() need Q to be in CV_32FC1\n Q.convertTo( Q_32, CV_32F );\n cv::Mat disparity_32f;\n disparity_raw.convertTo( disparity_32f, CV_32F );\n disparity_32f *= (1/16.);\n\n // if using opencv's reprojectImageTo3D() it is crucial that you convert the raw_disparity to\n // CV_32F and divide by 16. This is because raw_disparity from StereoBM() is in CV_16SC1\n // (16bit) and a disparity value := NumDisparities*16. so if you have NumDisparities as 64\n // the maximum value in disparity will be 1024. This is a bit weird. So be careful.\n\n cout << \"Q cvinfo\" << MiscUtils::cvmat_info( Q ) << endl;\n cout << \"Q_32 cvinfo\" << MiscUtils::cvmat_info( Q_32 ) << endl;\n cout << \"Q\" << Q << endl;\n cout << ( (Q_32.size() == cv::Size(4,4))?\"true\":\"false\" )<< endl;\n cv::reprojectImageTo3D( disparity_32f, out3D, Q_32, true );\n cv::Mat disparity = disparity_raw;\n // END /\\/\\/\\/\\/\\/\\/\\/\\/\\ ATTEMPT-2 /\\/\\/\\/\\/\\/\\/\\/\\/\n */\n\n if( fill_eigen_matrix == false )\n return ;\n\n\n //--------------------------- Eigen 3D points as 3xN or 4xN\n // TODO: Make this more efficient. If I enable 4xN matrix it takes 8ms/image if I disable it takes 3ms/image.\n // Step-1: loop over out3D to know how how many were valid points. remove inf points and remove behind points (points where disparity was invalid)\n int N = 0;\n for (int i = 0; i < disparity.rows; i++)\n\t{\n\t\tconst float* disp_ptr = disparity.ptr(i);\n\t\tcv::Vec3f* out3D_ptr = out3D.ptr(i);\n\n\t\tfor (int j = 0; j < disparity.cols; j++)\n\t\t{\n cv::Vec3f& point = out3D_ptr[j];\n if( point[2] < 500. && point[2] > 0.0001 ) {\n N++;\n }\n }\n }\n\n // Step-2: Allocate Memory\n if( make_homogeneous )\n {\n _3dpts = MatrixXd::Zero( 4, N );\n // Step-3: Copy values into Eigen::Matrix of shape 4xN.\n int n=0;\n for (int i = 0; i < disparity.rows; i++)\n {\n const float* disp_ptr = disparity.ptr(i);\n cv::Vec3f* out3D_ptr = out3D.ptr(i);\n\n for (int j = 0; j < disparity.cols; j++)\n {\n cv::Vec3f& point = out3D_ptr[j];\n if( point[2] < 500. && point[2] > 0.0001 ) {\n _3dpts(0,n) = (double)point[0];\n _3dpts(1,n) = (double)point[1];\n _3dpts(2,n) = (double)point[2];\n _3dpts(3,n) = (double)1.0;\n n++;\n }\n }\n }\n }\n else\n {\n _3dpts = MatrixXd::Zero( 3, N );\n // Step-3: Copy values into Eigen::Matrix of shape 4xN.\n int n=0;\n for (int i = 0; i < disparity.rows; i++)\n {\n const float* disp_ptr = disparity.ptr(i);\n cv::Vec3f* out3D_ptr = out3D.ptr(i);\n\n for (int j = 0; j < disparity.cols; j++)\n {\n cv::Vec3f& point = out3D_ptr[j];\n if( point[2] < 500. && point[2] > 0.0001 ) {\n _3dpts(0,n) = (double)point[0];\n _3dpts(1,n) = (double)point[1];\n _3dpts(2,n) = (double)point[2];\n n++;\n }\n }\n }\n }\n\n\n\n}\n\n\n\n\n// _3dpts : 4xN\n// 1. raw --> srectified\n// 2. srectified --> disparity_raw\n// 3. disparity_raw --> 3d points\n// return only 3d points\nvoid StereoGeometry::get3dpoints_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& _3dpts )\n{\n cv::Mat disp_raw;\n this->do_stereoblockmatching_of_raw_images( imleft_raw, imright_raw, disp_raw );\n\n\n const cv::Mat Q = this->get_Q();\n cv::Mat _3dImage;\n this->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, true, true );\n\n // cout << \"notimplemented\\n\";\n // exit(11);\n}\n\n\n// returns both 3dpoints and 3dimage\nvoid StereoGeometry::get3dpoints_and_3dmap_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& _3dpts, cv::Mat& _3dImage )\n{\n cv::Mat disp_raw;\n this->do_stereoblockmatching_of_raw_images( imleft_raw, imright_raw, disp_raw );\n\n\n const cv::Mat Q = this->get_Q();\n this->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, true, true );\n\n}\n\n// returns both 3dpoints and 3dimage along with srectified image pair\nvoid StereoGeometry::get3dpoints_and_3dmap_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& _3dpts, cv::Mat& _3dImage,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified )\n{\n // raw --> srectified\n this->do_stereo_rectification_of_raw_images( imleft_raw, imright_raw, imleft_srectified, imright_srectified );\n\n // srectified --> disp\n cv::Mat disp_raw;\n this->do_stereoblockmatching_of_srectified_images( imleft_srectified, imright_srectified, disp_raw );\n\n\n const cv::Mat Q = this->get_Q();\n this->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, true, true );\n\n}\n\n\nvoid StereoGeometry::get3dpoints_and_disparity_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& _3dpts, cv::Mat& disparity_for_visualization )\n{\n cv::Mat disp_raw;\n this->do_stereoblockmatching_of_raw_images( imleft_raw, imright_raw, disp_raw );\n\n\n cv::Mat _3dImage;\n this->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, true, true );\n\n cv::Mat disparity_for_visualization_gray;\n cv::normalize(disp_raw, disparity_for_visualization_gray, 0, 255, CV_MINMAX, CV_8U); //< disp8 used just for visualization\n cv::applyColorMap(disparity_for_visualization_gray, disparity_for_visualization, cv::COLORMAP_HOT);\n\n\n\n // cout << \"notimplemented\\n\";\n // exit(11);\n}\n\n\n\nvoid StereoGeometry::get_srectifiedim_and_3dpoints_and_disparity_from_raw_images(\n const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified,\n MatrixXd& _3dpts, cv::Mat& disparity_for_visualization )\n{\n // raw --> srectified\n this->do_stereo_rectification_of_raw_images( imleft_raw, imright_raw, imleft_srectified, imright_srectified );\n\n // block matching\n cv::Mat disp_raw;\n this->do_stereoblockmatching_of_srectified_images( imleft_srectified, imright_srectified, disp_raw );\n\n\n // disparity to 3D points\n cv::Mat _3dImage;\n this->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, true, true );\n\n cv::Mat disparity_for_visualization_gray;\n cv::normalize(disp_raw, disparity_for_visualization_gray, 0, 255, CV_MINMAX, CV_8U); //< disp8 used just for visualization\n cv::applyColorMap(disparity_for_visualization_gray, disparity_for_visualization, cv::COLORMAP_HOT);\n\n\n\n\n}\n\nvoid StereoGeometry::get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images(\n const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified,\n MatrixXd& _3dpts, cv::Mat& _3dImage, cv::Mat& disparity_for_visualization )\n{\n // raw --> srectified\n this->do_stereo_rectification_of_raw_images( imleft_raw, imright_raw, imleft_srectified, imright_srectified );\n\n // block matching\n cv::Mat disp_raw;\n this->do_stereoblockmatching_of_srectified_images( imleft_srectified, imright_srectified, disp_raw );\n\n\n // disparity to 3D points\n this->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, true, true );\n\n cv::Mat disparity_for_visualization_gray;\n cv::normalize(disp_raw, disparity_for_visualization_gray, 0, 255, CV_MINMAX, CV_8U); //< disp8 used just for visualization\n cv::applyColorMap(disparity_for_visualization_gray, disparity_for_visualization, cv::COLORMAP_HOT);\n\n\n\n\n}\n\n\n// _3dImage: 3d points as 3 channel image. 1st channel is X, 2nd channel is Y and 3rd channel is Z.\n// Also note that these co-ordinates and imleft_raw co-ordinate do not correspond. They correspond to\n// the srectified images. Incase you want to use the color info with these 3d points, this\n// will lead to wrong. You should compute the srectified images for that.\n// 1. raw --> srectified\n// 2. srectified --> disparity_raw\n// 3. disparity_raw --> 3d points\nvoid StereoGeometry::get3dmap_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& _3dImage )\n{\n cv::Mat disp_raw;\n this->do_stereoblockmatching_of_raw_images( imleft_raw, imright_raw, disp_raw );\n\n\n const cv::Mat Q = this->get_Q();\n MatrixXd _3dpts;\n this->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, false, true );\n\n // cout << \"notimplemented\\n\";\n // exit(11);\n}\n\n\n// e_3dImageX, e_3dImageY, e_3dImageZ: cv::split( _3dImage ). same size as imleft_raw.shape.\n// Also note that these co-ordinates and imleft_raw co-ordinate do not correspond. They correspond to\n// the srectified images. Incase you want to use the color info with these 3d points, this\n// will lead to wrong. You should compute the srectified images for that.\n// 1. raw --> srectified\n// 2. srectified --> disparity_raw\n// 3. disparity_raw --> 3d points\nvoid StereoGeometry::get3dmap_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& e_3dImageX, MatrixXd& e_3dImageY, MatrixXd& e_3dImageZ )\n{\n // TODO : perhaps return vector\n cout << \"this is somewhat fragile. I am making a 3 array of matrix and returning it. This is bad. this need to be tested for correctness after this can be used safely.\";\n exit(11);\n\n cv::Mat disp_raw;\n this->do_stereoblockmatching_of_raw_images( imleft_raw, imright_raw, disp_raw );\n\n\n const cv::Mat Q = this->get_Q();\n cv::Mat _3dImage;\n MatrixXd _3dpts;\n this->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, false, true );\n\n cv::Mat _3dImage_XYZ[3]; //destination array\n cv::split(_3dImage,_3dImage_XYZ);//split source\n cv::cv2eigen( _3dImage_XYZ[0], e_3dImageX );\n cv::cv2eigen( _3dImage_XYZ[1], e_3dImageY );\n cv::cv2eigen( _3dImage_XYZ[2], e_3dImageZ );\n}\n\n\n\n\n//-------------------------------------------------------------------------------------//\nvoid GeometryUtils::make_K( float new_fx, float new_fy, float new_cx, float new_cy, Matrix3d& K )\n{\n K << new_fx, 0, new_cx,\n 0, new_fy, new_cy,\n 0, 0, 1;\n}\n\n\n\n\n\nvoid GeometryUtils::getK( camodocal::CameraPtr m_cam, Matrix3d& K )\n{\n Matrix3d K_rect;\n vector m_camera_params;\n m_cam->writeParameters( m_camera_params ); // retrive camera params from Abstract Camera.\n // camodocal::CataCamera::Parameters p();\n // cout << \"size=\" << m_camera_params.size() << \" ::>\\n\" ;\n // for( int i=0 ; imodelType() )\n {\n case camodocal::Camera::ModelType::MEI:\n K_rect << m_camera_params[5], 0, m_camera_params[7],\n 0, m_camera_params[6], m_camera_params[8],\n 0, 0, 1;\n break;\n case camodocal::Camera::ModelType::PINHOLE:\n K_rect << m_camera_params[4], 0, m_camera_params[6],\n 0, m_camera_params[5], m_camera_params[7],\n 0, 0, 1;\n break;\n case camodocal::Camera::ModelType::KANNALA_BRANDT:\n K_rect << m_camera_params[4], 0, m_camera_params[6],\n 0, m_camera_params[5], m_camera_params[7],\n 0, 0, 1;\n break;\n default:\n // TODO: Implement for other models. Look at initUndistortRectifyMap for each of the abstract class.\n cout << \"[getK] Wrong\\nQuit....\";\n cout << \"Implement for other models. Look at initUndistortRectifyMap for each of the abstract class.\\n\";\n exit(10);\n\n }\n K = Matrix3d(K_rect);\n}\n\n\n// given a point cloud as 3xN or 4xN matrix, gets colors for each based on the depth\nvoid GeometryUtils::depthColors( const MatrixXd& ptcld, vector& out_colors, double min, double max )\n{\n assert( ( ptcld.rows() == 3 || ptcld.rows() == 4 ) && \"ptcld need to be either a 3xN or a 4xN matrix\" );\n assert( ptcld.cols() > 0 );\n\n cv::Mat colormap_gray = cv::Mat::zeros( 1, 256, CV_8UC1 );\n for( int i=0 ; i<256; i++ ) colormap_gray.at(0,i) = i;\n cv::Mat colormap_color;\n cv::applyColorMap(colormap_gray, colormap_color, cv::COLORMAP_HOT);\n\n if( min < 0 )\n min = ptcld.row(2).minCoeff();\n if( max < 0 )\n max = ptcld.row(2).maxCoeff();\n double mean = ptcld.row(2).mean();\n assert( max > min );\n\n out_colors.clear();\n for( int k=0 ; k 255 ) ixx = 255;\n\n cv::Vec3b f = colormap_color.at(0, (int)ixx );\n out_colors.push_back( cv::Scalar(f[0], f[1], f[2]) );\n }\n\n // cout << \"min = \" << min << \" max=\" << max << \" mean=\" << mean << endl;\n\n}\n\n\n// given a point cloud as 3xN or 4xN matrix, gets colors for each based on the depth.\n// return in out_colors as 3xN\nvoid GeometryUtils::depthColors( const MatrixXd& ptcld, MatrixXd& out_colors, double min, double max )\n{\n assert( ( ptcld.rows() == 3 || ptcld.rows() == 4 ) && \"ptcld need to be either a 3xN or a 4xN matrix\" );\n assert( ptcld.cols() > 0 );\n\n cv::Mat colormap_gray = cv::Mat::zeros( 1, 256, CV_8UC1 );\n for( int i=0 ; i<256; i++ ) colormap_gray.at(0,i) = i;\n cv::Mat colormap_color;\n cv::applyColorMap(colormap_gray, colormap_color, cv::COLORMAP_HOT);\n\n if( min < 0 )\n min = ptcld.row(2).minCoeff();\n if( max < 0 )\n max = ptcld.row(2).maxCoeff();\n double mean = ptcld.row(2).mean();\n assert( max > min );\n\n out_colors = MatrixXd( 3,ptcld.cols() );\n for( int k=0 ; k 255 ) ixx = 255;\n\n cv::Vec3b f = colormap_color.at(0, (int)ixx );\n\n // out_colors.push_back( cv::Scalar(f[0], f[1], f[2]) );\n out_colors( 0, k ) = float(f[2]) / 255.;\n out_colors( 1, k ) = float(f[1]) / 255.;\n out_colors( 2, k ) = float(f[0]) / 255.;\n }\n\n // cout << \"min = \" << min << \" max=\" << max << \" mean=\" << mean << endl;\n\n}\n\n\nvoid GeometryUtils::idealProjection( const Matrix3d& K, const MatrixXd& c_X, MatrixXd& uv )\n{\n assert( c_X.rows() == 4 && \"c_X need to be expressed in homogeneous co-ordinates\\n\" );\n assert( c_X.cols() > 0 );\n\n // a) c_X = c_X / c_X.row(2). ==> Z divide\n // b) perspective_proj = c_X.topRows(3)\n MatrixXd uv_normalized = MatrixXd::Constant( 3, c_X.cols(), 1.0 );\n uv_normalized.row(0) = c_X.row(0).array() / c_X.row(2).array();\n uv_normalized.row(1) = c_X.row(1).array() / c_X.row(2).array();\n\n\n // c) uv = K * c_X\n uv = K * uv_normalized;\n}\n\n\n\nvoid GeometryUtils::idealProjection( const Matrix3d& K, const vector& c_X, MatrixXd& uv )\n{\n assert( c_X.size() > 0 );\n\n // a) c_X = c_X / c_X.row(2). ==> Z divide\n // b) perspective_proj = c_X.topRows(3)\n MatrixXd uv_normalized = MatrixXd::Constant( 3, c_X.size(), 1.0 );\n // uv_normalized.row(0) = c_X.row(0).array() / c_X.row(2).array();\n // uv_normalized.row(1) = c_X.row(1).array() / c_X.row(2).array();\n for( int i=0 ; i& c_X, MatrixXd& uv )\n{\n assert( c_X.size() > 0 );\n assert( abs(w_T_c(3,3)-1.) < 1.0E-5 );\n\n // a) c_X = c_X / c_X.row(2). ==> Z divide\n // b) perspective_proj = c_X.topRows(3)\n MatrixXd uv_normalized = MatrixXd::Constant( 3, c_X.size(), 1.0 );\n // uv_normalized.row(0) = c_X.row(0).array() / c_X.row(2).array();\n // uv_normalized.row(1) = c_X.row(1).array() / c_X.row(2).array();\n for( int i=0 ; i\n#include \n#include \n#include \n\n#include \n#include \n\n\n#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n#include \"MiscUtils.h\"\n#include \"ElapsedTime.h\"\n#include \"PoseManipUtils.h\"\n#include \"TermColor.h\"\n\n// Threads and threadsafety\n#include \n#include \n#include \n\nclass MonoGeometry {\npublic:\n MonoGeometry() { }\n\n MonoGeometry(camodocal::CameraPtr _camera );\n\n // Set the new intrinsic matrix (optional). By default will use the same as\n // the abstract camera. but the K matrix is essentially just scaling pixel sizes.\n // this comes to play in stereopairs and/or motion stereo.\n void set_K( Matrix3d K );\n\n // this is just a call to cv::remap with the maps already computed.\n void do_image_undistortion( const cv::Mat& im_raw, cv::Mat & im_undistorted );\n\nprivate:\n camodocal::CameraPtr camera;\n Matrix3d K_new;\n float new_fx, new_fy, new_cx, new_cy;\n\n cv::Mat map_x, map_y;\n\n std::mutex m;\n\n};\n\n\n// There are 3 types of images\n// im*_raw\n// im*_undistorted\n// im*_stereo_rectified\nclass StereoGeometry {\npublic:\n // right_T_left: extrinsic calibration for stereo pair. The position of left camera as viewed from right camera.\n StereoGeometry( camodocal::CameraPtr _left_camera,\n camodocal::CameraPtr _right_camera,\n const Matrix4d& right_T_left\n );\n\n\n // intrinsic cam matrix for both cameras. Here it is important to set this\n // else the block matching computation won't be valid. If this is not set\n // we will use the matrix for left camera as K_new.\n // Everytime a new K is set, we have to recompute the maps.\n void set_K( Matrix3d K );\n void set_K( float _fx, float _fy, float _cx, float _cy );\n const Matrix3d& get_K();\n\n // set extrinsic. THis is thread safe\n // Everytime a new extrinsic is set, we have to recompute the maps.\n void set_stereoextrinsic( Matrix4d __right_T_left );\n void set_stereoextrinsic( Vector4d quat_xyzw, Vector3d tr_xyz );\n const Matrix4d& get_stereoextrinsic();\n\n void fundamentalmatrix_from_stereoextrinsic( Matrix3d& F );\n //TODO write another function to return fundamental matrix of rectified stereo pair.\n\n // Draw epipolar lines on both views. These images will be overwritten. You need\n // to give me undistorted image-pairs. You can undistory image pair with\n // StereoGeometry::do_image_undistortion()\n void draw_epipolarlines( cv::Mat& imleft_undistorted, cv::Mat& imright_undistorted );\n void draw_srectified_epipolarlines( cv::Mat& imleft_srectified, cv::Mat& imright_srectified );\n\n\n void do_image_undistortion( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& imleft_undistorted, cv::Mat& imright_undistorted\n );\n\n // given the undistorted images outputs the stereo-rectified pairs\n // This is essentially just cv::remap using the stereo-rectification maps\n void do_stereo_rectification_of_undistorted_images(\n const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified );\n\n\n // Given raw image pair return stereo-rectified pair. This is a 2 step process\n // raw --> undistorted --> stereo rectified\n void do_stereo_rectification_of_raw_images(\n const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified );\n\n\n // stereo rectified --> disparity. Uses the stereo-block matching algorithm,\n // ie. cv::StereoBM\n // if you use this function, besure to call `do_stereo_rectification_of_undistorted_images`\n // or `do_stereo_rectification_of_raw_images` on your images before calling this function.\n // returned disparity is CV_16SC1. Be cautioned.\n void do_stereoblockmatching_of_srectified_images(\n const cv::Mat& imleft_srectified, const cv::Mat& imright_srectified,\n cv::Mat& disparity\n );\n\n // raw--> disparity\n // internally does:\n // step-1: raw-->undistorted\n // step-2: undistorted--> srectify (stereo rectify)\n // step-3: srectify --> disparity\n void do_stereoblockmatching_of_raw_images(\n const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& disparity\n );\n\n\n // undistorted --> disparity\n // step-1: undistorted --> srectified\n // step-2 : srectified --> disparity\n void do_stereoblockmatching_of_undistorted_images(\n const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n cv::Mat& disparity\n );\n\n // # Inputs\n // disparity_raw : The disparity image\n // fill_eigen_matrix : _3dpts will be allocated and filled only if this flag is true. If this flag is false then eigen matrix is not filled in this function.\n // make_homogeneous : true will result in Eigen matrix being 4xN else will be 3xN. fill_eigen_matrix need to be true for this to matter.\n // # Outputs\n // out3d : 3 channel image. X,Y,Z --> ch0, ch1, ch2. Will also contain invalid 3d points.\n // _3dpts : 4xN matrix containing only valid points. N < disparity_raw.shape[0]*disparity_raw.shape[1].\n void disparity_to_3DPoints(const cv::Mat& disparity_raw,\n cv::Mat& out3D, MatrixXd& _3dpts,\n bool fill_eigen_matrix=true, bool make_homogeneous=true );\n\n\n // #Input\n // imleft_raw, imright_raw : Raw images. As is from the camera.\n // # Output\n // _3dpts : 4xN\n // 1. raw --> srectified\n // 2. srectified --> disparity_raw\n // 3. disparity_raw --> 3d points\n void get3dpoints_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& _3dpts );\n\n\n\n\n // #Input\n // imleft_raw, imright_raw : Raw images. As is from the camera.\n // # Output\n // _3dpts : 4xN\n // 1. raw --> srectified\n // 2. srectified --> disparity_raw\n // 3. disparity_raw --> 3d points\n // _3dImage: 3d points as 3 channel image. 1st channel is X, 2nd channel is Y and 3rd channel is Z.\n void get3dpoints_and_3dmap_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& _3dpts, cv::Mat& _3dImage );\n\n\n // returns both 3dpoints and 3dimage along with srectified image pair\n void get3dpoints_and_3dmap_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& _3dpts, cv::Mat& _3dImage,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified );\n\n\n // #Input\n // imleft_raw, imright_raw : Raw images. As is from the camera.\n // # Output\n // _3dImage: 3d points as 3 channel image. 1st channel is X, 2nd channel is Y and 3rd channel is Z.\n // Also note that these co-ordinates and imleft_raw co-ordinate do not correspond. They correspond to\n // the srectified images. Incase you want to use the color info with these 3d points, this\n // will lead to wrong. You should compute the srectified images for that.\n // 1. raw --> srectified\n // 2. srectified --> disparity_raw\n // 3. disparity_raw --> 3d points\n void get3dmap_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& _3dImage );\n\n\n // #Input\n // imleft_raw, imright_raw : Raw images. As is from the camera.\n // # Output\n // e_3dImageX, e_3dImageY, e_3dImageZ: cv::split( _3dImage ). same size as imleft_raw.shape.\n // Also note that these co-ordinates and imleft_raw co-ordinate do not correspond. They correspond to\n // the srectified images. Incase you want to use the color info with these 3d points, this\n // will lead to wrong. You should compute the srectified images for that.\n // 1. raw --> srectified\n // 2. srectified --> disparity_raw\n // 3. disparity_raw --> 3d points\n void get3dmap_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& e_3dImageX, MatrixXd& e_3dImageY, MatrixXd& e_3dImageZ );\n\n\n\n // Given raw image pair returns valid 3d points and disparity false color map.\n // #Input\n // imleft_raw, imright_raw : Raw images. As is from the camera.\n // #Output\n // _3dpts : 4xN matrix.\n // disparity_for_visualization : false color mapped for visualization only. Don't do any processing with this one.\n void get3dpoints_and_disparity_from_raw_images( const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n MatrixXd& _3dpts, cv::Mat& disparity_for_visualization );\n\n // Given raw image pair return a) stereo-rectified image pair b) valid 3d points c) disparity false color map\n void get_srectifiedim_and_3dpoints_and_disparity_from_raw_images(\n const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified,\n MatrixXd& _3dpts, cv::Mat& disparity_for_visualization );\n void get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images(\n const cv::Mat& imleft_raw, const cv::Mat& imright_raw,\n cv::Mat& imleft_srectified, cv::Mat& imright_srectified,\n MatrixXd& _3dpts, cv::Mat& _3dImage, cv::Mat& disparity_for_visualization );\n\n\n // Some semi private getters of internal variables\npublic:\n const cv::Mat& get_Q() const { return rm_Q; }\n // TODO: (if need be) also have getters from rm_R1, rm_R2, rm_P1, rm_P2, rm_shift, rm_fundamental_matrix.\n const cv::Mat& get_rm_R1( ) const { return rm_R1; }\n const cv::Mat& get_rm_R2( ) const { return rm_R2; }\n const cv::Mat& get_rm_P1( ) const { return rm_P1; }\n const cv::Mat& get_rm_P2( ) const { return rm_P2; }\n\n void print_blockmatcher_algo_info();\n\nprivate:\n camodocal::CameraPtr camera_left, camera_right;\n Matrix4d right_T_left; ///< extrinsic calibration of the stereopair.\n\n Matrix3d K_new;\n float new_fx, new_fy, new_cx, new_cy;\n\n std::shared_ptr left_geom, right_geom;\n\n // stereo rectify maps\n // theory : http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/FUSIELLO/tutorial.html\n\n mutable std::mutex m_extrinsics;\n mutable std::mutex m_intrinsics;\n\n // This function is called everytime you change the intrinsics and extrinsics for\n // the stereo pair. This outputs the stereo-rectification maps, ie. map1_x, map1_y, map2_x, map2_y\n // This is not intended to be a public call.\n // TODO: thread safety\n void make_stereo_rectification_maps();\n cv::Mat rm_R1, rm_R2, rm_P1, rm_P2, rm_Q;\n Vector3d rm_shift; // right_T_left\n Matrix3d rm_fundamental_matrix; // fundamental matrix after rectification\n cv::Mat map1_x, map1_y, map2_x, map2_y;\n\n\n // Stereo Block Matching\n cv::Ptr bm;\n cv::Ptr sgbm;\n\n};\n\nclass GeometryUtils {\npublic:\n // Given the focal length make a K out of it\n static void make_K( float new_fx, float new_fy, float new_cx, float new_cy, Matrix3d& K );\n\n // For a camera gets a K\n static void getK( camodocal::CameraPtr m_cam, Matrix3d& K );\n\n // Ideal Projection:\n // a) c_X = c_X / c_X.row(2). ==> Z divide\n // b) perspective_proj = c_X.topRows(3)\n // c) uv = K * c_X\n // # Input\n // K : Camera params. K_new\n // c_X : 3D points expressed in frame of the camera.\n // Note that if you have world 3d points you will need to transform\n // it to camera co-ordinates before you pass it to this function.\n // uv : image co-ordinates (xy)\n static void idealProjection( const Matrix3d& K, const MatrixXd& c_X, MatrixXd& uv );\n static void idealProjection( const Matrix3d& K, const vector& c_X, MatrixXd& uv );\n static void idealProjection( const Matrix3d& K, const Matrix4d& w_T_c, const vector& c_X, MatrixXd& uv );\n\n\n // given a point cloud as 3xN or 4xN matrix, gets colors for each based on the depth\n // min==-1 then we will compute the min from data, else will use whatever was supplied\n // max==-1 then we will compute the max from data, else will use whatever.\n static void depthColors( const MatrixXd& ptcld, vector& out_colors, double min=-1, double max=-1 );\n static void depthColors( const MatrixXd& ptcld, MatrixXd& out_colors, double min=-1, double max=-1 ); // out_colors : 3xN rgb \\in [0,1]\n\n};\n" }, { "path": "src/utils/ElapsedTime.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n\nclass ElapsedTime\n{\npublic:\n ElapsedTime() {\n // start timer\n this->begin = std::chrono::steady_clock::now();\n }\n\n void tic() {\n // start timer\n this->begin = std::chrono::steady_clock::now();\n }\n\n\n int toc_milli() {\n std::chrono::steady_clock::time_point end= std::chrono::steady_clock::now();\n return (int) std::chrono::duration_cast(end - begin).count();\n }\n\n int toc_micro() {\n std::chrono::steady_clock::time_point end= std::chrono::steady_clock::now();\n return (int) std::chrono::duration_cast(end - begin).count();\n }\n\n int toc( ) {\n std::chrono::steady_clock::time_point end= std::chrono::steady_clock::now();\n return (int) std::chrono::duration_cast(end - begin).count();\n }\n\n\n\nprivate:\n std::chrono::steady_clock::time_point begin;\n\n};\n\nclass DateAndTime\n{\npublic:\n static std::string current_date_and_time()\n {\n auto now = std::chrono::system_clock::now();\n auto in_time_t = std::chrono::system_clock::to_time_t(now);\n\n std::stringstream ss;\n ss << std::put_time(std::localtime(&in_time_t), \"%Y-%m-%d %X\");\n return ss.str();\n }\n\n // TODO Can make more functions later to get hr, min, date etc. Not priority.\n\n};\n" }, { "path": "src/utils/GMSMatcher/gms_matcher.cpp", "content": "#include \"gms_matcher.h\"\n\n\n\nint gms_matcher::GetInlierMask(vector &vbInliers, bool WithScale, bool WithRotation) {\n\n\tint max_inlier = 0;\n\n\tif (!WithScale && !WithRotation)\n\t{\n\t\tSetScale(0);\n\t\tmax_inlier = run(1);\n\t\tvbInliers = mvbInlierMask;\n\t\treturn max_inlier;\n\t}\n\n\tif (WithRotation && WithScale)\n\t{\n\t\tfor (int Scale = 0; Scale < 5; Scale++)\n\t\t{\n\t\t\tSetScale(Scale);\n\t\t\tfor (int RotationType = 1; RotationType <= 8; RotationType++)\n\t\t\t{\n\t\t\t\tint num_inlier = run(RotationType);\n\n\t\t\t\tif (num_inlier > max_inlier)\n\t\t\t\t{\n\t\t\t\t\tvbInliers = mvbInlierMask;\n\t\t\t\t\tmax_inlier = num_inlier;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\t\treturn max_inlier;\n\t}\n\n\tif (WithRotation && !WithScale)\n\t{\n\t\tSetScale(0);\n\t\tfor (int RotationType = 1; RotationType <= 8; RotationType++)\n\t\t{\n\t\t\tint num_inlier = run(RotationType);\n\n\t\t\tif (num_inlier > max_inlier)\n\t\t\t{\n\t\t\t\tvbInliers = mvbInlierMask;\n\t\t\t\tmax_inlier = num_inlier;\n\t\t\t}\n\t\t}\n\t\treturn max_inlier;\n\t}\n\n\tif (!WithRotation && WithScale)\n\t{\n\t\tfor (int Scale = 0; Scale < 5; Scale++)\n\t\t{\n\t\t\tSetScale(Scale);\n\n\t\t\tint num_inlier = run(1);\n\n\t\t\tif (num_inlier > max_inlier)\n\t\t\t{\n\t\t\t\tvbInliers = mvbInlierMask;\n\t\t\t\tmax_inlier = num_inlier;\n\t\t\t}\n\n\t\t}\n\t\treturn max_inlier;\n\t}\n\n\treturn max_inlier;\n}\n\nvoid gms_matcher::AssignMatchPairs(int GridType) {\n\n\tfor (size_t i = 0; i < mNumberMatches; i++)\n\t{\n\t\tPoint2f &lp = mvP1[mvMatches[i].first];\n\t\tPoint2f &rp = mvP2[mvMatches[i].second];\n\n\t\tint lgidx = mvMatchPairs[i].first = GetGridIndexLeft(lp, GridType);\n\t\tint rgidx = -1;\n\n\t\tif (GridType == 1)\n\t\t{\n\t\t\trgidx = mvMatchPairs[i].second = GetGridIndexRight(rp);\n\t\t}\n\t\telse\n\t\t{\n\t\t\trgidx = mvMatchPairs[i].second;\n\t\t}\n\n\t\tif (lgidx < 0 || rgidx < 0)\tcontinue;\n\n\t\tmMotionStatistics.at(lgidx, rgidx)++;\n\t\tmNumberPointsInPerCellLeft[lgidx]++;\n\t}\n\n}\n\nvoid gms_matcher::VerifyCellPairs(int RotationType) {\n\n\tconst int *CurrentRP = mRotationPatterns[RotationType - 1];\n\n\tfor (int i = 0; i < mGridNumberLeft; i++)\n\t{\n\t\tif (sum(mMotionStatistics.row(i))[0] == 0)\n\t\t{\n\t\t\tmCellPairs[i] = -1;\n\t\t\tcontinue;\n\t\t}\n\n\t\tint max_number = 0;\n\t\tfor (int j = 0; j < mGridNumberRight; j++)\n\t\t{\n\t\t\tint *value = mMotionStatistics.ptr(i);\n\t\t\tif (value[j] > max_number)\n\t\t\t{\n\t\t\t\tmCellPairs[i] = j;\n\t\t\t\tmax_number = value[j];\n\t\t\t}\n\t\t}\n\n\t\tint idx_grid_rt = mCellPairs[i];\n\n\t\tconst int *NB9_lt = mGridNeighborLeft.ptr(i);\n\t\tconst int *NB9_rt = mGridNeighborRight.ptr(idx_grid_rt);\n\n\t\tint score = 0;\n\t\tdouble thresh = 0;\n\t\tint numpair = 0;\n\n\t\tfor (size_t j = 0; j < 9; j++)\n\t\t{\n\t\t\tint ll = NB9_lt[j];\n\t\t\tint rr = NB9_rt[CurrentRP[j] - 1];\n\t\t\tif (ll == -1 || rr == -1)\tcontinue;\n\n\t\t\tscore += mMotionStatistics.at(ll, rr);\n\t\t\tthresh += mNumberPointsInPerCellLeft[ll];\n\t\t\tnumpair++;\n\t\t}\n\n\t\tthresh = THRESH_FACTOR * sqrt(thresh / numpair);\n\n\t\tif (score < thresh)\n\t\t\tmCellPairs[i] = -2;\n\t}\n}\n\nint gms_matcher::run(int RotationType) {\n\n\tmvbInlierMask.assign(mNumberMatches, false);\n\n\t// Initialize Motion Statisctics\n\tmMotionStatistics = Mat::zeros(mGridNumberLeft, mGridNumberRight, CV_32SC1);\n\tmvMatchPairs.assign(mNumberMatches, pair(0, 0));\n\n\tfor (int GridType = 1; GridType <= 4; GridType++)\n\t{\n\t\t// initialize\n\t\tmMotionStatistics.setTo(0);\n\t\tmCellPairs.assign(mGridNumberLeft, -1);\n\t\tmNumberPointsInPerCellLeft.assign(mGridNumberLeft, 0);\n\n\t\tAssignMatchPairs(GridType);\n\t\tVerifyCellPairs(RotationType);\n\n\t\t// Mark inliers\n\t\tfor (size_t i = 0; i < mNumberMatches; i++)\n\t\t{\n\t\t\tif (mvMatchPairs[i].first >= 0) {\n\t\t\t\tif (mCellPairs[mvMatchPairs[i].first] == mvMatchPairs[i].second)\n\t\t\t\t{\n\t\t\t\t\tmvbInlierMask[i] = true;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\t}\n\tint num_inlier = sum(mvbInlierMask)[0];\n\treturn num_inlier;\n}\n" }, { "path": "src/utils/GMSMatcher/gms_matcher.h", "content": "#pragma once\n#include \n#include \n#include \n#include \nusing namespace std;\nusing namespace cv;\n\n#define THRESH_FACTOR 6\n\n// 8 possible rotation and each one is 3 X 3\nconst int mRotationPatterns[8][9] = {\n\t1,2,3,\n\t4,5,6,\n\t7,8,9,\n\n\t4,1,2,\n\t7,5,3,\n\t8,9,6,\n\n\t7,4,1,\n\t8,5,2,\n\t9,6,3,\n\n\t8,7,4,\n\t9,5,1,\n\t6,3,2,\n\n\t9,8,7,\n\t6,5,4,\n\t3,2,1,\n\n\t6,9,8,\n\t3,5,7,\n\t2,1,4,\n\n\t3,6,9,\n\t2,5,8,\n\t1,4,7,\n\n\t2,3,6,\n\t1,5,9,\n\t4,7,8\n};\n\n// 5 level scales\nconst double mScaleRatios[5] = { 1.0, 1.0 / 2, 1.0 / sqrt(2.0), sqrt(2.0), 2.0 };\n\n\nclass gms_matcher\n{\npublic:\n\t// OpenCV Keypoints & Correspond Image Size & Nearest Neighbor Matches\n\tgms_matcher(const vector &vkp1, const Size size1, const vector &vkp2, const Size size2, const vector &vDMatches)\n\t{\n\t\t// Input initialize\n\t\tNormalizePoints(vkp1, size1, mvP1);\n\t\tNormalizePoints(vkp2, size2, mvP2);\n\t\tmNumberMatches = vDMatches.size();\n\t\tConvertMatches(vDMatches, mvMatches);\n\n\t\t// Grid initialize\n\t\tmGridSizeLeft = Size(20, 20);\n\t\tmGridNumberLeft = mGridSizeLeft.width * mGridSizeLeft.height;\n\n\t\t// Initialize the neihbor of left grid\n\t\tmGridNeighborLeft = Mat::zeros(mGridNumberLeft, 9, CV_32SC1);\n\t\tInitalizeNiehbors(mGridNeighborLeft, mGridSizeLeft);\n\t};\n\t~gms_matcher() {};\n\nprivate:\n\n\t// Normalized Points\n\tvector mvP1, mvP2;\n\n\t// Matches\n\tvector > mvMatches;\n\n\t// Number of Matches\n\tsize_t mNumberMatches;\n\n\t// Grid Size\n\tSize mGridSizeLeft, mGridSizeRight;\n\tint mGridNumberLeft;\n\tint mGridNumberRight;\n\n\t// x\t : left grid idx\n\t// y : right grid idx\n\t// value : how many matches from idx_left to idx_right\n\tMat mMotionStatistics;\n\n\t//\n\tvector mNumberPointsInPerCellLeft;\n\n\t// Inldex : grid_idx_left\n\t// Value : grid_idx_right\n\tvector mCellPairs;\n\n\t// Every Matches has a cell-pair\n\t// first : grid_idx_left\n\t// second : grid_idx_right\n\tvector > mvMatchPairs;\n\n\t// Inlier Mask for output\n\tvector mvbInlierMask;\n\n\t//\n\tMat mGridNeighborLeft;\n\tMat mGridNeighborRight;\n\npublic:\n\n\t// Get Inlier Mask\n\t// Return number of inliers\n\tint GetInlierMask(vector &vbInliers, bool WithScale = false, bool WithRotation = false);\n\nprivate:\n\n\t// Normalize Key Points to Range(0 - 1)\n\tvoid NormalizePoints(const vector &kp, const Size &size, vector &npts) {\n\t\tconst size_t numP = kp.size();\n\t\tconst int width = size.width;\n\t\tconst int height = size.height;\n\t\tnpts.resize(numP);\n\n\t\tfor (size_t i = 0; i < numP; i++)\n\t\t{\n\t\t\tnpts[i].x = kp[i].pt.x / width;\n\t\t\tnpts[i].y = kp[i].pt.y / height;\n\t\t}\n\t}\n\n\t// Convert OpenCV DMatch to Match (pair)\n\tvoid ConvertMatches(const vector &vDMatches, vector > &vMatches) {\n\t\tvMatches.resize(mNumberMatches);\n\t\tfor (size_t i = 0; i < mNumberMatches; i++)\n\t\t{\n\t\t\tvMatches[i] = pair(vDMatches[i].queryIdx, vDMatches[i].trainIdx);\n\t\t}\n\t}\n\n\tint GetGridIndexLeft(const Point2f &pt, int type) {\n\t\tint x = 0, y = 0;\n\n\t\tif (type == 1) {\n\t\t\tx = floor(pt.x * mGridSizeLeft.width);\n\t\t\ty = floor(pt.y * mGridSizeLeft.height);\n\n\t\t\tif (y >= mGridSizeLeft.height || x >= mGridSizeLeft.width){\n\t\t\t\treturn -1;\n\t\t\t}\n\t\t}\n\n\t\tif (type == 2) {\n\t\t\tx = floor(pt.x * mGridSizeLeft.width + 0.5);\n\t\t\ty = floor(pt.y * mGridSizeLeft.height);\n\n\t\t\tif (x >= mGridSizeLeft.width || x < 1) {\n\t\t\t\treturn -1;\n\t\t\t}\n\t\t}\n\n\t\tif (type == 3) {\n\t\t\tx = floor(pt.x * mGridSizeLeft.width);\n\t\t\ty = floor(pt.y * mGridSizeLeft.height + 0.5);\n\n\t\t\tif (y >= mGridSizeLeft.height || y < 1) {\n\t\t\t\treturn -1;\n\t\t\t}\n\t\t}\n\n\t\tif (type == 4) {\n\t\t\tx = floor(pt.x * mGridSizeLeft.width + 0.5);\n\t\t\ty = floor(pt.y * mGridSizeLeft.height + 0.5);\n\n\t\t\tif (y >= mGridSizeLeft.height || y < 1 || x >= mGridSizeLeft.width || x < 1) {\n\t\t\t\treturn -1;\n\t\t\t}\n\t\t}\n\n\t\treturn x + y * mGridSizeLeft.width;\n\t}\n\n\tint GetGridIndexRight(const Point2f &pt) {\n\t\tint x = floor(pt.x * mGridSizeRight.width);\n\t\tint y = floor(pt.y * mGridSizeRight.height);\n\n\t\treturn x + y * mGridSizeRight.width;\n\t}\n\n\t// Assign Matches to Cell Pairs\n\tvoid AssignMatchPairs(int GridType);\n\n\t// Verify Cell Pairs\n\tvoid VerifyCellPairs(int RotationType);\n\n\t// Get Neighbor 9\n\tvector GetNB9(const int idx, const Size& GridSize) {\n\t\tvector NB9(9, -1);\n\n\t\tint idx_x = idx % GridSize.width;\n\t\tint idx_y = idx / GridSize.width;\n\n\t\tfor (int yi = -1; yi <= 1; yi++)\n\t\t{\n\t\t\tfor (int xi = -1; xi <= 1; xi++)\n\t\t\t{\n\t\t\t\tint idx_xx = idx_x + xi;\n\t\t\t\tint idx_yy = idx_y + yi;\n\n\t\t\t\tif (idx_xx < 0 || idx_xx >= GridSize.width || idx_yy < 0 || idx_yy >= GridSize.height)\n\t\t\t\t\tcontinue;\n\n\t\t\t\tNB9[xi + 4 + yi * 3] = idx_xx + idx_yy * GridSize.width;\n\t\t\t}\n\t\t}\n\t\treturn NB9;\n\t}\n\n\tvoid InitalizeNiehbors(Mat &neighbor, const Size& GridSize) {\n\t\tfor (int i = 0; i < neighbor.rows; i++)\n\t\t{\n\t\t\tvector NB9 = GetNB9(i, GridSize);\n\t\t\tint *data = neighbor.ptr(i);\n\t\t\tmemcpy(data, &NB9[0], sizeof(int) * 9);\n\t\t}\n\t}\n\n\tvoid SetScale(int Scale) {\n\t\t// Set Scale\n\t\tmGridSizeRight.width = mGridSizeLeft.width * mScaleRatios[Scale];\n\t\tmGridSizeRight.height = mGridSizeLeft.height * mScaleRatios[Scale];\n\t\tmGridNumberRight = mGridSizeRight.width * mGridSizeRight.height;\n\n\t\t// Initialize the neihbor of right grid\n\t\tmGridNeighborRight = Mat::zeros(mGridNumberRight, 9, CV_32SC1);\n\t\tInitalizeNiehbors(mGridNeighborRight, mGridSizeRight);\n\t}\n\n\t// Run\n\tint run(int RotationType);\n};\n" }, { "path": "src/utils/MiscUtils.cpp", "content": "#include \"MiscUtils.h\"\n\nstring MiscUtils::type2str(int type) {\n string r;\n\n uchar depth = type & CV_MAT_DEPTH_MASK;\n uchar chans = 1 + (type >> CV_CN_SHIFT);\n\n switch ( depth ) {\n case CV_8U: r = \"8U\"; break;\n case CV_8S: r = \"8S\"; break;\n case CV_16U: r = \"16U\"; break;\n case CV_16S: r = \"16S\"; break;\n case CV_32S: r = \"32S\"; break;\n case CV_32F: r = \"32F\"; break;\n case CV_64F: r = \"64F\"; break;\n default: r = \"User\"; break;\n }\n\n r += \"C\";\n r += (chans+'0');\n\n return r;\n}\n\nstring MiscUtils::cvmat_info( const cv::Mat& mat )\n{\n std::stringstream buffer;\n buffer << \"shape=\" << mat.rows << \",\" << mat.cols << \",\" << mat.channels() ;\n buffer << \"\\t\" << \"dtype=\" << MiscUtils::type2str( mat.type() );\n return buffer.str();\n}\n\nstring MiscUtils::imgmsg_info(const sensor_msgs::ImageConstPtr &img_msg)\n{\n std::stringstream buffer;\n buffer << \"---\\n\";\n buffer << \"\\theader:\\n\";\n buffer << \"\\t\\tseq: \" << img_msg->header.seq << endl;\n buffer << \"\\t\\tstamp: \" << img_msg->header.stamp << endl;\n buffer << \"\\t\\tframe_id: \" << img_msg->header.frame_id << endl;\n buffer << \"\\twidth:\" << img_msg->width << endl;\n buffer << \"\\theight:\" << img_msg->height << endl;\n buffer << \"\\tencoding:\" << img_msg->encoding << endl;\n buffer << \"\\tis_bigendian:\" << (int) img_msg->is_bigendian << endl;\n buffer << \"\\tstep:\" << img_msg->step << endl;\n buffer << \"\\tdata:\" << \"[...\" << img_msg->step * img_msg->height << \" sized array...]\" << endl;\n\n return buffer.str();\n\n}\n\nstring MiscUtils::imgmsg_info(const sensor_msgs::Image& img_msg)\n{\n std::stringstream buffer;\n buffer << \"---\\n\";\n buffer << \"\\theader:\\n\";\n buffer << \"\\t\\tseq: \" << img_msg.header.seq << endl;\n buffer << \"\\t\\tstamp: \" << img_msg.header.stamp << endl;\n buffer << \"\\t\\tframe_id: \" << img_msg.header.frame_id << endl;\n buffer << \"\\twidth:\" << img_msg.width << endl;\n buffer << \"\\theight:\" << img_msg.height << endl;\n buffer << \"\\tencoding:\" << img_msg.encoding << endl;\n buffer << \"\\tis_bigendian:\" << (int) img_msg.is_bigendian << endl;\n buffer << \"\\tstep:\" << img_msg.step << endl;\n buffer << \"\\tdata:\" << \"[...\" << img_msg.step * img_msg.height << \" sized array...]\" << endl;\n\n return buffer.str();\n\n}\n\ncv::Mat MiscUtils::getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)\n{\n cv_bridge::CvImageConstPtr ptr;\n if (img_msg->encoding == \"8UC1\")\n {\n sensor_msgs::Image img;\n img.header = img_msg->header;\n img.height = img_msg->height;\n img.width = img_msg->width;\n img.is_bigendian = img_msg->is_bigendian;\n img.step = img_msg->step;\n img.data = img_msg->data;\n img.encoding = \"mono8\";\n ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);\n }\n else\n ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);\n\n cv::Mat img = ptr->image.clone();\n return img;\n}\n\nstd::vector\nMiscUtils::split( std::string const& original, char separator )\n{\n std::vector results;\n std::string::const_iterator start = original.begin();\n std::string::const_iterator end = original.end();\n std::string::const_iterator next = std::find( start, end, separator );\n while ( next != end ) {\n results.push_back( std::string( start, next ) );\n start = next + 1;\n next = std::find( start, end, separator );\n }\n results.push_back( std::string( start, next ) );\n return results;\n}\n\nvoid MiscUtils::keypoint_2_eigen( const std::vector& kp, MatrixXd& uv, bool make_homogeneous )\n{\n assert( kp.size() > 0 && \"MiscUtils::keypoint_2_eigen empty kp.\");\n uv = MatrixXd::Constant( (make_homogeneous?3:2), kp.size(), 1.0 );\n for( int i=0; i& kp1, const std::vector& kp2,\n const std::vector matches,\n MatrixXd& M1, MatrixXd& M2,\n bool make_homogeneous\n )\n{\n assert( matches.size() > 0 && \"MiscUtils::dmatch_2_eigen DMatch cannot be empty\" );\n assert( kp1.size() > 0 && kp2.size() > 0 && \"MiscUtils::dmatch_2_eigen keypoints cannot be empty\" );\n\n M1 = MatrixXd::Constant( (make_homogeneous?3:2), matches.size(), 1.0 );\n M2 = MatrixXd::Constant( (make_homogeneous?3:2), matches.size(), 1.0 );\n for( int i=0 ; i=0 && queryIdx < kp1.size() );\n assert( trainIdx >=0 && trainIdx < kp2.size() );\n M1(0,i) = kp1[queryIdx].pt.x;\n M1(1,i) = kp1[queryIdx].pt.y;\n\n M2(0,i) = kp2[trainIdx].pt.x;\n M2(1,i) = kp2[trainIdx].pt.y;\n }\n}\n\n\n\nvoid MiscUtils::plot_point_sets( const cv::Mat& im, const MatrixXd& pts_set, cv::Mat& dst,\n const cv::Scalar& color, bool enable_keypoint_annotation, const string& msg )\n{\n MatrixXf pts_set_float;\n pts_set_float = pts_set.cast();\n\n cv::Mat pts_set_mat;\n cv::eigen2cv( pts_set_float, pts_set_mat );\n\n MiscUtils::plot_point_sets( im, pts_set_mat, dst, color, enable_keypoint_annotation, msg );\n}\n\n// in place manip\nvoid MiscUtils::plot_point_sets( cv::Mat& im, const MatrixXd& pts_set,\n const cv::Scalar& color, bool enable_keypoint_annotation, const string& msg )\n{\n MatrixXf pts_set_float;\n pts_set_float = pts_set.cast();\n\n cv::Mat pts_set_mat;\n cv::eigen2cv( pts_set_float, pts_set_mat );\n\n MiscUtils::plot_point_sets( im, pts_set_mat, im, color, enable_keypoint_annotation, msg );\n}\n\n// TODO: call the next function instead of actually doing the work.\nvoid MiscUtils::plot_point_sets( const cv::Mat& im, const cv::Mat& pts_set, cv::Mat& dst, const cv::Scalar& color, bool enable_keypoint_annotation, const string& msg )\n{\n\n if( im.data == dst.data ) { // this is pointer comparison to know if this operation is inplace\n // cout << \"src and dst are same\\n\";\n // src and dst images are same, so dont copy. just ensure it is a 3 channel image.\n assert( im.rows > 0 && im.cols > 0 && \"\\n[MiscUtils::plot_point_sets]Image appears to be emoty. cannot plot.\\n\");\n assert( im.channels() == 3 && dst.channels() == 3 && \"[MiscUtils::plot_point_sets]src and dst image are same physical image in memory. They need to be 3 channel.\" );\n }\n else {\n // dst = cv::Mat( im.rows, im.cols, CV_8UC3 );\n if( im.channels() == 1 )\n cv::cvtColor( im, dst, cv::COLOR_GRAY2BGR );\n else\n im.copyTo(dst);\n }\n\n // cv::putText( dst, to_string(msg.length()), cv::Point(5,5), cv::FONT_HERSHEY_COMPLEX_SMALL, .95, cv::Scalar(0,255,255) );\n if( msg.length() > 0 ) {\n vector msg_split;\n msg_split = MiscUtils::split( msg, ';' );\n for( int q=0 ; q(0,i),pts_set.at(1,i) );\n cv::circle( dst, pt, 2, color, -1 );\n\n char to_s[20];\n sprintf( to_s, \"%d\", i);\n if( enable_keypoint_annotation ) {\n // cv::putText( dst, to_s, pt, cv::FONT_HERSHEY_COMPLEX_SMALL, 0.5, cv::Scalar(255,255,255) - color );\n cv::putText( dst, to_s, pt, cv::FONT_HERSHEY_COMPLEX_SMALL, 0.5, color );\n }\n\n }\n}\n\n\nvoid MiscUtils::plot_point_sets( cv::Mat& im, const MatrixXd& pts_set,\n const cv::Scalar& color, const VectorXi& annotations, const string& msg )\n{\n plot_point_sets( im, pts_set, im, color, annotations, msg );\n}\n\n\nvoid MiscUtils::plot_point_sets( cv::Mat& im, const MatrixXd& pts_set, cv::Mat& dst,\n const cv::Scalar& color, const VectorXi& annotations, const string& msg )\n\n{\n // TODO consider addressof(a) == addressof(b)\n // dst = im.clone();\n\n\n assert( im.rows > 0 && im.cols > 0 && \"\\n[MiscUtils::plot_point_sets]Image appears to be emoty. cannot plot.\\n\");\n assert( pts_set.cols() > 0 && pts_set.cols() == annotations.rows() && \"[MiscUtils::plot_point_sets] VectorXi annotation size must be equal to number of points. If you wish to use the default annotation ie. 0,1,...n use `true` instead. If you do not want annotation use `false`.\" );\n if( im.data == dst.data ) {\n // cout << \"src and dst are same\\n\";\n // src and dst images are same, so dont copy. just ensure it is a 3 channel image.\n assert( im.channels() == 3 && dst.channels() == 3 && \"[MiscUtils::plot_point_sets]src and dst image are same physical image in memory. They need to be 3 channel.\" );\n }\n else {\n // dst = cv::Mat( im.rows, im.cols, CV_8UC3 );\n if( im.channels() == 1 )\n cv::cvtColor( im, dst, cv::COLOR_GRAY2BGR );\n else\n im.copyTo(dst);\n }\n\n // cv::putText( dst, to_string(msg.length()), cv::Point(5,5), cv::FONT_HERSHEY_COMPLEX_SMALL, .95, cv::Scalar(0,255,255) );\n if( msg.length() > 0 ) {\n vector msg_split;\n msg_split = MiscUtils::split( msg, ';' );\n for( int q=0 ; q(0,i),pts_set.at(1,i) );\n pt = cv::Point2d( (float)pts_set(0,i), (float)pts_set(1,i) );\n cv::circle( dst, pt, 2, color, -1 );\n\n char to_s[20];\n sprintf( to_s, \"%d\", annotations(i) );\n cv::putText( dst, to_s, pt, cv::FONT_HERSHEY_COMPLEX_SMALL, 0.5, color );\n\n\n }\n}\n\n\nvoid MiscUtils::plot_point_sets( const cv::Mat& im, const MatrixXd& pts_set, cv::Mat& dst,\n vector& color_annotations, float alpha, const string& msg )\n{\n\n assert( im.rows > 0 && im.cols > 0 && \"\\n[MiscUtils::plot_point_sets]Image appears to be emoty. cannot plot.\\n\");\n assert( pts_set.cols() > 0 && pts_set.cols() == color_annotations.size() && \"[MiscUtils::plot_point_sets] len of color vector must be equal to number of pts.\\n\" );\n if( im.data == dst.data ) {\n // cout << \"src and dst are same\\n\";\n // src and dst images are same, so dont copy. just ensure it is a 3 channel image.\n assert( im.channels() == 3 && dst.channels() == 3 && \"[MiscUtils::plot_point_sets]src and dst image are same physical image in memory. They need to be 3 channel.\" );\n }\n else {\n // dst = cv::Mat( im.rows, im.cols, CV_8UC3 );\n if( im.channels() == 1 )\n cv::cvtColor( im, dst, cv::COLOR_GRAY2BGR );\n else\n im.copyTo(dst);\n }\n\n // cv::putText( dst, to_string(msg.length()), cv::Point(5,5), cv::FONT_HERSHEY_COMPLEX_SMALL, .95, cv::Scalar(0,255,255) );\n if( msg.length() > 0 ) {\n vector msg_split;\n msg_split = MiscUtils::split( msg, ';' );\n for( int q=0 ; q im.cols || pts_set(1,i) < 0 || pts_set(1,i) > im.rows ) { // avoid points which are outside\n n_outside_image_domain++;\n continue;\n }\n pt = cv::Point( (int)pts_set(0,i), (int)pts_set(1,i) );\n\n\n cv::Scalar _color = color_annotations[i];\n dst.at< cv::Vec3b >( pt )[0] = (uchar) ( (1.0-alpha)*(float)dst.at< cv::Vec3b >( pt )[0] + (alpha)*(float)_color[0] );\n dst.at< cv::Vec3b >( pt )[1] = (uchar) ( (1.0-alpha)*(float)dst.at< cv::Vec3b >( pt )[1] + (alpha)*(float)_color[1] );\n dst.at< cv::Vec3b >( pt )[2] = (uchar) ( (1.0-alpha)*(float)dst.at< cv::Vec3b >( pt )[2] + (alpha)*(float)_color[2] );\n\n // cv::Vec3d new_color( .5*_orgcolor[0]+.5*_color[0] + .5*_orgcolor[1]+.5*_color[1] + .5*_orgcolor[2]+.5*_color[2] )\n }\n\n if( float(n_outside_image_domain)/ float(pts_set.cols() ) > 0.2 ) // print only if u see too many outside the image\n cout << \"[MiscUtils::plot_point_sets] with color at every point, found \" << n_outside_image_domain << \" outside the image of total points to plot=\" << pts_set.cols() << \"\\n\";\n}\n\n\n\nvoid MiscUtils::plot_point_pair( const cv::Mat& imA, const MatrixXd& ptsA, int idxA,\n const cv::Mat& imB, const MatrixXd& ptsB, int idxB,\n // const VectorXd& mask,\n cv::Mat& dst,\n const cv::Scalar& color_marker,\n const cv::Scalar& color_line,\n bool annotate_pts,\n /*const vector& msg,*/\n const string& msg\n )\n{\n // ptsA : ptsB : 2xN or 3xN\n\n assert( imA.rows == imB.rows && imA.rows > 0 );\n assert( imA.cols == imB.cols && imA.cols > 0 );\n assert( ptsA.cols() == ptsB.cols() && ptsA.cols() > 0 );\n // assert( mask.size() == ptsA.cols() );\n\n cv::Mat outImg_;\n cv::hconcat(imA, imB, outImg_);\n\n cv::Mat outImg;\n if( outImg_.channels() == 3 )\n outImg = outImg_;\n else\n cv::cvtColor( outImg_, outImg, CV_GRAY2BGR );\n\n\n\n\n // loop over all points\n int count = 0;\n for( int kl=0 ; kl 0 ) { // ':' separated. Each will go in new line\n std::vector msg_tokens = split(msg, ';');\n for( int h=0 ; h(0,i) = i;\n cv::applyColorMap(colormap_gray, colormap_color, cv::COLORMAP_JET\t);\n if( f<0 ) {\n cv::Vec3b f_ = colormap_color.at(0, (int)0 );\n cv::Scalar color_marker = cv::Scalar(f_[0],f_[1],f_[2]);\n return color_marker;\n }\n if( f>1. ) {\n cv::Vec3b f_ = colormap_color.at(0, (int)255 );\n cv::Scalar color_marker = cv::Scalar(f_[0],f_[1],f_[2]);\n return color_marker;\n }\n\n int idx = (int) (f*255.);\n cv::Vec3b f_ = colormap_color.at(0, (int)idx );\n cv::Scalar color_marker = cv::Scalar(f_[0],f_[1],f_[2]);\n return color_marker;\n}\n\nvoid MiscUtils::plot_point_pair( const cv::Mat& imA, const MatrixXd& ptsA, int idxA,\n const cv::Mat& imB, const MatrixXd& ptsB, int idxB,\n cv::Mat& dst,\n short color_map_direction,\n const string& msg\n )\n{\n // ptsA : ptsB : 2xN or 3xN\n assert( color_map_direction >= 0 && color_map_direction<=3 );\n assert( imA.rows == imB.rows && imA.rows > 0 );\n assert( imA.cols == imB.cols && imA.cols > 0 );\n assert( ptsA.cols() == ptsB.cols() && ptsA.cols() > 0 );\n // assert( mask.size() == ptsA.cols() );\n\n\n // make colormap\n cv::Mat colormap_gray = cv::Mat::zeros( 1, 256, CV_8UC1 );\n for( int i=0 ; i<256; i++ ) colormap_gray.at(0,i) = i;\n cv::Mat colormap_color;\n cv::applyColorMap(colormap_gray, colormap_color, cv::COLORMAP_HSV);\n\n\n\n cv::Mat outImg_;\n cv::hconcat(imA, imB, outImg_);\n\n cv::Mat outImg;\n if( outImg_.channels() == 3 )\n outImg = outImg_;\n else\n cv::cvtColor( outImg_, outImg, CV_GRAY2BGR );\n\n\n\n\n\n // loop over all points\n int count = 0;\n for( int kl=0 ; kl255 ) coloridx=0;\n cv::Vec3b f = colormap_color.at(0, (int)coloridx );\n cv::Scalar color_marker = cv::Scalar(f[0],f[1],f[2]);\n\n cv::circle( outImg, A, 2,color_marker, -1 );\n cv::circle( outImg, B+cv::Point2d(imA.cols,0), 2,color_marker, -1 );\n\n /*\n cv::line( outImg, A, B+cv::Point2d(imA.cols,0), color_line );\n\n if( annotate_pts )\n {\n cv::putText( outImg, to_string(kl), A, cv::FONT_HERSHEY_SIMPLEX, 0.3, color_marker, 1 );\n cv::putText( outImg, to_string(kl), B+cv::Point2d(imA.cols,0), cv::FONT_HERSHEY_SIMPLEX, 0.3, color_marker, 1 );\n }\n */\n }\n\n\n\n cv::Mat status = cv::Mat(150, outImg.cols, CV_8UC3, cv::Scalar(0,0,0) );\n cv::putText( status, to_string(idxA).c_str(), cv::Point(10,30), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255,255,255), 2 );\n cv::putText( status, to_string(idxB).c_str(), cv::Point(imA.cols+10,30), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255,255,255), 2 );\n cv::putText( status, \"marked # pts: \"+to_string(count), cv::Point(10,60), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255,255,255), 1.5 );\n\n\n // put msg in status image\n if( msg.length() > 0 ) { // ':' separated. Each will go in new line\n std::vector msg_tokens = split(msg, ';');\n for( int h=0 ; h2)?0.4:txt_size;\n\n std::vector msg_tokens = split(msg, ';');\n int status_im_height = 50+20*msg_tokens.size();\n\n cv::Mat status;\n if( is_single_channel )\n status = cv::Mat(status_im_height, im.cols, CV_8UC1, cv::Scalar(0,0,0) );\n else\n status = cv::Mat(status_im_height, im.cols, CV_8UC3, bg_color );\n\n\n for( int h=0 ; h\" << b << endl;\n // cv::line( im, a, b, cv::Scalar(255,255,255) );\n MiscUtils::draw_fullLine( im, a, b, color );\n}\n\n// mark point on the image, pt is in homogeneous co-ordinate.\nvoid MiscUtils::draw_point( const Vector3d pt, cv::Mat& im, cv::Scalar color )\n{\n // C++: void circle(Mat& img, Point center, int radius, const Scalar& color, int thickness=1, int lineType=8, int shift=0)\n cv::circle( im, cv::Point2f( pt(0)/pt(2), pt(1)/pt(2) ), 2, color, -1 );\n\n}\n\n// mark point on image\nvoid MiscUtils::draw_point( const Vector2d pt, cv::Mat& im, cv::Scalar color )\n{\n cv::circle( im, cv::Point2f( pt(0), pt(1) ), 2, color, -1 );\n}\n" }, { "path": "src/utils/MiscUtils.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n//opencv\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \nusing namespace Eigen;\n#include \n\nusing namespace std;\n\n#include \"GMSMatcher/gms_matcher.h\"\n#include \"ElapsedTime.h\"\n\n#include \n#include \n\n\nclass MiscUtils\n{\npublic:\n static string type2str(int type);\n static string cvmat_info( const cv::Mat& mat );\n static string imgmsg_info(const sensor_msgs::ImageConstPtr &img_msg);\n static string imgmsg_info(const sensor_msgs::Image& img_msg);\n static cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg);\n\n static std::vector\n split( std::string const& original, char separator );\n\n\n //---------------------------- Conversions ---------------------------------//\n // convert from opencv format of keypoints to Eigen\n static void keypoint_2_eigen( const std::vector& kp, MatrixXd& uv, bool make_homogeneous=true );\n\n // given opencv keypoints and DMatch will produce M1, and M2 the co-ordinates\n static void dmatch_2_eigen( const std::vector& kp1, const std::vector& kp2,\n const std::vector matches,\n MatrixXd& M1, MatrixXd& M2,\n bool make_homogeneous=true\n );\n\n\n //---------------------------- Conversions ---------------------------------//\n\n\n\n //--------------------- Plot Keypoints on Image ----------------------------//\n // Eigen Interace: PLotting functions with Eigen Interfaces\n static void plot_point_sets( const cv::Mat& im, const MatrixXd& pts_set, cv::Mat& dst,\n const cv::Scalar& color, bool enable_keypoint_annotation=true,const string& msg=string(\"\") );\n\n // cv::Mat Interfaces: Plotting Functions with cv::Mat Interfaces.\n static void plot_point_sets( const cv::Mat& im, const cv::Mat& pts_set, cv::Mat& dst,\n const cv::Scalar& color, bool enable_keypoint_annotation=true, const string& msg=string(\"\") );\n\n // Inplace plotting. Here dont need to specify a separate destination. src is modified.\n static void plot_point_sets( cv::Mat& im, const MatrixXd& pts_set,\n const cv::Scalar& color, bool enable_keypoint_annotation=true, const string& msg=string(\"\") );\n\n // Plotting with annotations specified by VectorXi\n static void plot_point_sets( cv::Mat& im, const MatrixXd& pts_set, cv::Mat& dst,\n const cv::Scalar& color, const VectorXi& annotations, const string& msg );\n\n // Plotting with annotations specified by VectorXi inplace\n static void plot_point_sets( cv::Mat& im, const MatrixXd& pts_set,\n const cv::Scalar& color, const VectorXi& annotations, const string& msg );\n\n // plot point with colors specified at every point. pts_set : 3xN or 2xN, len(color_annotations) == pts_set.cols()\n static void plot_point_sets( const cv::Mat& im, const MatrixXd& pts_set, cv::Mat& dst,\n vector& color_annotations, float alpha=0.8, const string& msg=string(\"N/A\") );\n\n // END--------------------- Plot Keypoints on Image ----------------------------//\n\n\n\n\n //------------------------------- Plot Matchings on image pair -------------------------//\n\n // Plots [ imA | imaB ] with points correspondences\n // [Input]\n // imA, imB : Images\n // ptsA, ptsB : 2xN or 3xN\n // idxA, idxB : Index of each of the image. This will appear in status part. No other imppact of these.\n // color_marker : color of the point marker\n // color_line : color of the line\n // annotate_pts : true with putText for each point. False will not putText.\n // [Output]\n // outImg : Output image\n static void plot_point_pair( const cv::Mat& imA, const MatrixXd& ptsA, int idxA,\n const cv::Mat& imB, const MatrixXd& ptsB, int idxB,\n cv::Mat& dst,\n const cv::Scalar& color_marker,\n const cv::Scalar& color_line=cv::Scalar(0,255,0),\n bool annotate_pts=false,\n const string& msg=string(\"N.A\")\n );\n\n\n // nearly same as the above, but will color every co-ordinate with different color\n // color_map_direction : 0 ==> // horizontal-gradiant\n // 1 ==> // vertical-gradiant\n // 2 ==> // manhattan-gradiant\n // 3 ==> // image centered manhattan-gradiant\n static void plot_point_pair( const cv::Mat& imA, const MatrixXd& ptsA, int idxA,\n const cv::Mat& imB, const MatrixXd& ptsB, int idxB,\n cv::Mat& dst,\n short color_map_direction,\n const string& msg=string(\"N.A\")\n );\n\n //------------------------------- Plot Matchings on image pair -------------------------//\n\n\n //------------------------- Points and Lines on Images --------------------------------//\n\n // Given two image-points draw line between them, extend both ways. Infinite line-segments\n static void draw_fullLine(cv::Mat& img, cv::Point2f a, cv::Point2f b, cv::Scalar color);\n\n // draw line on the image, given a line equation in homogeneous co-ordinates. l = (a,b,c) for ax+by+c = 0\n static void draw_line( const Vector3d l, cv::Mat& im, cv::Scalar color );\n\n // mark point on the image, pt is in homogeneous co-ordinate.\n static void draw_point( const Vector3d pt, cv::Mat& im, cv::Scalar color );\n\n // mark point on image\n static void draw_point( const Vector2d pt, cv::Mat& im, cv::Scalar color );\n\n\n // append a status image . ';' separated\n static void append_status_image( cv::Mat& im, const string& msg, float txt_size=0.4, cv::Scalar bg_color=cv::Scalar(0,0,0), cv::Scalar txt_color=cv::Scalar(255,255,255), float line_thinkness=1.5 );\n static bool side_by_side( const cv::Mat& A, const cv::Mat& B, cv::Mat& dst );\n static bool vertical_side_by_side( const cv::Mat& A, const cv::Mat& B, cv::Mat& dst );\n\n // END ------------------------- Points and Lines on Images --------------------------------//\n\n\n // [Input] : f a float between 0 and 1.\n // [Output]: cv::Scalar gives out a bgr color pallet.\n // Note: This is inefficient, don't use it too often. If you are going to do lot of quering for colors use `class FalseColors`\n static cv::Scalar getFalseColor( float f );\n\n\nprivate:\n\n static double Slope(int x0, int y0, int x1, int y1);\n\n};\n\n\nclass FalseColors\n{\npublic:\n FalseColors() {\n cv::Mat colormap_gray = cv::Mat::zeros( 1, 256, CV_8UC1 );\n for( int i=0 ; i<256; i++ ) colormap_gray.at(0,i) = i;\n cv::applyColorMap(colormap_gray, colormap_color, cv::COLORMAP_JET\t);\n }\n\n cv::Scalar getFalseColor( float f ) {\n int idx = (int) (f*255.);\n if( f<0 ) {\n idx=0;\n }\n if( f>255 ) {\n idx=255;\n }\n\n\n cv::Vec3b f_ = colormap_color.at(0, (int)idx );\n cv::Scalar color_marker = cv::Scalar(f_[0],f_[1],f_[2]);\n return color_marker;\n }\n\n cv::Mat getStrip( int nrows, int ncols ) {\n cv::Mat colormap_gray = cv::Mat::zeros( nrows, ncols, CV_8UC1 );\n\n for( int r=0; r(r,c) = (uchar) ( (float(c)/ncols)*256 );\n }\n\n cv::Mat __dst;\n cv::applyColorMap(colormap_gray, __dst, cv::COLORMAP_JET\t);\n return __dst;\n }\n\n\nprivate:\n cv::Mat colormap_color;\n\n\n};\n" }, { "path": "src/utils/Plot2Mat.cpp", "content": "#include \"Plot2Mat.h\"\n\nPlot2Mat::Plot2Mat() {\n im_width = 640;\n im_height = 480;\n bg_color = cv::Scalar(80,80,80);\n create( bg_color );\n}\n\nPlot2Mat::Plot2Mat(int _width, int _height, cv::Scalar _bg_color)\n{\n im_width = _width;\n im_height = _height;\n bg_color = _bg_color;\n create( bg_color );\n}\n\n\nvoid Plot2Mat::resetCanvas() {\n create( bg_color );\n}\n\nvoid Plot2Mat::setYminmax( float _ymin, float _ymax ) {\n assert( _ymin<_ymax );\n ymin=_ymin;\n ymax=_ymax;\n setYminmax_dynamically = false;\n}\n\nvoid Plot2Mat::setYminmaxDynamic() {\n setYminmax_dynamically = true;\n}\n\nconst cv::Mat& Plot2Mat::getCanvasConstPtr()\n{\n return (const cv::Mat&) im;\n}\n\n\nvoid Plot2Mat::create( const cv::Scalar bg_color )\n{\n this->im = cv::Mat( int(im_height*1.2), int(im_width*1.2), CV_8UC3, bg_color );\n}\n\n\n\nbool Plot2Mat::plot( const VectorXd& y, const cv::Scalar line_color, bool mark_plottedpts, bool label_plottedpts )\n{\n // cout << \"y.rows = \" << y.rows() << endl;\n L = y.rows();\n assert( y.rows() > 0 );\n\n\n auto x = VectorXd( L );\n for( int i=0; i pt[i]\n for( int i=1 ; i0 && \"you called mark before plotting. the intended use is to call plot and then optionally call mark\" );\n float xd = float(x_) / float(L) * im_width;\n auto c = cv::Point2f( xd, im_height );\n cv::circle( im, c, 7, color, -1 );\n\n if( enable_text_label )\n cv::putText(im, to_string(x_), c, cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, color, 1, CV_AA);\n\n}\n" }, { "path": "src/utils/Plot2Mat.h", "content": "#pragma once\n/**\nA OpenCV based plotting. The idea is, given a vector (of floats) to\nplot them and make a cv::Mat out of it.\n\nAuthor : Manohar Kuse \nCreated : 9th Apr, 2019\n**/\n\n\n/****\n\nimport cv2\nimport numpy as np\n\nimport matplotlib\nimport code\n\nclass Plot2Mat:\n def __init__(self):\n self.width = 640\n self.height = 480\n self.im = None\n self.create()\n\n def xprint( self, msg ):\n print '[Plot2Mat]', msg\n\n def create( self ):\n self.im = np.zeros( ( int(self.height*1.2), int(self.width*1.2), 3) ) + 80\n\n def mark( self, xm_array, color=(0,0,255) ):\n \"\"\" Give 1 iteration number (x-axis) to mark. This can be called after plot \"\"\"\n # self.xprint( 'mark called'+str(xm_array.shape) )\n if self.im is None:\n return self.im\n # self.xprint( str(xm_array.shape) )\n for xm in xm_array:\n # self.xprint( str(xm) )\n xd = xm / self.L * self.width\n c = ( int(xd), int(self.height) )\n self.im = cv2.circle( self.im, c, 7, color, -1 )\n\n cv2.putText(self.im, 'loop_items = %d' %(len(xm_array)), ( 10,40 ), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (255,255,255), 3)\n return self.im\n\n\n\n def plot( self, y, line_color=(0,255,0) ):\n \"\"\" y is a 1-d array. Plot with indices. \"\"\"\n im = self.im\n\n\n x = np.array( range( len(y) ) ) # [0 to N] ---> [ 0 to self.width ]\n L = float( len(x) )\n self.L = L\n\n # y [ min(y) to max(y) ] ---> [ 0 to self.height ]\n\n # code.interact( local=locals() )\n xd = x / ( L ) * (self.width)\n\n ymin = 0.0#y.min()\n ymax = 1.0#y.max()\n yd = (y - ymin) / (ymax - ymin) * (self.height)\n\n # Plot each point as circle\n for i in range( int(L) ):\n # print 'draw circle at ', xd[i], yd[i]\n c = ( int(xd[i]), int(-yd[i] + self.height) ) # circle center\n # cv2.circle(im, c, 2, (0,0,255) )\n\n c_xlabel = ( int(xd[i]), int(0.0 + self.height) )\n if i % int(L/5) == 0 :\n cv2.circle(im, c_xlabel, 4, (255,255,255), -1 )\n cv2.putText(im,str(i), (c_xlabel[0],c_xlabel[1]+50), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (255,255,255), 3)\n\n cv2.line( im, c_xlabel, (c_xlabel[0],0), (255,255,255), 2 )\n\n\n for i in range( 1, int(L) ):\n p1 = ( int(xd[i]), int(-yd[i] + self.height) ) # circle center\n p2 = ( int(xd[i-1]), int(-yd[i-1] + self.height) ) # circle center\n cv2.line( im, p1, p2, line_color, 3 )\n\n # cv2.line( )\n\n\n for _yd in np.linspace( 0, 1, 11 ):\n #line (in image cord space): (0, _y) --- (self.width, _y)\n _y = (_yd) / (ymax - ymin) * self.height + ymin\n _y = int(-_y+self.height)\n # print '(%d,%d) <---> (%d,%d)' %(0,_y, self.width, _y)\n cv2.line( im, (0,_y), (self.width, _y), (255,255,255),3)\n\n cv2.putText(im,str(_yd),(self.width+40, _y+10), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255,255,255), 3)\n\n self.im = im\n return im\n\n****/\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n//opencv\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \nusing namespace Eigen;\n#include \n\nusing namespace std;\n\n\nclass Plot2Mat {\n\npublic:\n Plot2Mat();\n Plot2Mat(int _width, int _height, cv::Scalar _bg_color);\n\n // Canvas controllers\n void resetCanvas();\n\n void setYminmax( float _ymin, float _ymax );\n void setYminmaxDynamic();\n\n\n\n bool mark( const int x_, const cv::Scalar color=cv::Scalar(0,0,255), bool enable_text_label=true );\n\n\n // y is a 1-d array. Plot with indices.\n // mark_plottedpts : This will draw circles for each pt plotted.\n // label_plottedpts : This will write in text (using cv::putText) the co-ordinate\n bool plot( const VectorXd& y, const cv::Scalar line_color=cv::Scalar(0,255,0), bool mark_plottedpts=false, bool label_plottedpts=false );\n\n const cv::Mat& getCanvasConstPtr();\n\nprivate:\n void create(const cv::Scalar bg_color=cv::Scalar(80,80,80));\n int im_width;\n int im_height;\n cv::Mat im;\n cv::Scalar bg_color = cv::Scalar(80,80,80);\n\n int L=-1;//length of y-plot\n\n double ymin, ymax;\n bool setYminmax_dynamically=true;\n};\n" }, { "path": "src/utils/PointFeatureMatching.cpp", "content": "#include \"PointFeatureMatching.h\"\n\n// #define ___StaticPointFeatureMatching__gms_point_feature_matches( msg ) msg;\n#define ___StaticPointFeatureMatching__gms_point_feature_matches( msg ) ;\nvoid StaticPointFeatureMatching::gms_point_feature_matches( const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n MatrixXd& u, MatrixXd& ud, int n_orb_feat )\n{\n ElapsedTime timer;\n\n\n //\n // Point feature and descriptors extract\n std::vector kp1, kp2;\n cv::Mat d1, d2; //< descriptors\n\n // cv::Ptr orb = cv::ORB::create(5000);\n cv::Ptr orb = cv::ORB::create(n_orb_feat);\n orb->setFastThreshold(0);\n\n ___StaticPointFeatureMatching__gms_point_feature_matches(timer.tic();)\n orb->detectAndCompute(imleft_undistorted, cv::Mat(), kp1, d1);\n orb->detectAndCompute(imright_undistorted, cv::Mat(), kp2, d2);\n ___StaticPointFeatureMatching__gms_point_feature_matches(cout << timer.toc_milli() << \" (ms) 2X detectAndCompute(ms) : \"<< endl;)\n // std::cout << \"d1 \" << MiscUtils::cvmat_info( d1 ) << std::endl;\n // std::cout << \"d2 \" << MiscUtils::cvmat_info( d2 ) << std::endl;\n\n //plot\n // cv::Mat dst_left, dst_right;\n // MatrixXd e_kp1, e_kp2;\n // MiscUtils::keypoint_2_eigen( kp1, e_kp1 );\n // MiscUtils::keypoint_2_eigen( kp2, e_kp2 );\n // MiscUtils::plot_point_sets( imleft_undistorted, e_kp1, dst_left, cv::Scalar(0,0,255), false );\n // MiscUtils::plot_point_sets( imright_undistorted, e_kp2, dst_right, cv::Scalar(0,0,255), false );\n // cv::imshow( \"dst_left\", dst_left );\n // cv::imshow( \"dst_right\", dst_right );\n\n //\n // Point feature matching\n cv::BFMatcher matcher(cv::NORM_HAMMING); // TODO try FLANN matcher here.\n vector matches_all, matches_gms;\n ___StaticPointFeatureMatching__gms_point_feature_matches(timer.tic();)\n matcher.match(d1, d2, matches_all);\n ___StaticPointFeatureMatching__gms_point_feature_matches(\n std::cout << timer.toc_milli() << \" : (ms) BFMatcher took (ms)\\t\";\n std::cout << \"BFMatcher : npts = \" << matches_all.size() << std::endl;\n )\n\n\n // gms_matcher\n ___StaticPointFeatureMatching__gms_point_feature_matches(timer.tic();)\n std::vector vbInliers;\n gms_matcher gms(kp1, imleft_undistorted.size(), kp2, imright_undistorted.size(), matches_all);\n int num_inliers = gms.GetInlierMask(vbInliers, false, false);\n ___StaticPointFeatureMatching__gms_point_feature_matches(\n cout << timer.toc_milli() << \" : (ms) GMSMatcher took.\\t\" ;\n cout << \"Got total gms matches \" << num_inliers << \" matches.\" << endl;\n )\n\n // collect matches\n for (size_t i = 0; i < vbInliers.size(); ++i)\n {\n if (vbInliers[i] == true)\n {\n matches_gms.push_back(matches_all[i]);\n }\n }\n // MatrixXd M1, M2;\n if( matches_gms.size() > 0 )\n MiscUtils::dmatch_2_eigen( kp1, kp2, matches_gms, u, ud, true );\n\n\n}\n\n\n\n\n//-----------------------------------------------\n//-------- NEW ----------------------------------\n//-----------------------------------------------\n// Given the point feature matches and the 3d image (from disparity map) will return\n// the valid world points and corresponding points.\n// [Input]\n// uv: 2xN matrix of point-feature in image-a. In image co-ordinates (not normalized image cords)\n// _3dImage_uv : 3d image from disparity map of image-a. sizeof( _3dImage_uv) === WxHx3\n// uv_d: 2xN matrix of point-feature in image-b. Note that uv<-->uv_d are correspondences so should of equal sizes\n// [Output]\n// feature_position_uv : a subset of uv but normalized_image_cordinates\n// feature_position_uv_d : a subset of uv_d. results in normalized_image_cordinates\n// world_point : 3d points of uv.\n// [Note]\n// feature_position_uv \\subset uv. Selects points which have valid depths.\n// size of output is same for all 3\n// world points are of uv and in co-ordinate system of camera center of uv (or image-a).\n\nbool StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity( std::shared_ptr stereogeom,\n const MatrixXd& uv, const cv::Mat& _3dImage_uv, const MatrixXd& uv_d,\n std::vector& feature_position_uv, std::vector& feature_position_uv_d,\n std::vector& world_point )\n{\n assert( (uv.cols() == uv_d.cols() ) && \"[StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity] pf-matches need to be of same length. You provided of different lengths\\n\" );\n assert( _3dImage_uv.type() == CV_32FC3 );\n\n if( uv.cols() != uv_d.cols() ) {\n cout << TermColor::RED() << \"[StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity] pf-matches need to be of same length. You provided of different lengths\\n\" << TermColor::RESET();\n return false;\n }\n\n if( _3dImage_uv.type() != CV_32FC3 && _3dImage_uv.rows <= 0 && _3dImage_uv.cols <= 0 ) {\n cout << TermColor::RED() << \"[StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity] _3dImage is expected to be of size CV_32FC3\\n\" << TermColor::RESET();\n return false;\n }\n\n\n\n int c = 0;\n MatrixXd ud_normalized = stereogeom->get_K().inverse() * uv_d;\n MatrixXd u_normalized = stereogeom->get_K().inverse() * uv;\n feature_position_uv.clear();\n feature_position_uv_d.clear();\n world_point.clear();\n\n for( int k=0 ; k( (int)uv(1,k), (int)uv(0,k) );\n if( _3dpt[2] < 0.1 || _3dpt[2] > 25. )\n continue;\n\n c++;\n #if 0\n cout << TermColor::RED() << \"---\" << k << \"---\" << TermColor::RESET() << endl;\n cout << \"ud=\" << ud.col(k).transpose() ;\n cout << \" <--> \";\n cout << \"u=\" << u.col(k).transpose() ;\n cout << \" 3dpt of u=\";\n cout << TermColor::GREEN() << _3dpt[0] << \" \" << _3dpt[1] << \" \" << _3dpt[2] << \" \" << TermColor::RESET();\n cout << endl;\n #endif\n\n feature_position_uv.push_back( Vector2d( u_normalized(0,k), u_normalized(1,k) ) );\n feature_position_uv_d.push_back( Vector2d( ud_normalized(0,k), ud_normalized(1,k) ) );\n world_point.push_back( Vector3d( _3dpt[0], _3dpt[1], _3dpt[2] ) );\n }\n\n // cout << \"[make_3d_2d_collection__using__pfmatches_and_disparity]of the total \" << uv.cols() << \" point feature correspondences \" << c << \" had valid depths\\n\";\n\n return true;\n\n if( c < 30 ) {\n cout << TermColor::RED() << \"too few valid 3d points between frames\" << TermColor::RESET() << endl;\n return false;\n }\n\n}\n\n// given pf-matches uv<-->ud_d and their _3dImages. returns the 3d point correspondences at points where it is valid\n// uv_X: the 3d points are in frame of ref of camera-uv\n// uvd_Y: these 3d points are in frame of ref of camera-uvd\nbool StaticPointFeatureMatching::make_3d_3d_collection__using__pfmatches_and_disparity(\n const MatrixXd& uv, const cv::Mat& _3dImage_uv,\n const MatrixXd& uv_d, const cv::Mat& _3dImage_uv_d,\n vector& uv_X, vector& uvd_Y\n)\n{\n assert( uv.cols() > 0 && uv.cols() == uv_d.cols() );\n uv_X.clear();\n uvd_Y.clear();\n if( uv.cols() != uv_d.cols() ) {\n cout << TermColor::RED() << \"[StaticPointFeatureMatching::make_3d_3d_collection__using__pfmatches_and_disparity] pf-matches need to be of same length. You provided of different lengths\\n\" << TermColor::RESET();\n return false;\n }\n\n if( _3dImage_uv.type() != CV_32FC3 && _3dImage_uv.rows <= 0 && _3dImage_uv.cols <= 0 ) {\n cout << TermColor::RED() << \"[StaticPointFeatureMatching::make_3d_3d_collection__using__pfmatches_and_disparity] _3dImage is expected to be of size CV_32FC3\\n\" << TermColor::RESET();\n return false;\n }\n\n\n // similar to above but should return world_point__uv and world_point__uv_d\n int c=0;\n for( int k=0 ; k( (int)uv(1,k), (int)uv(0,k) );\n cv::Vec3f uvd_3dpt = _3dImage_uv_d.at( (int)uv_d(1,k), (int)uv_d(0,k) );\n\n if( uv_3dpt[2] < 0.1 || uv_3dpt[2] > 25. || uvd_3dpt[2] < 0.1 || uvd_3dpt[2] > 25. )\n continue;\n\n uv_X.push_back( Vector3d( uv_3dpt[0], uv_3dpt[1], uv_3dpt[2] ) );\n uvd_Y.push_back( Vector3d( uvd_3dpt[0], uvd_3dpt[1], uvd_3dpt[2] ) );\n c++;\n\n }\n // cout << \"[make_3d_3d_collection__using__pfmatches_and_disparity] of the total \" << uv.cols() << \" point feature correspondences \" << c << \" had valid depths\\n\";\n return true;\n}\n" }, { "path": "src/utils/PointFeatureMatching.h", "content": "#pragma once\n#include \n#include \n#include \n#include \n#include \n#include \n#include //for std::shared_ptr\n\n\n//opencv\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \nusing namespace Eigen;\n#include \n\nusing namespace std;\n\n#include \"GMSMatcher/gms_matcher.h\"\n#include \"ElapsedTime.h\"\n#include \"MiscUtils.h\"\n#include \"TermColor.h\"\n#include \"CameraGeometry.h\"\n\n\nclass StaticPointFeatureMatching\n{\npublic:\n\n static void gms_point_feature_matches( const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n MatrixXd& u, MatrixXd& ud, int n_orb_feat=5000 ); //< n_orb_feat has to be a few thousands atleast for spatial consistency checks.\n\n\n // Given the point feature matches and the 3d image (from disparity map) will return\n // the valid world points and corresponding points.\n // [Input]\n // uv: 2xN matrix of point-feature in image-a. In image co-ordinates (not normalized image cords)\n // _3dImage_uv : 3d image from disparity map of image-a. sizeof( _3dImage_uv) === WxHx3\n // uv_d: 2xN matrix of point-feature in image-b. Note that uv<-->uv_d are correspondences so should of equal sizes\n // [Output]\n // feature_position_uv : a subset of uv but normalized_image_cordinates\n // feature_position_uv_d : a subset of uv_d. results in normalized_image_cordinates\n // world_point : 3d points of uv.\n // [Note]\n // feature_position_uv \\subset uv. Selects points which have valid depths.\n // size of output is same for all 3\n // world points are of uv and in co-ordinate system of camera center of uv (or image-a).\n static bool make_3d_2d_collection__using__pfmatches_and_disparity( std::shared_ptr stereogeom,\n const MatrixXd& uv, const cv::Mat& _3dImage_uv, const MatrixXd& uv_d,\n std::vector& feature_position_uv, std::vector& feature_position_uv_d,\n std::vector& world_point );\n\n\n // given pf-matches uv<-->ud_d and their _3dImages. returns the 3d point correspondences at points where it is valid\n // uv_X: the 3d points are in frame of ref of camera-uv\n // uvd_Y: these 3d points are in frame of ref of camera-uvd\n static bool make_3d_3d_collection__using__pfmatches_and_disparity(\n const MatrixXd& uv, const cv::Mat& _3dImage_uv,\n const MatrixXd& uv_d, const cv::Mat& _3dImage_uv_d,\n vector& uv_X, vector& uvd_Y\n );\n\n};\n" }, { "path": "src/utils/PoseManipUtils.cpp", "content": "#include \"PoseManipUtils.h\"\n\nvoid PoseManipUtils::raw_to_eigenmat( const double * quat, const double * t, Matrix4d& dstT )\n{\n Quaterniond q = Quaterniond( quat[0], quat[1], quat[2], quat[3] );\n\n dstT = Matrix4d::Zero();\n dstT.topLeftCorner<3,3>() = q.toRotationMatrix();\n\n dstT(0,3) = t[0];\n dstT(1,3) = t[1];\n dstT(2,3) = t[2];\n dstT(3,3) = 1.0;\n}\n\n#ifdef __PoseManipUtils__with_ROS\nvoid PoseManipUtils::geometry_msgs_Pose_to_eigenmat( const geometry_msgs::Pose& pose, Matrix4d& dstT )\n{\n Quaterniond q = Quaterniond( pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z );\n\n dstT = Matrix4d::Zero();\n dstT.topLeftCorner<3,3>() = q.toRotationMatrix();\n\n dstT(0,3) = pose.position.x;\n dstT(1,3) = pose.position.y;\n dstT(2,3) = pose.position.z;\n dstT(3,3) = 1.0;\n}\n\n\nvoid PoseManipUtils::eigenmat_to_geometry_msgs_Pose( const Matrix4d& T, geometry_msgs::Pose& pose )\n{\n assert( T(3,3) == 1 );\n Quaterniond q( T.topLeftCorner<3,3>() );\n\n pose.position.x = T(0,3);\n pose.position.y = T(1,3);\n pose.position.z = T(2,3);\n pose.orientation.x = q.x();\n pose.orientation.y = q.y();\n pose.orientation.z = q.z();\n pose.orientation.w = q.w();\n\n}\n#endif\n\nvoid PoseManipUtils::eigenmat_to_raw( const Matrix4d& T, double * quat, double * t)\n{\n assert( T(3,3) == 1 );\n Quaterniond q( T.topLeftCorner<3,3>() );\n quat[0] = q.w();\n quat[1] = q.x();\n quat[2] = q.y();\n quat[3] = q.z();\n t[0] = T(0,3);\n t[1] = T(1,3);\n t[2] = T(2,3);\n}\n\n\nvoid PoseManipUtils::raw_xyzw_to_eigenmat( const double * quat, const double * t, Matrix4d& dstT )\n{\n Quaterniond q = Quaterniond( quat[3], quat[0], quat[1], quat[2] );\n\n dstT = Matrix4d::Zero();\n dstT.topLeftCorner<3,3>() = q.toRotationMatrix();\n\n dstT(0,3) = t[0];\n dstT(1,3) = t[1];\n dstT(2,3) = t[2];\n dstT(3,3) = 1.0;\n}\n\nvoid PoseManipUtils::raw_xyzw_to_eigenmat( const Vector4d& quat, const Vector3d& t, Matrix4d& dstT )\n{\n Quaterniond q = Quaterniond( quat(3), quat(0), quat(1), quat(2) );\n\n dstT = Matrix4d::Zero();\n dstT.topLeftCorner<3,3>() = q.toRotationMatrix();\n\n dstT(0,3) = t(0);\n dstT(1,3) = t(1);\n dstT(2,3) = t(2);\n dstT(3,3) = 1.0;\n}\n\nvoid PoseManipUtils::eigenmat_to_raw_xyzw( const Matrix4d& T, double * quat, double * t)\n{\n assert( T(3,3) == 1 );\n Quaterniond q( T.topLeftCorner<3,3>() );\n quat[3] = q.w();\n quat[0] = q.x();\n quat[1] = q.y();\n quat[2] = q.z();\n t[0] = T(0,3);\n t[1] = T(1,3);\n t[2] = T(2,3);\n}\n\n\nvoid PoseManipUtils::rawyprt_to_eigenmat( const double * ypr, const double * t, Matrix4d& dstT )\n{\n dstT = Matrix4d::Identity();\n Vector3d eigen_ypr;\n eigen_ypr << ypr[0], ypr[1], ypr[2];\n dstT.topLeftCorner<3,3>() = ypr2R( eigen_ypr );\n dstT(0,3) = t[0];\n dstT(1,3) = t[1];\n dstT(2,3) = t[2];\n}\n\n// input ypr must be in degrees.\nvoid PoseManipUtils::rawyprt_to_eigenmat( const Vector3d& eigen_ypr_degrees, const Vector3d& t, Matrix4d& dstT )\n{\n dstT = Matrix4d::Identity();\n dstT.topLeftCorner<3,3>() = ypr2R( eigen_ypr_degrees );\n dstT(0,3) = t(0);\n dstT(1,3) = t(1);\n dstT(2,3) = t(2);\n}\n\nvoid PoseManipUtils::eigenmat_to_rawyprt( const Matrix4d& T, double * ypr, double * t)\n{\n assert( T(3,3) == 1 );\n Vector3d T_cap_ypr = R2ypr( T.topLeftCorner<3,3>() );\n ypr[0] = T_cap_ypr(0);\n ypr[1] = T_cap_ypr(1);\n ypr[2] = T_cap_ypr(2);\n\n t[0] = T(0,3);\n t[1] = T(1,3);\n t[2] = T(2,3);\n}\n\nvoid PoseManipUtils::eigenmat_to_rawyprt( const Matrix4d& T, Vector3d& ypr, Vector3d& t)\n{\n assert( T(3,3) == 1 );\n Vector3d T_cap_ypr = R2ypr( T.topLeftCorner<3,3>() );\n ypr(0) = T_cap_ypr(0);\n ypr(1) = T_cap_ypr(1);\n ypr(2) = T_cap_ypr(2);\n\n t(0) = T(0,3);\n t(1) = T(1,3);\n t(2) = T(2,3);\n}\n\nVector3d PoseManipUtils::R2ypr( const Matrix3d& R)\n{\n Eigen::Vector3d n = R.col(0);\n Eigen::Vector3d o = R.col(1);\n Eigen::Vector3d a = R.col(2);\n\n Eigen::Vector3d ypr(3);\n double y = atan2(n(1), n(0));\n double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));\n double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));\n ypr(0) = y;\n ypr(1) = p;\n ypr(2) = r;\n\n return ypr / M_PI * 180.0;\n}\n\n\nMatrix3d PoseManipUtils::ypr2R( const Vector3d& ypr)\n{\n double y = ypr(0) / 180.0 * M_PI;\n double p = ypr(1) / 180.0 * M_PI;\n double r = ypr(2) / 180.0 * M_PI;\n\n // Eigen::Matrix Rz;\n Matrix3d Rz;\n Rz << cos(y), -sin(y), 0,\n sin(y), cos(y), 0,\n 0, 0, 1;\n\n // Eigen::Matrix Ry;\n Matrix3d Ry;\n Ry << cos(p), 0., sin(p),\n 0., 1., 0.,\n -sin(p), 0., cos(p);\n\n // Eigen::Matrix Rx;\n Matrix3d Rx;\n Rx << 1., 0., 0.,\n 0., cos(r), -sin(r),\n 0., sin(r), cos(r);\n\n return Rz * Ry * Rx;\n}\n\nvoid PoseManipUtils::prettyprintPoseMatrix( const Matrix4d& M )\n{\n cout << \"YPR : \" << R2ypr( M.topLeftCorner<3,3>() ).transpose() << \"; \";\n cout << \"Tx,Ty,Tz : \" << M(0,3) << \", \" << M(1,3) << \", \" << M(2,3) << endl;\n}\n\nvoid PoseManipUtils::prettyprintPoseMatrix( const Matrix4d& M, string& return_string )\n{\n Vector3d ypr;\n ypr = R2ypr( M.topLeftCorner<3,3>() );\n\n char __tmp[200];\n snprintf( __tmp, 200, \":YPR(deg)=(%4.2f,%4.2f,%4.2f) :TxTyTz=(%4.2f,%4.2f,%4.2f)\", ypr(0), ypr(1), ypr(2), M(0,3), M(1,3), M(2,3) );\n return_string = string( __tmp );\n}\n\n\nstring PoseManipUtils::prettyprintMatrix4d( const Matrix4d& M )\n{\n Vector3d ypr;\n ypr = R2ypr( M.topLeftCorner<3,3>() );\n\n char __tmp[200];\n snprintf( __tmp, 200, \":YPR(deg)=(%4.3f,%4.3f,%4.3f) :TxTyTz=(%4.3f,%4.3f,%4.3f)\", ypr(0), ypr(1), ypr(2), M(0,3), M(1,3), M(2,3) );\n string return_string = string( __tmp );\n return return_string;\n}\n\nstring PoseManipUtils::prettyprintMatrix4d_YPR( const Matrix4d& M )\n{\n Vector3d ypr;\n ypr = R2ypr( M.topLeftCorner<3,3>() );\n\n char __tmp[200];\n snprintf( __tmp, 200, \" YPR(deg)=(%4.2f,%4.2f,%4.2f) \", ypr(0), ypr(1), ypr(2) );\n string return_string = string( __tmp );\n return return_string;\n}\n\nstring PoseManipUtils::prettyprintMatrix4d_t( const Matrix4d& M )\n{\n Vector3d ypr;\n ypr = R2ypr( M.topLeftCorner<3,3>() );\n\n char __tmp[200];\n snprintf( __tmp, 200, \" TxTyTz=(%4.2f,%4.2f,%4.2f) \", M(0,3), M(1,3), M(2,3) );\n string return_string = string( __tmp );\n return return_string;\n}\n\n\nvoid PoseManipUtils::vec_to_cross_matrix( const Vector3d& t, Matrix3d& A_x )\n{\n // Tx = Matrix3d::Zero();\n A_x << 0, -t(2) , t(1) ,\n t(2), 0, -t(0),\n -t(1), t(0), 0;\n}\n\nvoid PoseManipUtils::vec_to_cross_matrix( double a, double b, double c, Matrix3d& A_x )\n{\n Vector3d t(a,b,c);\n vec_to_cross_matrix( t, A_x );\n}\n" }, { "path": "src/utils/PoseManipUtils.h", "content": "#pragma once\n//\n// This provides utilities for pose manipulation.\n//\n// Author : Manohar Kuse \n//\n\n#include \n#include \n\n\n#include \n#include \n\n\n// uncomment this to compile without ros dependency for this file\n#define __PoseManipUtils__with_ROS 1\n\n#ifdef __PoseManipUtils__with_ROS\n#include \n#endif\n\nusing namespace std;\nusing namespace Eigen;\n\nclass PoseManipUtils\n{\npublic:\n static void raw_to_eigenmat( const double * quat, const double * t, Matrix4d& dstT );\n static void eigenmat_to_raw( const Matrix4d& T, double * quat, double * t);\n static void rawyprt_to_eigenmat( const double * ypr, const double * t, Matrix4d& dstT );\n static void rawyprt_to_eigenmat( const Vector3d& eigen_ypr_degrees, const Vector3d& t, Matrix4d& dstT );\n\n static void eigenmat_to_rawyprt( const Matrix4d& T, double * ypr, double * t);\n static void eigenmat_to_rawyprt( const Matrix4d& T, Vector3d& ypr, Vector3d& t); \n\n static Vector3d R2ypr( const Matrix3d& R);\n static Matrix3d ypr2R( const Vector3d& ypr); // input ypr must be in degrees.\n static void prettyprintPoseMatrix( const Matrix4d& M );\n static void prettyprintPoseMatrix( const Matrix4d& M, string& return_string );\n\n static string prettyprintMatrix4d( const Matrix4d& M );\n static string prettyprintMatrix4d_YPR( const Matrix4d& M );\n static string prettyprintMatrix4d_t( const Matrix4d& M );\n\n\n static void raw_xyzw_to_eigenmat( const double * quat, const double * t, Matrix4d& dstT );\n static void raw_xyzw_to_eigenmat( const Vector4d& quat, const Vector3d& t, Matrix4d& dstT );\n\n static void eigenmat_to_raw_xyzw( const Matrix4d& T, double * quat, double * t);\n\n #ifdef __PoseManipUtils__with_ROS\n static void geometry_msgs_Pose_to_eigenmat( const geometry_msgs::Pose& pose, Matrix4d& dstT );\n static void eigenmat_to_geometry_msgs_Pose( const Matrix4d& T, geometry_msgs::Pose& pose );\n #endif\n\n // Given a point convert it to cross-product matrix. A_x = [ [ 0, -c, -b ], [c,0,-a], [-b,-a,0] ]\n static void vec_to_cross_matrix( const Vector3d& a, Matrix3d& A_x );\n static void vec_to_cross_matrix( double a, double b, double c, Matrix3d& A_x );\n\n};\n" }, { "path": "src/utils/RawFileIO.cpp", "content": "#include \"RawFileIO.h\"\n\n\nvoid RawFileIO::write_image( string fname, const cv::Mat& img)\n{\n __RawFileIO__write_image_debug_dm( std::cout << \"write_image: \"<< fname << endl );\n cv::imwrite( (fname).c_str(), img );\n}\n\n\nvoid RawFileIO::write_string( string fname, const string& my_string)\n{\n __RawFileIO__write_image_debug_dm( std::cout << \"write_string: \"<< fname << endl );\n std::ofstream outfile( fname );\n outfile << my_string << endl;\n outfile.close();\n}\n\n// templated static function canot only exist in header files.\n// so this was moved to the header file\n// template \n// void RawFileIO::write_EigenMatrix(const string& filename, const MatrixBase& a)\n// {\n// // string base = string(\"/home/mpkuse/Desktop/bundle_adj/dump/datamgr_mateigen_\");\n// std::ofstream file(filename);\n// if( file.is_open() )\n// {\n// // file << a.format(CSVFormat) << endl;\n// file << a << endl;\n// write_image_debug_dm(std::cout << \"\\033[1;32m\" <<\"Written to file: \"<< filename << \"\\033[0m\\n\" );\n// }\n// else\n// {\n// cout << \"\\033[1;31m\" << \"FAIL TO OPEN FILE for writing: \"<< filename << \"\\033[0m\\n\";\n//\n// }\n// }\n\n\nvoid RawFileIO::write_Matrix2d( const string& filename, const double * D, int nRows, int nCols )\n{\n // string base = string(\"/home/mpkuse/Desktop/bundle_adj/dump/datamgr_mat2d_\");\n std::ofstream file(filename);\n if( file.is_open() )\n {\n int c = 0 ;\n for( int i=0; i> buff[cols*rows+temp_cols++];\n\n if (temp_cols == 0)\n continue;\n\n if (cols == 0)\n cols = temp_cols;\n\n rows++;\n }\n\n infile.close();\n\n rows--;\n\n // Populate matrix with numbers.\n result = MatrixXd::Zero(rows,cols);\n for (int i = 0; i < rows; i++)\n for (int j = 0; j < cols; j++)\n result(i,j) = buff[ cols*i+j ];\n\n // return result;\n __RawFileIO__write_image_debug_dm( cout << \"\\tshape=\" << result.rows() << \"x\" << result.cols() << endl; )\n\n return true;\n };\n\n\nbool RawFileIO::read_eigen_matrix( string filename, Matrix4d& result )\n {\n int cols = 0, rows = 0;\n const int MAXBUFSIZE = ((int) 2000);\n double buff[MAXBUFSIZE];\n\n // Read numbers from file into buffer.\n __RawFileIO__write_image_debug_dm( std::cout << \"\\033[1;32m\" << \"read_eigen_matrix: \"<< filename << \"\\033[0m\" );\n ifstream infile;\n infile.open(filename);\n if( !infile ) {\n cout << \"\\n\\033[1;31m\" << \"failed to open file \" << filename << \"\\033[0m\" << endl;\n return false;\n }\n while (! infile.eof())\n {\n string line;\n getline(infile, line);\n\n int temp_cols = 0;\n stringstream stream(line);\n while(! stream.eof())\n stream >> buff[cols*rows+temp_cols++];\n\n if (temp_cols == 0)\n continue;\n\n if (cols == 0)\n cols = temp_cols;\n\n rows++;\n }\n\n infile.close();\n\n rows--;\n\n assert( rows==4 && cols==4 && \"\\n[RawFileIO::read_eigen_matrix( string filename, Matrix4d& result )] The input file is not 4x4\" );\n\n // Populate matrix with numbers.\n result = Matrix4d::Zero();\n for (int i = 0; i < rows; i++)\n for (int j = 0; j < cols; j++)\n result(i,j) = buff[ cols*i+j ];\n\n // return result;\n __RawFileIO__write_image_debug_dm( cout << \"\\tshape=\" << result.rows() << \"x\" << result.cols() << endl; )\n return true;\n };\n\n\n\nbool RawFileIO::read_eigen_matrix( string filename, Matrix3d& result )\n{\n int cols = 0, rows = 0;\n const int MAXBUFSIZE = ((int) 2000);\n double buff[MAXBUFSIZE];\n\n // Read numbers from file into buffer.\n __RawFileIO__write_image_debug_dm( std::cout << \"\\033[1;32m\" << \"read_eigen_matrix: \"<< filename << \"\\033[0m\" );\n ifstream infile;\n infile.open(filename);\n if( !infile ) {\n cout << \"\\n\\033[1;31m\" << \"failed to open file \" << filename << \"\\033[0m\" << endl;\n return false;\n }\n while (! infile.eof())\n {\n string line;\n getline(infile, line);\n\n int temp_cols = 0;\n stringstream stream(line);\n while(! stream.eof())\n stream >> buff[cols*rows+temp_cols++];\n\n if (temp_cols == 0)\n continue;\n\n if (cols == 0)\n cols = temp_cols;\n\n rows++;\n }\n\n infile.close();\n\n rows--;\n\n assert( rows==3 && cols==3 && \"\\n[RawFileIO::read_eigen_matrix( string filename, Matrix3d& result )] The input file is not 3x3\" );\n\n // Populate matrix with numbers.\n result = Matrix3d::Zero();\n for (int i = 0; i < rows; i++)\n for (int j = 0; j < cols; j++)\n result(i,j) = buff[ cols*i+j ];\n\n // return result;\n __RawFileIO__write_image_debug_dm( cout << \"\\tshape=\" << result.rows() << \"x\" << result.cols() << endl; )\n return true;\n}\n\n\nbool RawFileIO::read_eigen_matrix( string filename, VectorXi& result )\n{\n int cols = 0, rows = 0;\n const int MAXBUFSIZE = ((int) 2000);\n double buff[MAXBUFSIZE];\n\n // Read numbers from file into buffer.\n __RawFileIO__write_image_debug_dm( std::cout << \"\\033[1;32m\" << \"read_eigen_matrix: \"<< filename << \"\\033[0m\" );\n ifstream infile;\n infile.open(filename);\n if( !infile ) {\n cout << \"\\n\\033[1;31m\" << \"failed to open file \" << filename << \"\\033[0m\" << endl;\n return false;\n }\n while (! infile.eof())\n {\n string line;\n getline(infile, line);\n\n int temp_cols = 0;\n stringstream stream(line);\n while(! stream.eof())\n stream >> buff[cols*rows+temp_cols++];\n\n if (temp_cols == 0)\n continue;\n\n if (cols == 0)\n cols = temp_cols;\n\n rows++;\n }\n\n infile.close();\n\n rows--;\n\n assert( cols==1 && \"\\n[RawFileIO::read_eigen_matrix( string filename, VectorXi& result )] The input file is not row-vector\" );\n\n // Populate matrix with numbers.\n result = VectorXi::Zero(rows);\n for (int i = 0; i < rows; i++)\n for (int j = 0; j < cols; j++)\n result(i,j) = (int) buff[ cols*i+j ];\n\n // return result;\n __RawFileIO__write_image_debug_dm( cout << \"\\tshape=\" << result.rows() << \"x\" << result.cols() << endl; )\n return true;\n};\n\n\nbool RawFileIO::read_eigen_matrix( const std::vector& ary, Matrix4d& result )\n{\n assert( ary.size() == result.rows() * result.cols() && \"[RawFileIO::read_eigen_matrix] size of vector need to be equal to the size of Eigen::Matrix\");\n\n for( int i=0 ; i 0 && ncols > 0 )\n output = MatrixXd::Zero(nrows, ncols );\n else {\n cout << \"[RawFileIO::read_eigen_matrix_fromjson] ERROR, nrows and cols should be positive\\n\";\n return false;\n }\n\n\n // split( data, '\\n')\n vector all_rows = MiscUtils::split( data, '\\n' );\n if( nrows != all_rows.size() )\n {\n cout << \"[RawFileIO::read_eigen_matrix_fromjson] ERROR, requested \" << nrows << \" but actually are \" << all_rows.size() << endl;\n return false;\n }\n for( int r=0 ; r all_cols_for_this_row = MiscUtils::split( all_rows[r], ',' );\n if( ncols != all_cols_for_this_row.size() )\n {\n cout << \"[RawFileIO::read_eigen_matrix_fromjson] ERROR, requested \" << ncols << \" but actually are \" << all_cols_for_this_row.size() << \" for row=\" << r << endl;\n return false;\n }\n\n for( int c=0 ; c all_rows = MiscUtils::split( data, '\\n' );\n if( nrows != all_rows.size() )\n {\n cout << \"[RawFileIO::read_eigen_matrix4d_fromjson] ERROR, requested \" << nrows << \" but actually are \" << all_rows.size() << endl;\n return false;\n }\n for( int r=0 ; r all_cols_for_this_row = MiscUtils::split( all_rows[r], ',' );\n if( ncols != all_cols_for_this_row.size() )\n {\n cout << \"[RawFileIO::read_eigen_matrix4d_fromjson] ERROR, requested \" << ncols << \" but actually are \" << all_cols_for_this_row.size() << \" for row=\" << r << endl;\n return false;\n }\n\n for( int c=0 ; c 0 ) {\n output = VectorXd::Zero( nrows );\n }\n else {\n cout << \"[RawFileIO::read_eigen_vector_fromjson] ERROR, nrows should be positive\\n\";\n return false;\n }\n\n if( ncols != 1 )\n {\n cout << \"[RawFileIO::read_eigen_vector_fromjson] ERROR, you request to convert to VectorXd however the input json cols != 1\\n\";\n return false;\n }\n\n vector all_rows = MiscUtils::split( data, '\\n' );\n if( nrows != all_rows.size() )\n {\n cout << \"[RawFileIO::read_eigen_vector_fromjson] ERROR, requested \" << nrows << \" but actually are \" << all_rows.size() << endl;\n return false;\n }\n\n for( int r=0 ; r all_cols_for_this_row = MiscUtils::split( all_rows[r], ',' );\n if( all_cols_for_this_row.size() != 1 )\n {\n cout << \"[RawFileIO::read_eigen_vector_fromjson] ERROR, requested \" << ncols << \" but actually are \" << all_cols_for_this_row.size() << \" for row=\" << r << endl;\n return false;\n }\n\n output( r ) = std::stod( all_cols_for_this_row[0] );\n }\n return true;\n\n\n}\n#endif //WITH_NLOHMANN_JSON\n" }, { "path": "src/utils/RawFileIO.h", "content": "#pragma once\n\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n//opencv\n#include \n#include \n#include \n#include \n\n\n#include \n#include \n#include \nusing namespace Eigen;\n#include \n\nusing namespace std;\n\n#define WITH_NLOHMANN_JSON\n#ifdef WITH_NLOHMANN_JSON\n#include \"nlohmann/json.hpp\"\nusing json = nlohmann::json;\n\n#include \"MiscUtils.h\"\n#endif\n\n\n#include // for is_directory\n\n#define __RawFileIO__write_image_debug_dm( msg ) msg;\n\nclass RawFileIO\n{\npublic:\n static void write_image( string fname, const cv::Mat& img);\n static void write_string( string fname, const string& my_string);\n\n // templated static function canot only exist in header files.\n template \n static void write_EigenMatrix(const string& filename, const MatrixBase& a)\n {\n // string base = string(\"/home/mpkuse/Desktop/bundle_adj/dump/datamgr_mateigen_\");\n std::ofstream file(filename);\n if( file.is_open() )\n {\n // file << a.format(CSVFormat) << endl;\n file << a << endl;\n __RawFileIO__write_image_debug_dm(std::cout << \"\\033[1;32m\" <<\"write_EigenMatrix: \"<< filename << \" size=\" << a.rows() << \"x\" << a.cols() << \"\\033[0m\\n\";)\n }\n else\n {\n cout << \"\\033[1;31m\" << \"FAIL TO OPEN FILE for writing: \"<< filename << \"\\033[0m\\n\";\n\n }\n }\n\n\n static void write_Matrix2d( const string& filename, const double * D, int nRows, int nCols );\n static void write_Matrix1d( const string& filename, const double * D, int n );\n\n static bool read_eigen_matrix( string filename, MatrixXd& result );\n static bool read_eigen_matrix( string filename, Matrix4d& result );\n static bool read_eigen_matrix( string filename, Matrix3d& result );\n static bool read_eigen_matrix( string filename, VectorXi& result );\n\n ///< read the flat vector ary as a rowmajor matrix.\n /// [ 1, 2, 3, 4,5,6...,16 ] ==> [ [1,2,3,4], [5,6,7,8], [9,10,11,12], [13,14,15,16] ]\n /// TODO: Have a flag to read interpret the 1d array as a colmajor.\n static bool read_eigen_matrix( const std::vector& ary, Matrix4d& result );\n\n #ifdef WITH_NLOHMANN_JSON\n // The input json need to be something like:\n // A new row is denoted with \\n, and a comma separates elements.\n //{\n // \"cols\": 4,\n // \"rows\": 4,\n // \"data\": \"0.2857131543876468, -0.2530077727001951, 0.9243132912401226, -0.02953755668229465\\n-0.9566719337894203, -0.01884313243445668, 0.2905576491157474, 0.2114882406102183,\\n-0.05609638588601445, -0.9672807262182707, -0.2474291659792429, 0.04534835279466286\\n0, 0, 0, 1\"\n // }\n static bool read_eigen_matrix_fromjson( const json str, MatrixXd& output );\n static bool read_eigen_matrix4d_fromjson( const json str, Matrix4d& output );\n static bool read_eigen_vector_fromjson( const json str, VectorXd& output );\n #endif\n\n\n static bool if_file_exist( char * fname );\n static bool if_file_exist( string fname );\n\n static bool is_path_a_directory(const char* path);\n static bool is_path_a_directory(const string path);\n\n\n static int exec_cmd( const string& cmd ); //< Executes a unix command.\n\n};\n" }, { "path": "src/utils/RosMarkerUtils.cpp", "content": "#include \"RosMarkerUtils.h\"\n\n// cam_size = 1: means basic size. 1.5 will make it 50% bigger.\nvoid RosMarkerUtils::init_camera_marker( visualization_msgs::Marker& marker, float cam_size )\n{\n marker.header.frame_id = \"world\";\n marker.header.stamp = ros::Time::now();\n marker.action = visualization_msgs::Marker::ADD;\n marker.color.a = .7; // Don't forget to set the alpha!\n marker.type = visualization_msgs::Marker::LINE_LIST;\n // marker.id = i;\n // marker.ns = \"camerapose_visual\";\n\n marker.scale.x = 0.003; //width of line-segments\n float __vcam_width = 0.07*cam_size;\n float __vcam_height = 0.04*cam_size;\n float __z = 0.1*cam_size;\n\n\n\n\n marker.points.clear();\n geometry_msgs::Point pt;\n pt.x = 0; pt.y=0; pt.z=0;\n marker.points.push_back( pt );\n pt.x = __vcam_width; pt.y=__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = 0; pt.y=0; pt.z=0;\n marker.points.push_back( pt );\n pt.x = -__vcam_width; pt.y=__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = 0; pt.y=0; pt.z=0;\n marker.points.push_back( pt );\n pt.x = __vcam_width; pt.y=-__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = 0; pt.y=0; pt.z=0;\n marker.points.push_back( pt );\n pt.x = -__vcam_width; pt.y=-__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n\n pt.x = __vcam_width; pt.y=__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = -__vcam_width; pt.y=__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = -__vcam_width; pt.y=__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = -__vcam_width; pt.y=-__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = -__vcam_width; pt.y=-__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = __vcam_width; pt.y=-__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = __vcam_width; pt.y=-__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n pt.x = __vcam_width; pt.y=__vcam_height; pt.z=__z;\n marker.points.push_back( pt );\n\n\n // TOSET\n marker.pose.position.x = 0.;\n marker.pose.position.y = 0.;\n marker.pose.position.z = 0.;\n marker.pose.orientation.x = 0.;\n marker.pose.orientation.y = 0.;\n marker.pose.orientation.z = 0.;\n marker.pose.orientation.w = 1.;\n // marker.id = i;\n // marker.ns = \"camerapose_visual\";\n marker.color.r = 0.2;marker.color.b = 0.;marker.color.g = 0.;\n}\n\nvoid RosMarkerUtils::setpose_to_marker( const Matrix4d& w_T_c, visualization_msgs::Marker& marker )\n{\n Quaterniond quat( w_T_c.topLeftCorner<3,3>() );\n marker.pose.position.x = w_T_c(0,3);\n marker.pose.position.y = w_T_c(1,3);\n marker.pose.position.z = w_T_c(2,3);\n marker.pose.orientation.x = quat.x();\n marker.pose.orientation.y = quat.y();\n marker.pose.orientation.z = quat.z();\n marker.pose.orientation.w = quat.w();\n}\n\nvoid RosMarkerUtils::setposition_to_marker( const Vector3d& w_t_c, visualization_msgs::Marker& marker )\n{\n marker.pose.position.x = w_t_c(0);\n marker.pose.position.y = w_t_c(1);\n marker.pose.position.z = w_t_c(2);\n}\n\nvoid RosMarkerUtils::setposition_to_marker( const Vector4d& w_t_c, visualization_msgs::Marker& marker )\n{\n marker.pose.position.x = w_t_c(0) / w_t_c(3); ;\n marker.pose.position.y = w_t_c(1) / w_t_c(3); ;\n marker.pose.position.z = w_t_c(2) / w_t_c(3); ;\n}\n\nvoid RosMarkerUtils::setposition_to_marker( float x, float y, float z, visualization_msgs::Marker& marker )\n{\n marker.pose.position.x = x;\n marker.pose.position.y = y;\n marker.pose.position.z = z;\n}\n\nvoid RosMarkerUtils::setcolor_to_marker( float r, float g, float b, visualization_msgs::Marker& marker )\n{\n assert( r>=0. && r<=1.0 && g>=0. && g<=1.0 && b>=0 && b<=1.0 );\n marker.color.a = 1.0;\n marker.color.r = r;\n marker.color.g = g;\n marker.color.b = b;\n}\n\nvoid RosMarkerUtils::setcolor_to_marker( float r, float g, float b, float a, visualization_msgs::Marker& marker )\n{\n assert( r>=0. && r<=1.0 && g>=0. && g<=1.0 && b>=0 && b<=1.0 && a>0. && a<=1.0);\n marker.color.a = a;\n marker.color.r = r;\n marker.color.g = g;\n marker.color.b = b;\n}\n\nvoid RosMarkerUtils::init_text_marker( visualization_msgs::Marker &marker )\n{\n marker.header.frame_id = \"world\";\n marker.header.stamp = ros::Time::now();\n marker.action = visualization_msgs::Marker::ADD;\n marker.color.a = .8; // Don't forget to set the alpha!\n marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;\n\n marker.scale.x = 1.; //not in use\n marker.scale.y = 1.; //not in use\n marker.scale.z = 1.;\n\n //// Done . no need to edit firther\n marker.pose.position.x = 0.;\n marker.pose.position.y = 0.;\n marker.pose.position.z = 0.;\n marker.pose.orientation.x = 0.;\n marker.pose.orientation.y = 0.;\n marker.pose.orientation.z = 0.;\n marker.pose.orientation.w = 1.;\n // marker.id = i;\n // marker.ns = \"camerapose_visual\";\n marker.color.r = 0.2;marker.color.b = 0.;marker.color.g = 0.;\n\n}\n\nvoid RosMarkerUtils::init_line_strip_marker( visualization_msgs::Marker &marker )\n{\n marker.header.frame_id = \"world\";\n marker.header.stamp = ros::Time::now();\n marker.action = visualization_msgs::Marker::ADD;\n marker.color.a = .8; // Don't forget to set the alpha!\n marker.type = visualization_msgs::Marker::LINE_STRIP;\n\n marker.scale.x = 0.02;\n\n marker.points.clear();\n\n //// Done . no need to edit firther\n marker.pose.position.x = 0.;\n marker.pose.position.y = 0.;\n marker.pose.position.z = 0.;\n marker.pose.orientation.x = 0.;\n marker.pose.orientation.y = 0.;\n marker.pose.orientation.z = 0.;\n marker.pose.orientation.w = 1.;\n // marker.id = i;\n // marker.ns = \"camerapose_visual\";\n marker.color.r = 0.2;marker.color.b = 0.;marker.color.g = 0.;\n\n}\n\nvoid RosMarkerUtils::init_line_marker( visualization_msgs::Marker &marker )\n{\n marker.header.frame_id = \"world\";\n marker.header.stamp = ros::Time::now();\n marker.action = visualization_msgs::Marker::ADD;\n marker.color.a = .8; // Don't forget to set the alpha!\n marker.type = visualization_msgs::Marker::LINE_LIST;\n\n marker.scale.x = 0.02;\n\n marker.points.clear();\n\n //// Done . no need to edit firther\n marker.pose.position.x = 0.;\n marker.pose.position.y = 0.;\n marker.pose.position.z = 0.;\n marker.pose.orientation.x = 0.;\n marker.pose.orientation.y = 0.;\n marker.pose.orientation.z = 0.;\n marker.pose.orientation.w = 1.;\n // marker.id = i;\n // marker.ns = \"camerapose_visual\";\n marker.color.r = 0.2;marker.color.b = 0.;marker.color.g = 0.;\n\n}\n\nvoid RosMarkerUtils::init_line_marker( visualization_msgs::Marker &marker, const Vector3d& p1, const Vector3d& p2 )\n{\n marker.header.frame_id = \"world\";\n marker.header.stamp = ros::Time::now();\n marker.action = visualization_msgs::Marker::ADD;\n marker.color.a = .8; // Don't forget to set the alpha!\n marker.type = visualization_msgs::Marker::LINE_LIST;\n\n marker.scale.x = 0.02;\n\n marker.points.clear();\n geometry_msgs::Point pt;\n pt.x = p1(0);\n pt.y = p1(1);\n pt.z = p1(2);\n marker.points.push_back( pt );\n pt.x = p2(0);\n pt.y = p2(1);\n pt.z = p2(2);\n marker.points.push_back( pt );\n\n //// Done . no need to edit firther\n marker.pose.position.x = 0.;\n marker.pose.position.y = 0.;\n marker.pose.position.z = 0.;\n marker.pose.orientation.x = 0.;\n marker.pose.orientation.y = 0.;\n marker.pose.orientation.z = 0.;\n marker.pose.orientation.w = 1.;\n // marker.id = i;\n // marker.ns = \"camerapose_visual\";\n marker.color.r = 0.2;marker.color.b = 0.;marker.color.g = 0.;\n\n}\n\n\nvoid RosMarkerUtils::init_points_marker( visualization_msgs::Marker &marker )\n{\n marker.header.frame_id = \"world\";\n marker.header.stamp = ros::Time::now();\n marker.action = visualization_msgs::Marker::ADD;\n marker.color.a = .8; // Don't forget to set the alpha!\n marker.type = visualization_msgs::Marker::POINTS;\n\n marker.scale.x = 0.04;\n marker.scale.y = 0.04;\n\n marker.points.clear();\n\n //// Done . no need to edit firther\n marker.pose.position.x = 0.;\n marker.pose.position.y = 0.;\n marker.pose.position.z = 0.;\n marker.pose.orientation.x = 0.;\n marker.pose.orientation.y = 0.;\n marker.pose.orientation.z = 0.;\n marker.pose.orientation.w = 1.;\n // marker.id = i;\n // marker.ns = \"camerapose_visual\";\n marker.color.r = 0.2;marker.color.b = 0.;marker.color.g = 0.;\n\n}\n\n\n\n\nvoid RosMarkerUtils::add_point_to_marker( float x, float y, float z, visualization_msgs::Marker& marker, bool clear_prev_points )\n{\n if( clear_prev_points )\n marker.points.clear();\n\n geometry_msgs::Point pt;\n pt.x = x; pt.y = y; pt.z = z;\n marker.points.push_back( pt );\n}\n\nvoid RosMarkerUtils::add_point_to_marker( const Vector3d& X, visualization_msgs::Marker& marker, bool clear_prev_points )\n{\n if( clear_prev_points )\n marker.points.clear();\n\n geometry_msgs::Point pt;\n pt.x = X(0); pt.y = X(1); pt.z = X(2);\n marker.points.push_back( pt );\n}\n\nvoid RosMarkerUtils::add_point_to_marker( const Vector4d& X, visualization_msgs::Marker& marker, bool clear_prev_points )\n{\n if( clear_prev_points )\n marker.points.clear();\n\n geometry_msgs::Point pt;\n assert( abs(X(3)) > 1E-5 );\n pt.x = X(0)/X(3); pt.y = X(1)/X(3); pt.z = X(2)/X(3);\n marker.points.push_back( pt );\n}\n\nvoid RosMarkerUtils::add_points_to_marker( const MatrixXd& X, visualization_msgs::Marker& marker, bool clear_prev_points ) //X : 3xN or 4xN.\n{\n assert( (X.rows() == 3 || X.rows() == 4) && \"[RosMarkerUtils::add_points_to_marker] X need to of size 3xN or 4xN\\n\" );\n geometry_msgs::Point pt;\n\n if( clear_prev_points )\n marker.points.clear();\n\n\n for( int i=0 ; i\n// Notes: http://wiki.ros.org/rviz/DisplayTypes/Marker\n\n#include \n#include \n\n\n#include \n#include \n\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n\nusing namespace std;\nusing namespace Eigen;\n\n\nclass RosMarkerUtils\n{\npublic:\n ////////////////// INIT /////////////////////////\n\n\n /// Initialize a camera with a few lines. You need to set the `ns` and `id` before publishing.\n static void init_camera_marker( visualization_msgs::Marker& marker, float cam_size );\n static void init_text_marker( visualization_msgs::Marker &marker );\n static void init_line_strip_marker( visualization_msgs::Marker &marker );\n static void init_line_marker( visualization_msgs::Marker &marker );\n static void init_line_marker( visualization_msgs::Marker &marker, const Vector3d& p1, const Vector3d& p2 );\n static void init_points_marker( visualization_msgs::Marker &marker );\n\n //////////////// SET //////////////////////////\n static void setpose_to_marker( const Matrix4d& w_T_c, visualization_msgs::Marker& marker );\n static void setposition_to_marker( const Vector3d& w_t_c, visualization_msgs::Marker& marker );\n static void setposition_to_marker( const Vector4d& w_t_c, visualization_msgs::Marker& marker );\n static void setposition_to_marker( float x, float y, float z, visualization_msgs::Marker& marker );\n static void setcolor_to_marker( float r, float g, float b, visualization_msgs::Marker& marker );\n static void setcolor_to_marker( float r, float g, float b, float a, visualization_msgs::Marker& marker );\n\n //////////////// Add Points ////////////////////\n static void add_point_to_marker( float x, float y, float z, visualization_msgs::Marker& marker, bool clear_prev_points=true );\n static void add_point_to_marker( const Vector3d& X, visualization_msgs::Marker& marker, bool clear_prev_points=true );\n static void add_point_to_marker( const Vector4d& X, visualization_msgs::Marker& marker, bool clear_prev_points=true );\n static void add_points_to_marker( const MatrixXd& X, visualization_msgs::Marker& marker, bool clear_prev_points=true ); //X : 3xN or 4xN.\n\n};\n" }, { "path": "src/utils/SafeQueue.h", "content": "#pragma once\n\n#include \n#include \n#include \n\n// A threadsafe-queue. - https://stackoverflow.com/questions/15278343/c11-thread-safe-queue\ntemplate \nclass SafeQueue\n{\npublic:\n SafeQueue(void)\n : q()\n , m()\n , c()\n {}\n\n ~SafeQueue(void)\n {}\n\n // Add an element to the queue.\n void enqueue(T t)\n {\n std::lock_guard lock(m);\n q.push(t);\n c.notify_one();\n }\n\n\n\n // Get the \"front\"-element.\n // If the queue is empty, wait till a element is avaiable.\n T dequeue(void)\n {\n std::unique_lock lock(m);\n while(q.empty())\n {\n // release lock as long as the wait and reaquire it afterwards.\n c.wait(lock);\n }\n T val = q.front();\n q.pop();\n return val;\n }\n\n ////// added my mpkuse\n\n // return size\n int size()\n {\n std::lock_guard lock(m);\n int n = q.size();\n c.notify_one();\n return n;\n }\n\n // Add an element to the queue.\n void push(T t)\n {\n std::lock_guard lock(m);\n q.push(t);\n c.notify_one();\n }\n\n // Get the \"front\"-element.\n // If the queue is empty, wait till a element is avaiable.\n T pop(void)\n {\n std::unique_lock lock(m);\n while(q.empty())\n {\n // release lock as long as the wait and reaquire it afterwards.\n c.wait(lock);\n }\n T val = q.front();\n q.pop();\n return val;\n }\n\n // most recently added element\n T back()\n {\n assert( size() > 0 && \"SafeQueue: You tried to SafeQueue::front(), but the queue was empty. This operation is only allowed when there are elements in the queue. \");\n std::lock_guard lock(m);\n T val = q.back();\n c.notify_one();\n return val;\n }\n\n // 1st element in queue\n T front()\n {\n assert( size() > 0 && \"SafeQueue: You tried to SafeQueue::front(), but the queue was empty. This operation is only allowed when there are elements in the queue. \");\n std::lock_guard lock(m);\n T val = q.front();\n c.notify_one();\n return val;\n }\n\nprivate:\n std::queue q;\n mutable std::mutex m;\n std::condition_variable c;\n};\n" }, { "path": "src/utils/TermColor.h", "content": "#pragma once\n\n/**\n\n// TODO Work in progress\n// https://stackoverflow.com/questions/2616906/how-do-i-output-coloured-text-to-a-linux-terminal\n\nblack 30 40\nred 31 41\ngreen 32 42\nyellow 33 43\nblue 34 44\nmagenta 35 45\ncyan 36 46\nwhite 37 47\n\nreset 0 (everything back to normal)\nbold/bright 1 (often a brighter shade of the same colour)\nunderline 4\ninverse 7 (swap foreground and background colours)\nbold/bright off 21\nunderline off 24\ninverse off 27\n\neg:\ncout << \"\\033[1;31mbold red text\\033[0m\\n\";\n\n\n\n*/\n#include \n#include \n#include \n\nclass TermColor\n{\npublic:\n static std::string RED() { return compose( TermColor::BG_RED ); }\n static std::string GREEN() { return compose( TermColor::BG_GREEN ); }\n static std::string YELLOW() { return compose( TermColor::BG_YELLOW ); }\n static std::string BLUE() { return compose( TermColor::BG_BLUE ); }\n static std::string MAGENTA() { return compose( TermColor::BG_MAGENTA ); }\n static std::string CYAN() { return compose( TermColor::BG_CYAN ); }\n static std::string WHITE() { return compose( TermColor::BG_WHITE ); }\n\n static std::string iRED() { return compose( TermColor::BG_RED, true ); }\n static std::string iGREEN() { return compose( TermColor::BG_GREEN, true ); }\n static std::string iYELLOW() { return compose( TermColor::BG_YELLOW, true ); }\n static std::string iBLUE() { return compose( TermColor::BG_BLUE, true ); }\n static std::string iMAGENTA() { return compose( TermColor::BG_MAGENTA, true ); }\n static std::string iCYAN() { return compose( TermColor::BG_CYAN, true ); }\n static std::string iWHITE() { return compose( TermColor::BG_WHITE, true ); }\n\n static std::string RESET()\n {\n std::stringstream buffer;\n buffer << \"\\033[\" << CTRL_RESET << \"m\";\n return buffer.str();\n }\n\n // modifier : bold, underline etc.\n // color : FG_RED, ..., BG_RED etc\n static std::string compose( const int modifier, const int color, bool invert=false )\n {\n\n std::stringstream buffer;\n if( !invert )\n buffer << \"\\033[\" << modifier << \";\" << color << \"m\";\n else\n buffer << \"\\033[\" << CTRL_INVERSE << \";\" << modifier << \";\" << color << \"m\";\n return buffer.str();\n }\n\n static std::string compose( const int color, bool invert=false )\n {\n std::stringstream buffer;\n if( !invert )\n buffer << \"\\033[\" << color << \"m\";\n else\n buffer << \"\\033[\" << CTRL_INVERSE << \";\" << color << \"m\";\n return buffer.str();\n }\n\n static const int BG_BLACK = 30;\n static const int BG_RED = 31;\n static const int BG_GREEN = 32;\n static const int BG_YELLOW = 33;\n static const int BG_BLUE = 34;\n static const int BG_MAGENTA = 35;\n static const int BG_CYAN = 36;\n static const int BG_WHITE = 37;\n\n static const int FG_BLACK = 40;\n static const int FG_RED = 41;\n static const int FG_GREEN = 42;\n static const int FG_YELLOW = 43;\n static const int FG_BLUE = 44;\n static const int FG_MAGENTA = 45;\n static const int FG_CYAN = 46;\n static const int FG_WHITE = 47;\n\n static const int CTRL_RESET = 0;\n static const int CTRL_BOLD = 1;\n static const int CTRL_UNDERLINE = 4;\n static const int CTRL_INVERSE = 7; //(swap foreground and background colours)\n static const int CTRL_BOLD_OFF = 21;\n static const int CTRL_UNDERLINE_OFF = 24;\n static const int CTRL_INVERSE_OFF = 27; //(swap foreground and background colours)\n};\n" }, { "path": "src/utils/camera_geometry_usage.cpp", "content": "// Sample usage for class CameraGeometry.h/MonoGeometry and CameraGeometry.h/StereoGeometry\n// These classes abstractout the Stereo Geometry, Undistort etc. They can be used\n// with any of the Camodocal cameras ie. Mei, Kannala-brandt, pinhole (ofcourse).\n// The whole idea of the abstract Camera clases and Geometry class is to make the\n// codebase truely object oriented and the core geometry abstracted. Hopefully\n// all this will help to develop more higher level applications faster.\n\n// YONGYEN'S METHOD TO CORRECT THE STEREO-EXTRINSIC WHOLLY CONTAINED IN THIS FILE\n// implements Yonggen Ling's method\n// Y. Ling and S. Shen, \"High-precision online markerless stereo extrinsic calibration,\" 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, 2016, pp. 1771-1778.\n\n// minimize_{R,t} \\sum_i || (f'_i)^T E f_i || , where R,t == right_T_left.\n// a) E is the Essential matrix E := [t]_x R\n// b) f and f' are point feature matches (f from left f from right) in normalized image co-ordinates\n\n#include \n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n//opencv\n#include \n#include \n#include \n#include \n\n#include \"../utils/MiscUtils.h\"\n#include \"../utils/ElapsedTime.h\"\n#include \"../utils/PoseManipUtils.h\"\n#include \"../utils/TermColor.h\"\n#include \"../utils/CameraGeometry.h\"\n#include \"../utils/RawFileIO.h\"\n\n#include \"gms_matcher.h\"\n\n#include \n\n#include \nusing namespace ceres;\n\n\nvoid point_feature_matches( const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n MatrixXd& u, MatrixXd& ud )\n{\n ElapsedTime timer;\n\n\n //\n // Point feature and descriptors extract\n std::vector kp1, kp2;\n cv::Mat d1, d2; //< descriptors\n\n cv::Ptr orb = cv::ORB::create(10000);\n orb->setFastThreshold(0);\n\n timer.tic();\n orb->detectAndCompute(imleft_undistorted, cv::Mat(), kp1, d1);\n orb->detectAndCompute(imright_undistorted, cv::Mat(), kp2, d2);\n cout << \"2X detectAndCompute(ms) : \" << timer.toc_milli() << endl;\n // std::cout << \"d1 \" << MiscUtils::cvmat_info( d1 ) << std::endl;\n // std::cout << \"d2 \" << MiscUtils::cvmat_info( d2 ) << std::endl;\n\n //plot\n // cv::Mat dst_left, dst_right;\n // MatrixXd e_kp1, e_kp2;\n // MiscUtils::keypoint_2_eigen( kp1, e_kp1 );\n // MiscUtils::keypoint_2_eigen( kp2, e_kp2 );\n // MiscUtils::plot_point_sets( imleft_undistorted, e_kp1, dst_left, cv::Scalar(0,0,255), false );\n // MiscUtils::plot_point_sets( imright_undistorted, e_kp2, dst_right, cv::Scalar(0,0,255), false );\n // cv::imshow( \"dst_left\", dst_left );\n // cv::imshow( \"dst_right\", dst_right );\n\n //\n // Point feature matching\n cv::BFMatcher matcher(cv::NORM_HAMMING); // TODO try FLANN matcher here.\n vector matches_all, matches_gms;\n timer.tic();\n matcher.match(d1, d2, matches_all);\n std::cout << \"BFMatcher : npts = \" << matches_all.size() << std::endl;\n std::cout << \"BFMatcher took (ms) : \"<< timer.toc_milli() << std::endl;\n\n\n // gms_matcher\n timer.tic();\n std::vector vbInliers;\n gms_matcher gms(kp1, imleft_undistorted.size(), kp2, imright_undistorted.size(), matches_all);\n int num_inliers = gms.GetInlierMask(vbInliers, false, false);\n cout << \"Got total gms matches \" << num_inliers << \" matches.\" << endl;\n cout << \"GMSMatcher took (ms) \" << timer.toc_milli() << std::endl;\n\n // collect matches\n for (size_t i = 0; i < vbInliers.size(); ++i)\n {\n if (vbInliers[i] == true)\n {\n matches_gms.push_back(matches_all[i]);\n }\n }\n // MatrixXd M1, M2;\n MiscUtils::dmatch_2_eigen( kp1, kp2, matches_gms, u, ud, true );\n\n\n}\n\n\n\n// Orthonormalization is needed because the optimization variable translation is unit normalized\n// and to preserve this property we need to define the '+' operation for it. Note that it can\n// only more in tangent space of the sphere. So graph schmit-orthonormalization gives out a basis\n// vector at this point. Ofcourse this is not a unique solution. See Fig. 3 in yonggen's iros2016 paper.\nclass UnitVectorParameterization : public ceres::LocalParameterization {\npublic:\n UnitVectorParameterization() {}\n virtual ~UnitVectorParameterization() {}\n\n virtual bool Plus(const double* x,\n const double* delta,\n double* x_plus_delta) const\n {\n // Compute T (tangent space)\n // tangent space is a function of x\n double m[5], n[5];\n bool statis = gram_schmidt_orthonormalization( x, m, n );\n if( !statis )\n return false;\n\n\n // x_new := x + T*[ delta[0]; delta[1] ]\n x_plus_delta[0] = x[0] + m[0]*delta[0] + n[0]*delta[1];\n x_plus_delta[1] = x[1] + m[1]*delta[0] + n[1]*delta[1];\n x_plus_delta[2] = x[2] + m[2]*delta[0] + n[2]*delta[1];\n return true;\n }\n\n virtual bool ComputeJacobian(const double* x,\n double* jacobian) const\n {\n // Compute T (tangent space)\n // tangent space is a function of x\n double m[5], n[5];\n bool statis = gram_schmidt_orthonormalization( x, m, n );\n if( !statis )\n return false;\n\n // \\del(x') / \\del(a) = [ T_{0,0}; T_{1,0} ]\n\n // \\del(x') / \\del(b) = [ T_{0,1}; T_{1,1} ]\n\n\n // jacobian = [ concate above 2 ]\n jacobian[0] = m[0]; jacobian[1] = n[0];\n jacobian[2] = m[1]; jacobian[3] = n[1];\n jacobian[4] = m[2]; jacobian[5] = n[2];\n return true;\n\n }\n virtual int GlobalSize() const { return 3; } // TODO : Generalize.\n virtual int LocalSize() const { return 2; }\n\n\n\n // This function return 2 vectors m, n each of dimension equal to x (ie. 3) such that x,m,n are orthogonal to each other.\n const bool gram_schmidt_orthonormalization( const double * _x , double * _m, double * _n, int len=3 ) const\n {\n VectorXd xp = VectorXd::Zero( len );\n for( int i=0 ; iXi = Xi;\n // this->Xid = Xid;\n }\n\n template \n bool operator()( const T* const q, const T* const t , T* residual ) const {\n // Optimization variables\n Quaternion eigen_q( q[0], q[1], q[2], q[3] );\n\n Eigen::Matrix eigen_tx;\n eigen_tx << T(0.0), -t[2] , t[1] ,\n t[2], T(0.0), -t[0],\n -t[1], t[0], T(0.0);\n\n\n Eigen::Matrix essential_matrix;\n essential_matrix = eigen_tx * eigen_q.toRotationMatrix() ;\n\n\n // Known Constant\n Eigen::Matrix eigen_Xi;\n Eigen::Matrix eigen_Xid;\n eigen_Xi << T(Xi(0)), T(Xi(1)), T(Xi(2));\n eigen_Xid << T(Xid(0)), T(Xid(1)), T(Xid(2));\n\n\n\n // Error term\n\n Eigen::Matrix e;\n // e = eigen_Xi - ( eigen_q.toRotationMatrix() * eigen_Xid + eigen_t );\n e = eigen_Xid.transpose() * essential_matrix * eigen_Xi;\n // e = eigen_Xi.transpose() * (essential_matrix * eigen_Xid);\n\n\n residual[0] = e(0);\n // residual[1] = e(1);\n // residual[2] = e(2);\n\n return true;\n }\n\n\n\n static ceres::CostFunction* Create(const Vector3d& _Xi, const Vector3d& _Xid)\n {\n return ( new ceres::AutoDiffCostFunction\n (\n new YonggenResidue(_Xi,_Xid)\n )\n );\n }\n\nprivate:\n Vector3d Xi, Xid;\n};\n\n\nvoid nudge_extrinsics( const cv::Mat& imleft_undistorted, const cv::Mat& imright_undistorted,\n const Matrix3d& K_new, const Matrix4d& right_T_left, Matrix4d& optimized_right_T_left )\n{\n // implements Yonggen Ling's method\n // Y. Ling and S. Shen, \"High-precision online markerless stereo extrinsic calibration,\" 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, 2016, pp. 1771-1778.\n\n // minimize_{R,t} \\sum_i || (f'_i)^T E f_i || , where R,t == right_T_left.\n // a) E is the Essential matrix E := [t]_x R\n // b) f and f' are point feature matches (f from left f from right) in normalized image co-ordinates\n\n\n //\n // Step-1 : Match point features from `imleft_undistorted, imright_undistorted`\n MatrixXd u, ud; //< 3xN, ie. image co-ordinates represented in homogeneous cord system.\n point_feature_matches( imleft_undistorted, imright_undistorted, u, ud );\n cv::Mat dst;\n MiscUtils::plot_point_pair( imleft_undistorted, u, -1, imright_undistorted, ud, -1, dst, 0 );\n cv::imshow( \"gms_matches\", dst );\n\n\n\n //\n // Step-2 : In normalized co-ordinates : inv(K_new) * f' and inv(K_inv) * f\n MatrixXd f = K_new.inverse() * u;\n MatrixXd fd = K_new.inverse() * ud;\n\n\n //\n // Step-3 : Setup CERES problem\n\n // 3.1 : Initial Guess\n double T_cap_q[10], T_cap_t[10];\n PoseManipUtils::eigenmat_to_raw( right_T_left, T_cap_q, T_cap_t );\n double n_norm = sqrt( T_cap_t[0]*T_cap_t[0] + T_cap_t[1]*T_cap_t[1] + T_cap_t[2]*T_cap_t[2] );\n T_cap_t[0] /= n_norm;\n T_cap_t[1] /= n_norm;\n T_cap_t[2] /= n_norm;\n\n cout << \"CERES Inital Guess : \" << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;\n cout << \"CERES Initial Guess n_norm: \" << n_norm << endl;\n cout << \"CERES Initial Guess T_cap_t: \" << T_cap_t[0] << \" \" << T_cap_t[1] << \" \" << T_cap_t[2] << endl;\n\n\n // 3.2 : Error Terms\n ceres::Problem problem;\n cout << \"CERES #residues : \" << f.cols() << endl;\n for( int i=0 ; igenerateCameraFromYamlFile(BASE+\"/cameraIntrinsic.0.yaml\");\n camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.1.yaml\");\n // camodocal::CameraPtr left_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(string(BASE+\"../leveled_cam_pinhole/\")+\"/camera_left.yaml\");\n // camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(string(BASE+\"../leveled_cam_pinhole/\")+\"/camera_right.yaml\");\n\n cout << left_camera->parametersToString() << endl;\n cout << right_camera->parametersToString() << endl;\n\n\n // Extrinsics - quat is wxyz . translation in meters.\n // mynt eye\n Vector4d q_wxyz = Vector4d( -1.8252509868889259e-04,-1.6291774489779708e-03,-1.2462127842978489e-03,9.9999787970731446e-01 );\n Vector3d tr_xyz = Vector3d( -1.2075905420832895e+02/1000.,5.4110610639412482e-01/1000.,2.4484815673909591e-01/1000. );\n\n // bluefox stereo leveled\n // Vector4d q_wxyz = Vector4d( -1.7809713490350254e-03, 4.2143149583451564e-04,4.1936662160154632e-02, 9.9911859501433165e-01 );\n // Vector3d tr_xyz = Vector3d( -1.4031938291177164e+02/1000.,-6.6214729932530441e+00/1000.,1.4808567571722902e+00/1000. );\n\n Matrix4d right_T_left;\n PoseManipUtils::raw_xyzw_to_eigenmat( q_wxyz, tr_xyz, right_T_left );\n\n #if 0\n cout << \"right_T_left(before applying delta): \" << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;\n Matrix4d delta;\n PoseManipUtils::rawyprt_to_eigenmat( Vector3d(4.0,4.0,4.0), Vector3d(0,0,0), delta );\n right_T_left = delta * right_T_left;\n cout << \"right_T_left(after applying delta): \" << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;\n #endif\n\n cout << \"right_T_left: \" << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;\n cout << \"right_T_left=\" << right_T_left.format( numpyFmt );\n\n\n // StereoGeometry stereogeom( left_camera,right_camera, right_T_left );\n std::shared_ptr stereogeom;\n stereogeom = std::make_shared( left_camera,right_camera, right_T_left );\n stereogeom->set_K( 375.0, 375.0, 376.0, 240.0 );\n\n // Raw Image - Image from camera\n int frame_id = 1005;\n // for( int frame_id=100 ; frame_id < 1000 ; frame_id++ )\n {\n cv::Mat imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_id)+\".jpg\", 0 );\n cv::Mat imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_id)+\"_1.jpg\", 0 );\n // cv::Mat imleft_raw = cv::imread( BASE+\"/cam0_\"+std::to_string(frame_id)+\".png\",0 );\n // cv::Mat imright_raw = cv::imread( BASE+\"/cam1_\"+ std::to_string(frame_id)+\".png\",0 );\n\n\n // Undistort Only\n cv::Mat imleft_undistorted, imright_undistorted;\n timer.tic();\n stereogeom->do_image_undistortion( imleft_raw, imright_raw, imleft_undistorted, imright_undistorted );\n cout << timer.toc_milli() << \" :(ms) stereogeom->do_image_undistortion()\\n\";\n cv::imshow( \"imleft_raw\", imleft_raw );\n cv::imshow( \"imright_raw\", imright_raw );\n cv::imshow( \"imleft_undistorted\", imleft_undistorted );\n cv::imshow( \"imright_undistorted\", imright_undistorted );\n\n // #define YONGGEN_MARKERLESS_ONLINE_STEREOCALIB\n #ifdef YONGGEN_MARKERLESS_ONLINE_STEREOCALIB\n Matrix4d optimized_right_T_left;\n nudge_extrinsics(imleft_undistorted, imright_undistorted, stereogeom->get_K(), right_T_left, optimized_right_T_left);\n cout << \"optimized_right_T_left: \" << PoseManipUtils::prettyprintMatrix4d( optimized_right_T_left) << endl;\n stereogeom->set_stereoextrinsic( optimized_right_T_left );\n #endif\n\n\n\n\n // Draw Epipolar Lines\n cv::Mat dst_imleft_undistorted = imleft_undistorted.clone();\n cv::Mat dst_imright_undistorted = imright_undistorted.clone();\n stereogeom->draw_epipolarlines(dst_imleft_undistorted, dst_imright_undistorted);\n cv::imshow( \"imleft_undistorted\", dst_imleft_undistorted );\n cv::imshow( \"imright_undistorted\", dst_imright_undistorted );\n\n\n\n // Stereo Rectify.\n cv::Mat imleft_srectified, imright_srectified;\n timer.tic();\n stereogeom->do_stereo_rectification_of_undistorted_images(\n imleft_undistorted, imright_undistorted,\n imleft_srectified, imright_srectified );\n cout << timer.toc_milli() << \" :(ms) stereogeom->do_stereo_rectification_of_undistorted_images\\n\";\n cv::imshow( \"imleft_srectified\", imleft_srectified );\n cv::imshow( \"imright_srectified\", imright_srectified );\n\n\n\n // Draw Epipolar lines on stereo rectified images\n cv::Mat dst_imleft_srectified = imleft_srectified.clone();\n cv::Mat dst_imright_srectified = imright_srectified.clone();\n stereogeom->draw_srectified_epipolarlines( dst_imleft_srectified, dst_imright_srectified );\n cv::imshow( \"dst_imleft_srectified\", dst_imleft_srectified );\n cv::imshow( \"dst_imright_srectified\", dst_imright_srectified );\n\n\n\n // Stereo Block Matching to compute disparity\n cv::Mat disp_raw, disp8;\n timer.tic();\n stereogeom->do_stereoblockmatching_of_srectified_images( imleft_srectified, imright_srectified, disp_raw );\n // stereogeom->do_stereoblockmatching_of_raw_images( imleft_raw, imright_raw, disp_raw );\n cout << timer.toc_milli() << \" : (ms)do_stereoblockmatching_of_srectified_images done in\\n\";\n cout << \"disp_raw info \" << MiscUtils::cvmat_info( disp_raw ) << endl;\n stereogeom->print_blockmatcher_algo_info();\n cv::normalize(disp_raw, disp8, 0, 255, CV_MINMAX, CV_8U); //< disp8 used just for visualization\n cv::imshow( \"disp8\", disp8 );\n\n\n #if 0\n {\n cv::FileStorage file(\"disp_raw.opencv.txt\", cv::FileStorage::WRITE);\n file << \"disp_raw\" << disp_raw;\n file.release();\n\n\n // save disp_raw\n MatrixXd e_disp_raw;\n cv::cv2eigen(disp_raw, e_disp_raw );\n RawFileIO::write_EigenMatrix( \"./disp_raw.txt\", e_disp_raw );\n\n }\n #endif\n\n\n // Disparity to pointcloud\n const cv::Mat Q = stereogeom->get_Q();\n cv::Mat _3dImage;\n MatrixXd _3dpts;\n timer.tic();\n stereogeom->disparity_to_3DPoints( disp_raw, _3dImage, _3dpts, true, true );\n cout << timer.toc_milli() << \" : (ms) disparity_to_3DPoints computed in \\n\";\n\n #if 0\n // split channels\n cout << \"_3dImage : \" << MiscUtils::cvmat_info( _3dImage ) << endl;\n Mat _3dImage_XYZ[3]; //destination array\n cv::split(_3dImage,_3dImage_XYZ);//split source\n cout << \"_3dImageX : \" << MiscUtils::cvmat_info( _3dImage_XYZ[0] ) << endl;\n cout << \"_3dImageY : \" << MiscUtils::cvmat_info( _3dImage_XYZ[1] ) << endl;\n cout << \"_3dImageZ : \" << MiscUtils::cvmat_info( _3dImage_XYZ[2] ) << endl;\n\n MatrixXf e_3dImage[3];\n cv::cv2eigen( _3dImage_XYZ[0], e_3dImage[0] );\n cv::cv2eigen( _3dImage_XYZ[1], e_3dImage[1] );\n cv::cv2eigen( _3dImage_XYZ[2], e_3dImage[2] );\n RawFileIO::write_EigenMatrix( \"./e_3dImageX.txt\", e_3dImage[0] );\n RawFileIO::write_EigenMatrix( \"./e_3dImageY.txt\", e_3dImage[1] );\n RawFileIO::write_EigenMatrix( \"./e_3dImageZ.txt\", e_3dImage[2] );\n RawFileIO::write_EigenMatrix( \"./_3dpts.txt\", _3dpts );\n #endif\n\n\n\n // Visualize 3d point cloud from stereo\n // a) as reprojections\n vector pt_colors;\n GeometryUtils::depthColors( _3dpts, pt_colors, .5, 4.5 );\n\n\n MatrixXd perspective_proj = MatrixXd::Zero( 3, _3dpts.cols() );\n for( int k=0 ; k<_3dpts.cols() ; k++ ) {\n perspective_proj(0,k) = _3dpts( 0, k ) / _3dpts( 2, k) ;\n perspective_proj(1,k) = _3dpts( 1, k ) / _3dpts( 2, k) ;\n perspective_proj(2,k) = 1.0;\n }\n\n MatrixXd reproj_uv = stereogeom->get_K() * perspective_proj;\n #if 0\n cout << \"_3dpts(sample)\\n\" << _3dpts.leftCols(10) << endl;\n cout << \"perspective_proj(sample)\\n\" << perspective_proj.leftCols(10) << endl;\n cout << \"reproj_uv(sample)\\n\" << reproj_uv.leftCols(10) << endl;\n #endif\n\n cv::Mat dst_reproj_uv;\n MiscUtils::plot_point_sets( imleft_srectified, reproj_uv, dst_reproj_uv, pt_colors, 0.6, \"colored by depth\" );\n cv::imshow( \"dst_reproj_uv\", dst_reproj_uv );\n cv::waitKey(0);\n }\n\n}\n\nint stereo_demo_easy()\n{\n ElapsedTime timer;\n // const std::string BASE = \"/Bulk_Data/_tmp_cerebro/mynt_multi_loops_in_lab/\";\n // const std::string BASE = \"/Bulk_Data/ros_bags/bluefox_stereo/calib/leveled_cam_sampled/\";\n const std::string BASE = \"/Bulk_Data/_tmp_cerebro/mynt_seng3/\";\n\n //--------- Intrinsics load\n camodocal::CameraPtr left_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.0.yaml\");\n camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.1.yaml\");\n // camodocal::CameraPtr left_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(string(BASE+\"../leveled_cam_pinhole/\")+\"/camera_left.yaml\");\n // camodocal::CameraPtr right_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(string(BASE+\"../leveled_cam_pinhole/\")+\"/camera_right.yaml\");\n\n cout << left_camera->parametersToString() << endl;\n cout << right_camera->parametersToString() << endl;\n\n\n //----------- Stereo Base line load (alsoed called extrinsic calibration)\n // mynt eye\n Vector4d q_wxyz = Vector4d( -1.8252509868889259e-04,-1.6291774489779708e-03,-1.2462127842978489e-03,9.9999787970731446e-01 );\n Vector3d tr_xyz = Vector3d( -1.2075905420832895e+02/1000.,5.4110610639412482e-01/1000.,2.4484815673909591e-01/1000. );\n\n // bluefox stereo leveled\n // Vector4d q_wxyz = Vector4d( -1.7809713490350254e-03, 4.2143149583451564e-04,4.1936662160154632e-02, 9.9911859501433165e-01 );\n // Vector3d tr_xyz = Vector3d( -1.4031938291177164e+02/1000.,-6.6214729932530441e+00/1000.,1.4808567571722902e+00/1000. );\n\n Matrix4d right_T_left;\n PoseManipUtils::raw_xyzw_to_eigenmat( q_wxyz, tr_xyz, right_T_left );\n // cout << \"right_T_left: \" << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;\n\n\n\n //-------------------- init stereogeom\n std::shared_ptr stereogeom;\n stereogeom = std::make_shared( left_camera,right_camera, right_T_left );\n stereogeom->set_K( 375.0, 375.0, 376.0, 240.0 );\n\n\n\n int frame_id = 1005;\n //----------------- load images_raw for left and right\n for( frame_id=0; frame_id < 2500 ; frame_id++ )\n {\n cout << \"READ IMAGE \" << frame_id << endl;\n cv::Mat imleft_raw = cv::imread( BASE+\"/\"+std::to_string(frame_id)+\".jpg\", 0 );\n cv::Mat imright_raw = cv::imread( BASE+\"/\"+std::to_string(frame_id)+\"_1.jpg\", 0 );\n // cv::Mat imleft_raw = cv::imread( BASE+\"/cam0_\"+std::to_string(frame_id)+\".png\",0 );\n // cv::Mat imright_raw = cv::imread( BASE+\"/cam1_\"+ std::to_string(frame_id)+\".png\",0 );\n\n if( imleft_raw.empty() || imright_raw.empty() )\n continue;\n\n //------------------- stereogeom->get3dpoints_from_raw_images()\n // can use one of the options depending on the need.\n #if 0\n // (A) fastest - if you are just looking for valid 3d points - look at the CameraGeometry.h header to see various options to call.\n timer.tic();\n MatrixXd _3dpts; //4xN\n stereogeom->get3dpoints_from_raw_images(imleft_raw, imright_raw, _3dpts );\n cout << timer.toc_milli() << \" (ms)!! get3dpoints_from_raw_images\\n\";\n\n cout << \"_3dpts.shape= \" << _3dpts.rows() << \" \" << _3dpts.cols() << endl;\n #endif\n\n\n #if 0\n // will get the 3d points and disparity. Takes about 2-4ms more than (A)\n MatrixXd _3dpts; //4xN\n cv::Mat disparity_for_visualization;\n cout << \"_3dpts.shape= \" << _3dpts.rows() << \" \" << _3dpts.cols() << endl;\n timer.tic();\n stereogeom->get3dpoints_and_disparity_from_raw_images(imleft_raw, imright_raw, _3dpts, disparity_for_visualization );\n cout << timer.toc_milli() << \" (ms)!! get3dpoints_and_disparity_from_raw_images\\n\";\n\n cv::imshow( \"disparity_for_visualization\", disparity_for_visualization );\n #endif\n\n\n #if 1\n // will get 3d points, stereo-rectified image, and disparity false colormap\n MatrixXd _3dpts; //4xN\n cv::Mat imleft_srectified, imright_srectified;\n cv::Mat disparity_for_visualization;\n\n timer.tic();\n stereogeom->get_srectifiedim_and_3dpoints_and_disparity_from_raw_images(imleft_raw, imright_raw,\n imleft_srectified, imright_srectified,\n _3dpts, disparity_for_visualization );\n cout << timer.toc_milli() << \" (ms)!! get_srectifiedim_and_3dpoints_and_disparity_from_raw_images\\n\";\n\n cv::imshow( \"imleft_srectified\", imleft_srectified );\n cv::imshow( \"imright_srectified\", imright_srectified );\n cv::imshow( \"disparity_for_visualization\", disparity_for_visualization );\n #endif\n\n\n //-------------------- reproject the 3d points.\n // note: that these 3d points after reprojections will be correct as plotted to imleft_srectified\n #if 0\n vector pt_colors;\n GeometryUtils::depthColors( _3dpts, pt_colors, .5, 4.5 );\n\n\n MatrixXd perspective_proj = MatrixXd::Zero( 3, _3dpts.cols() ); // perspective project _3dpts.\n for( int k=0 ; k<_3dpts.cols() ; k++ ) {\n perspective_proj(0,k) = _3dpts( 0, k ) / _3dpts( 2, k) ;\n perspective_proj(1,k) = _3dpts( 1, k ) / _3dpts( 2, k) ;\n perspective_proj(2,k) = 1.0;\n }\n\n MatrixXd reproj_uv = stereogeom->get_K() * perspective_proj; //< scaling with camera intrinsic for visualization\n cout << \"_3dpts(sample)\\n\" << _3dpts.leftCols(10) << endl;\n cout << \"perspective_proj(sample)\\n\" << perspective_proj.leftCols(10) << endl;\n cout << \"reproj_uv(sample)\\n\" << reproj_uv.leftCols(10) << endl;\n\n\n cv::Mat dst_reproj_uv;\n MiscUtils::plot_point_sets( imleft_srectified, reproj_uv, dst_reproj_uv, pt_colors, 0.6, \"plot of reprojected points;colored by depth\" );\n cv::imshow( \"dst_reproj_uv\", dst_reproj_uv );\n #endif\n\n //-------------------- visualize 3d points with rviz\n\n cv::waitKey(0);\n }\n\n\n\n}\n\n// Monocular\nint monocular_demo()\n{\n const std::string BASE = \"/Bulk_Data/_tmp_cerebro/mynt_multi_loops_in_lab/\";\n\n\n // Abstract Camera\n camodocal::CameraPtr m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(BASE+\"/cameraIntrinsic.0.yaml\");\n cout << m_camera->parametersToString() << endl;\n\n // Init Monocular Geometry\n MonoGeometry monogeom( m_camera );\n\n // Raw Image - Image from camera\n int frame_id = 23;\n cv::Mat im_raw = cv::imread( BASE+\"/\"+std::to_string(frame_id)+\".jpg\" );\n\n cv::Mat im_undistorted;\n monogeom.do_image_undistortion( im_raw, im_undistorted );\n\n\n cv::imshow( \"im_raw\", im_raw );\n cv::imshow( \"im_undistorted\", im_undistorted );\n cv::waitKey(0);\n}\n\nint main() {\n // monocular_demo();\n // stereo_demo();\n stereo_demo_easy();\n\n /*\n double _x[5];\n _x[0] = 0.545435;\n _x[1] = .8;\n _x[2] = .25;\n\n double _m[5], _n[5];\n UnitVectorParameterization parm = UnitVectorParameterization();\n parm.gram_schmidt_orthonormalization( _x, _m, _n );\n */\n}\n" }, { "path": "src/utils/camodocal/README.md", "content": "# Camodocal\n\nOriginal Implementation : https://github.com/hengli/camodocal\nMy (Manohar Kuse, mpkuse@connect.ust.hk) Adaptation : https://github.com/mpkuse/camera_model\n\n## CMakefile.txt\nIf you put this folder under `src/utils/` and CMakeLists.txt is in `./`\nyou will need to add the following to you CMake files\n\n```\ninclude_directories(src/utils/camodocal/include)\nFILE(GLOB CamodocalCameraModelSources\n src/utils/camodocal/src/chessboard/Chessboard.cc\n src/utils/camodocal/src/calib/CameraCalibration.cc\n src/utils/camodocal/src/camera_models/Camera.cc\n src/utils/camodocal/src/camera_models/CameraFactory.cc\n src/utils/camodocal/src/camera_models/CostFunctionFactory.cc\n src/utils/camodocal/src/camera_models/PinholeCamera.cc\n src/utils/camodocal/src/camera_models/CataCamera.cc\n src/utils/camodocal/src/camera_models/EquidistantCamera.cc\n src/utils/camodocal/src/camera_models/ScaramuzzaCamera.cc\n src/utils/camodocal/src/sparse_graph/Transform.cc\n src/utils/camodocal/src/gpl/gpl.cc\n src/utils/camodocal/src/gpl/EigenQuaternionParameterization.cc\n )\n```\n\nDependencies if not already added:\n```\nfind_package(Eigen3 REQUIRED)\nfind_package(Ceres REQUIRED)\nfind_package(OpenCV 3 REQUIRED)\nfind_package(Boost REQUIRED COMPONENTS filesystem program_options system)\n\n```\n" }, { "path": "src/utils/camodocal/include/camodocal/calib/CameraCalibration.h", "content": "#ifndef CAMERACALIBRATION_H\n#define CAMERACALIBRATION_H\n\n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace camodocal\n{\n\nclass CameraCalibration\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n CameraCalibration();\n\n CameraCalibration(Camera::ModelType modelType,\n const std::string& cameraName,\n const cv::Size& imageSize,\n const cv::Size& boardSize,\n float squareSize);\n\n void clear(void);\n\n void addChessboardData(const std::vector& corners);\n\n bool calibrate(void);\n\n int sampleCount(void) const;\n std::vector >& imagePoints(void);\n const std::vector >& imagePoints(void) const;\n std::vector >& scenePoints(void);\n const std::vector >& scenePoints(void) const;\n CameraPtr& camera(void);\n const CameraConstPtr camera(void) const;\n\n Eigen::Matrix2d& measurementCovariance(void);\n const Eigen::Matrix2d& measurementCovariance(void) const;\n\n cv::Mat& cameraPoses(void);\n const cv::Mat& cameraPoses(void) const;\n\n void drawResults(std::vector& images) const;\n\n void writeParams(const std::string& filename) const;\n\n bool writeChessboardData(const std::string& filename) const;\n bool readChessboardData(const std::string& filename);\n\n void setVerbose(bool verbose);\n\nprivate:\n bool calibrateHelper(CameraPtr& camera,\n std::vector& rvecs, std::vector& tvecs) const;\n\n void optimize(CameraPtr& camera,\n std::vector& rvecs, std::vector& tvecs) const;\n\n template\n void readData(std::ifstream& ifs, T& data) const;\n\n template\n void writeData(std::ofstream& ofs, T data) const;\n\n cv::Size m_boardSize;\n float m_squareSize;\n\n CameraPtr m_camera;\n cv::Mat m_cameraPoses;\n\n std::vector > m_imagePoints;\n std::vector > m_scenePoints;\n\n Eigen::Matrix2d m_measurementCovariance;\n\n bool m_verbose;\n};\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/camera_models/Camera.h", "content": "#ifndef CAMERA_H\n#define CAMERA_H\n\n#include \n#include \n#include \n#include \n\nnamespace camodocal\n{\n\nclass Camera\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n enum ModelType\n {\n KANNALA_BRANDT,\n MEI,\n PINHOLE,\n SCARAMUZZA\n };\n\n class Parameters\n {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n Parameters(ModelType modelType);\n\n Parameters(ModelType modelType, const std::string& cameraName,\n int w, int h);\n\n ModelType& modelType(void);\n std::string& cameraName(void);\n int& imageWidth(void);\n int& imageHeight(void);\n\n ModelType modelType(void) const;\n const std::string& cameraName(void) const;\n int imageWidth(void) const;\n int imageHeight(void) const;\n\n int nIntrinsics(void) const;\n\n virtual bool readFromYamlFile(const std::string& filename) = 0;\n virtual void writeToYamlFile(const std::string& filename) const = 0;\n\n protected:\n ModelType m_modelType;\n int m_nIntrinsics;\n std::string m_cameraName;\n int m_imageWidth;\n int m_imageHeight;\n };\n\n virtual ModelType modelType(void) const = 0;\n virtual const std::string& cameraName(void) const = 0;\n virtual int imageWidth(void) const = 0;\n virtual int imageHeight(void) const = 0;\n\n virtual cv::Mat& mask(void);\n virtual const cv::Mat& mask(void) const;\n\n virtual void estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints) = 0;\n virtual void estimateExtrinsics(const std::vector& objectPoints,\n const std::vector& imagePoints,\n cv::Mat& rvec, cv::Mat& tvec) const;\n\n // Lift points from the image plane to the sphere\n virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;\n //%output P\n\n // Lift points from the image plane to the projective space\n virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;\n //%output P\n\n // Projects 3D points to the image plane (Pi function)\n virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0;\n //%output p\n\n // Projects 3D points to the image plane (Pi function)\n // and calculates jacobian\n //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n // Eigen::Matrix& J) const = 0;\n //%output p\n //%output J\n\n virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0;\n //%output p\n\n //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0;\n virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx = -1.0f, float fy = -1.0f,\n cv::Size imageSize = cv::Size(0, 0),\n float cx = -1.0f, float cy = -1.0f,\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0;\n\n virtual int parameterCount(void) const = 0;\n\n virtual void readParameters(const std::vector& parameters) = 0;\n virtual void writeParameters(std::vector& parameters) const = 0;\n\n virtual void writeParametersToYamlFile(const std::string& filename) const = 0;\n\n virtual std::string parametersToString(void) const = 0;\n\n /**\n * \\brief Calculates the reprojection distance between points\n *\n * \\param P1 first 3D point coordinates\n * \\param P2 second 3D point coordinates\n * \\return euclidean distance in the plane\n */\n double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const;\n\n double reprojectionError(const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints,\n const std::vector& rvecs,\n const std::vector& tvecs,\n cv::OutputArray perViewErrors = cv::noArray()) const;\n\n double reprojectionError(const Eigen::Vector3d& P,\n const Eigen::Quaterniond& camera_q,\n const Eigen::Vector3d& camera_t,\n const Eigen::Vector2d& observed_p) const;\n\n void projectPoints(const std::vector& objectPoints,\n const cv::Mat& rvec,\n const cv::Mat& tvec,\n std::vector& imagePoints) const;\nprotected:\n cv::Mat m_mask;\n};\n\ntypedef boost::shared_ptr CameraPtr;\ntypedef boost::shared_ptr CameraConstPtr;\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/camera_models/CameraFactory.h", "content": "#ifndef CAMERAFACTORY_H\n#define CAMERAFACTORY_H\n\n#include \n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace camodocal\n{\n\nclass CameraFactory\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n CameraFactory();\n\n static boost::shared_ptr instance(void);\n\n CameraPtr generateCamera(Camera::ModelType modelType,\n const std::string& cameraName,\n cv::Size imageSize) const;\n\n CameraPtr generateCameraFromYamlFile(const std::string& filename);\n\nprivate:\n static boost::shared_ptr m_instance;\n};\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/camera_models/CataCamera.h", "content": "#ifndef CATACAMERA_H\r\n#define CATACAMERA_H\r\n\r\n#include \r\n#include \r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n/**\r\n * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration\r\n * from Planar Grids, ICRA 2007\r\n */\r\n\r\nclass CataCamera: public Camera\r\n{\r\npublic:\r\n class Parameters: public Camera::Parameters\r\n {\r\n public:\r\n Parameters();\r\n Parameters(const std::string& cameraName,\r\n int w, int h,\r\n double xi,\r\n double k1, double k2, double p1, double p2,\r\n double gamma1, double gamma2, double u0, double v0);\r\n\r\n double& xi(void);\r\n double& k1(void);\r\n double& k2(void);\r\n double& p1(void);\r\n double& p2(void);\r\n double& gamma1(void);\r\n double& gamma2(void);\r\n double& u0(void);\r\n double& v0(void);\r\n\r\n double xi(void) const;\r\n double k1(void) const;\r\n double k2(void) const;\r\n double p1(void) const;\r\n double p2(void) const;\r\n double gamma1(void) const;\r\n double gamma2(void) const;\r\n double u0(void) const;\r\n double v0(void) const;\r\n\r\n bool readFromYamlFile(const std::string& filename);\r\n void writeToYamlFile(const std::string& filename) const;\r\n\r\n Parameters& operator=(const Parameters& other);\r\n friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n private:\r\n double m_xi;\r\n double m_k1;\r\n double m_k2;\r\n double m_p1;\r\n double m_p2;\r\n double m_gamma1;\r\n double m_gamma2;\r\n double m_u0;\r\n double m_v0;\r\n };\r\n\r\n CataCamera();\r\n\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n CataCamera(const std::string& cameraName,\r\n int imageWidth, int imageHeight,\r\n double xi, double k1, double k2, double p1, double p2,\r\n double gamma1, double gamma2, double u0, double v0);\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n CataCamera(const Parameters& params);\r\n\r\n Camera::ModelType modelType(void) const;\r\n const std::string& cameraName(void) const;\r\n int imageWidth(void) const;\r\n int imageHeight(void) const;\r\n\r\n void estimateIntrinsics(const cv::Size& boardSize,\r\n const std::vector< std::vector >& objectPoints,\r\n const std::vector< std::vector >& imagePoints);\r\n\r\n // Lift points from the image plane to the sphere\r\n void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Lift points from the image plane to the projective space\r\n void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n // and calculates jacobian\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n Eigen::Matrix& J) const;\r\n //%output p\r\n //%output J\r\n\r\n void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n template \r\n static void spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p);\r\n\r\n void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;\r\n void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\r\n Eigen::Matrix2d& J) const;\r\n\r\n void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n float fx = -1.0f, float fy = -1.0f,\r\n cv::Size imageSize = cv::Size(0, 0),\r\n float cx = -1.0f, float cy = -1.0f,\r\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n int parameterCount(void) const;\r\n\r\n const Parameters& getParameters(void) const;\r\n void setParameters(const Parameters& parameters);\r\n\r\n void readParameters(const std::vector& parameterVec);\r\n void writeParameters(std::vector& parameterVec) const;\r\n\r\n void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n std::string parametersToString(void) const;\r\n\r\nprivate:\r\n Parameters mParameters;\r\n\r\n double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\r\n bool m_noDistortion;\r\n};\r\n\r\ntypedef boost::shared_ptr CataCameraPtr;\r\ntypedef boost::shared_ptr CataCameraConstPtr;\r\n\r\ntemplate \r\nvoid\r\nCataCamera::spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p)\r\n{\r\n T P_w[3];\r\n P_w[0] = T(P(0));\r\n P_w[1] = T(P(1));\r\n P_w[2] = T(P(2));\r\n\r\n // Convert quaternion from Eigen convention (x, y, z, w)\r\n // to Ceres convention (w, x, y, z)\r\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n T P_c[3];\r\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n P_c[0] += t[0];\r\n P_c[1] += t[1];\r\n P_c[2] += t[2];\r\n\r\n // project 3D object point to the image plane\r\n T xi = params[0];\r\n T k1 = params[1];\r\n T k2 = params[2];\r\n T p1 = params[3];\r\n T p2 = params[4];\r\n T gamma1 = params[5];\r\n T gamma2 = params[6];\r\n T alpha = T(0); //cameraParams.alpha();\r\n T u0 = params[7];\r\n T v0 = params[8];\r\n\r\n // Transform to model plane\r\n T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);\r\n P_c[0] /= len;\r\n P_c[1] /= len;\r\n P_c[2] /= len;\r\n\r\n T u = P_c[0] / (P_c[2] + xi);\r\n T v = P_c[1] / (P_c[2] + xi);\r\n\r\n T rho_sqr = u * u + v * v;\r\n T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;\r\n T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);\r\n T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;\r\n\r\n u = L * u + du;\r\n v = L * v + dv;\r\n p(0) = gamma1 * (u + alpha * v) + u0;\r\n p(1) = gamma2 * v + v0;\r\n}\r\n\r\n}\r\n\r\n#endif\r\n" }, { "path": "src/utils/camodocal/include/camodocal/camera_models/CostFunctionFactory.h", "content": "#ifndef COSTFUNCTIONFACTORY_H\n#define COSTFUNCTIONFACTORY_H\n\n#include \n#include \n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace ceres\n{\n class CostFunction;\n}\n\nnamespace camodocal\n{\n\nenum\n{\n CAMERA_INTRINSICS = 1 << 0,\n CAMERA_POSE = 1 << 1,\n POINT_3D = 1 << 2,\n ODOMETRY_INTRINSICS = 1 << 3,\n ODOMETRY_3D_POSE = 1 << 4,\n ODOMETRY_6D_POSE = 1 << 5,\n CAMERA_ODOMETRY_TRANSFORM = 1 << 6\n};\n\nclass CostFunctionFactory\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n CostFunctionFactory();\n\n static boost::shared_ptr instance(void);\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n int flags) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat,\n int flags) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector2d& observed_p,\n int flags, bool optimize_cam_odo_z = true) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat,\n int flags, bool optimize_cam_odo_z = true) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n int flags, bool optimize_cam_odo_z = true) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Quaterniond& cam_odo_q,\n const Eigen::Vector3d& cam_odo_t,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n int flags) const;\n\n ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft,\n const CameraConstPtr& cameraRight,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p_left,\n const Eigen::Vector2d& observed_p_right) const;\n\nprivate:\n static boost::shared_ptr m_instance;\n};\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/camera_models/EquidistantCamera.h", "content": "#ifndef EQUIDISTANTCAMERA_H\r\n#define EQUIDISTANTCAMERA_H\r\n\r\n#include \r\n#include \r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n/**\r\n * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method\r\n * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006\r\n */\r\n\r\nclass EquidistantCamera: public Camera\r\n{\r\npublic:\r\n class Parameters: public Camera::Parameters\r\n {\r\n public:\r\n Parameters();\r\n Parameters(const std::string& cameraName,\r\n int w, int h,\r\n double k2, double k3, double k4, double k5,\r\n double mu, double mv,\r\n double u0, double v0);\r\n\r\n double& k2(void);\r\n double& k3(void);\r\n double& k4(void);\r\n double& k5(void);\r\n double& mu(void);\r\n double& mv(void);\r\n double& u0(void);\r\n double& v0(void);\r\n\r\n double k2(void) const;\r\n double k3(void) const;\r\n double k4(void) const;\r\n double k5(void) const;\r\n double mu(void) const;\r\n double mv(void) const;\r\n double u0(void) const;\r\n double v0(void) const;\r\n\r\n bool readFromYamlFile(const std::string& filename);\r\n void writeToYamlFile(const std::string& filename) const;\r\n\r\n Parameters& operator=(const Parameters& other);\r\n friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n private:\r\n // projection\r\n double m_k2;\r\n double m_k3;\r\n double m_k4;\r\n double m_k5;\r\n\r\n double m_mu;\r\n double m_mv;\r\n double m_u0;\r\n double m_v0;\r\n };\r\n\r\n EquidistantCamera();\r\n\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n EquidistantCamera(const std::string& cameraName,\r\n int imageWidth, int imageHeight,\r\n double k2, double k3, double k4, double k5,\r\n double mu, double mv,\r\n double u0, double v0);\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n EquidistantCamera(const Parameters& params);\r\n\r\n Camera::ModelType modelType(void) const;\r\n const std::string& cameraName(void) const;\r\n int imageWidth(void) const;\r\n int imageHeight(void) const;\r\n\r\n void estimateIntrinsics(const cv::Size& boardSize,\r\n const std::vector< std::vector >& objectPoints,\r\n const std::vector< std::vector >& imagePoints);\r\n\r\n // Lift points from the image plane to the sphere\r\n virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Lift points from the image plane to the projective space\r\n void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n // and calculates jacobian\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n Eigen::Matrix& J) const;\r\n //%output p\r\n //%output J\r\n\r\n void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n template \r\n static void spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p);\r\n\r\n void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n float fx = -1.0f, float fy = -1.0f,\r\n cv::Size imageSize = cv::Size(0, 0),\r\n float cx = -1.0f, float cy = -1.0f,\r\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n int parameterCount(void) const;\r\n\r\n const Parameters& getParameters(void) const;\r\n void setParameters(const Parameters& parameters);\r\n\r\n void readParameters(const std::vector& parameterVec);\r\n void writeParameters(std::vector& parameterVec) const;\r\n\r\n void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n std::string parametersToString(void) const;\r\n\r\nprivate:\r\n template\r\n static T r(T k2, T k3, T k4, T k5, T theta);\r\n\r\n\r\n void fitOddPoly(const std::vector& x, const std::vector& y,\r\n int n, std::vector& coeffs) const;\r\n\r\n void backprojectSymmetric(const Eigen::Vector2d& p_u,\r\n double& theta, double& phi) const;\r\n\r\n Parameters mParameters;\r\n\r\n double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\r\n};\r\n\r\ntypedef boost::shared_ptr EquidistantCameraPtr;\r\ntypedef boost::shared_ptr EquidistantCameraConstPtr;\r\n\r\ntemplate\r\nT\r\nEquidistantCamera::r(T k2, T k3, T k4, T k5, T theta)\r\n{\r\n // k1 = 1\r\n return theta +\r\n k2 * theta * theta * theta +\r\n k3 * theta * theta * theta * theta * theta +\r\n k4 * theta * theta * theta * theta * theta * theta * theta +\r\n k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;\r\n}\r\n\r\ntemplate \r\nvoid\r\nEquidistantCamera::spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p)\r\n{\r\n T P_w[3];\r\n P_w[0] = T(P(0));\r\n P_w[1] = T(P(1));\r\n P_w[2] = T(P(2));\r\n\r\n // Convert quaternion from Eigen convention (x, y, z, w)\r\n // to Ceres convention (w, x, y, z)\r\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n T P_c[3];\r\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n P_c[0] += t[0];\r\n P_c[1] += t[1];\r\n P_c[2] += t[2];\r\n\r\n // project 3D object point to the image plane;\r\n T k2 = params[0];\r\n T k3 = params[1];\r\n T k4 = params[2];\r\n T k5 = params[3];\r\n T mu = params[4];\r\n T mv = params[5];\r\n T u0 = params[6];\r\n T v0 = params[7];\r\n\r\n T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);\r\n T theta = acos(P_c[2] / len);\r\n T phi = atan2(P_c[1], P_c[0]);\r\n\r\n Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi));\r\n\r\n p(0) = mu * p_u(0) + u0;\r\n p(1) = mv * p_u(1) + v0;\r\n}\r\n\r\n}\r\n\r\n#endif\r\n" }, { "path": "src/utils/camodocal/include/camodocal/camera_models/PinholeCamera.h", "content": "#ifndef PINHOLECAMERA_H\n#define PINHOLECAMERA_H\n\n#include \n#include \n\n#include \"ceres/rotation.h\"\n#include \"Camera.h\"\n\nnamespace camodocal\n{\n\nclass PinholeCamera: public Camera\n{\npublic:\n class Parameters: public Camera::Parameters\n {\n public:\n Parameters();\n Parameters(const std::string& cameraName,\n int w, int h,\n double k1, double k2, double p1, double p2,\n double fx, double fy, double cx, double cy);\n\n double& k1(void);\n double& k2(void);\n double& p1(void);\n double& p2(void);\n double& fx(void);\n double& fy(void);\n double& cx(void);\n double& cy(void);\n\n double xi(void) const;\n double k1(void) const;\n double k2(void) const;\n double p1(void) const;\n double p2(void) const;\n double fx(void) const;\n double fy(void) const;\n double cx(void) const;\n double cy(void) const;\n\n bool readFromYamlFile(const std::string& filename);\n void writeToYamlFile(const std::string& filename) const;\n\n Parameters& operator=(const Parameters& other);\n friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\n\n private:\n double m_k1;\n double m_k2;\n double m_p1;\n double m_p2;\n double m_fx;\n double m_fy;\n double m_cx;\n double m_cy;\n };\n\n PinholeCamera();\n\n /**\n * \\brief Constructor from the projection model parameters\n */\n PinholeCamera(const std::string& cameraName,\n int imageWidth, int imageHeight,\n double k1, double k2, double p1, double p2,\n double fx, double fy, double cx, double cy);\n /**\n * \\brief Constructor from the projection model parameters\n */\n PinholeCamera(const Parameters& params);\n\n Camera::ModelType modelType(void) const;\n const std::string& cameraName(void) const;\n int imageWidth(void) const;\n int imageHeight(void) const;\n\n void estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints);\n\n // Lift points from the image plane to the sphere\n virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\n //%output P\n\n // Lift points from the image plane to the projective space\n void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\n //%output P\n\n // Projects 3D points to the image plane (Pi function)\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\n //%output p\n\n // Projects 3D points to the image plane (Pi function)\n // and calculates jacobian\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n Eigen::Matrix& J) const;\n //%output p\n //%output J\n\n void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\n //%output p\n\n template \n static void spaceToPlane(const T* const params,\n const T* const q, const T* const t,\n const Eigen::Matrix& P,\n Eigen::Matrix& p);\n\n void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;\n void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n Eigen::Matrix2d& J) const;\n\n void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\n cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx = -1.0f, float fy = -1.0f,\n cv::Size imageSize = cv::Size(0, 0),\n float cx = -1.0f, float cy = -1.0f,\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\n\n int parameterCount(void) const;\n\n const Parameters& getParameters(void) const;\n void setParameters(const Parameters& parameters);\n\n void readParameters(const std::vector& parameterVec);\n void writeParameters(std::vector& parameterVec) const;\n\n void writeParametersToYamlFile(const std::string& filename) const;\n\n std::string parametersToString(void) const;\n\nprivate:\n Parameters mParameters;\n\n double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\n bool m_noDistortion;\n};\n\ntypedef boost::shared_ptr PinholeCameraPtr;\ntypedef boost::shared_ptr PinholeCameraConstPtr;\n\ntemplate \nvoid\nPinholeCamera::spaceToPlane(const T* const params,\n const T* const q, const T* const t,\n const Eigen::Matrix& P,\n Eigen::Matrix& p)\n{\n T P_w[3];\n P_w[0] = T(P(0));\n P_w[1] = T(P(1));\n P_w[2] = T(P(2));\n\n // Convert quaternion from Eigen convention (x, y, z, w)\n // to Ceres convention (w, x, y, z)\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\n\n T P_c[3];\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\n\n P_c[0] += t[0];\n P_c[1] += t[1];\n P_c[2] += t[2];\n\n // project 3D object point to the image plane\n T k1 = params[0];\n T k2 = params[1];\n T p1 = params[2];\n T p2 = params[3];\n T fx = params[4];\n T fy = params[5];\n T alpha = T(0); //cameraParams.alpha();\n T cx = params[6];\n T cy = params[7];\n\n // Transform to model plane\n T u = P_c[0] / P_c[2];\n T v = P_c[1] / P_c[2];\n\n T rho_sqr = u * u + v * v;\n T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;\n T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);\n T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;\n\n u = L * u + du;\n v = L * v + dv;\n p(0) = fx * (u + alpha * v) + cx;\n p(1) = fy * v + cy;\n}\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/camera_models/ScaramuzzaCamera.h", "content": "#ifndef SCARAMUZZACAMERA_H\r\n#define SCARAMUZZACAMERA_H\r\n\r\n#include \r\n#include \r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n#define SCARAMUZZA_POLY_SIZE 5\r\n#define SCARAMUZZA_INV_POLY_SIZE 20\r\n\r\n#define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/)\r\n\r\n/**\r\n * Scaramuzza Camera (Omnidirectional)\r\n * https://sites.google.com/site/scarabotix/ocamcalib-toolbox\r\n */\r\n\r\nclass OCAMCamera: public Camera\r\n{\r\npublic:\r\n class Parameters: public Camera::Parameters\r\n {\r\n public:\r\n Parameters();\r\n\r\n double& C(void) { return m_C; }\r\n double& D(void) { return m_D; }\r\n double& E(void) { return m_E; }\r\n\r\n double& center_x(void) { return m_center_x; }\r\n double& center_y(void) { return m_center_y; }\r\n\r\n double& poly(int idx) { return m_poly[idx]; }\r\n double& inv_poly(int idx) { return m_inv_poly[idx]; }\r\n\r\n double C(void) const { return m_C; }\r\n double D(void) const { return m_D; }\r\n double E(void) const { return m_E; }\r\n\r\n double center_x(void) const { return m_center_x; }\r\n double center_y(void) const { return m_center_y; }\r\n\r\n double poly(int idx) const { return m_poly[idx]; }\r\n double inv_poly(int idx) const { return m_inv_poly[idx]; }\r\n\r\n bool readFromYamlFile(const std::string& filename);\r\n void writeToYamlFile(const std::string& filename) const;\r\n\r\n Parameters& operator=(const Parameters& other);\r\n friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n private:\r\n double m_poly[SCARAMUZZA_POLY_SIZE];\r\n double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n double m_C;\r\n double m_D;\r\n double m_E;\r\n double m_center_x;\r\n double m_center_y;\r\n };\r\n\r\n OCAMCamera();\r\n\r\n /**\r\n * \\brief Constructor from the projection model parameters\r\n */\r\n OCAMCamera(const Parameters& params);\r\n\r\n Camera::ModelType modelType(void) const;\r\n const std::string& cameraName(void) const;\r\n int imageWidth(void) const;\r\n int imageHeight(void) const;\r\n\r\n void estimateIntrinsics(const cv::Size& boardSize,\r\n const std::vector< std::vector >& objectPoints,\r\n const std::vector< std::vector >& imagePoints);\r\n\r\n // Lift points from the image plane to the sphere\r\n void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Lift points from the image plane to the projective space\r\n void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n //%output P\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n // Projects 3D points to the image plane (Pi function)\r\n // and calculates jacobian\r\n //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n // Eigen::Matrix& J) const;\r\n //%output p\r\n //%output J\r\n\r\n void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n //%output p\r\n\r\n template \r\n static void spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p);\r\n template \r\n static void spaceToSphere(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& P_s);\r\n template \r\n static void LiftToSphere(const T* const params,\r\n const Eigen::Matrix& p,\r\n Eigen::Matrix& P);\r\n\r\n template \r\n static void SphereToPlane(const T* const params, const Eigen::Matrix& P,\r\n Eigen::Matrix& p);\r\n\r\n\r\n void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n float fx = -1.0f, float fy = -1.0f,\r\n cv::Size imageSize = cv::Size(0, 0),\r\n float cx = -1.0f, float cy = -1.0f,\r\n cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n int parameterCount(void) const;\r\n\r\n const Parameters& getParameters(void) const;\r\n void setParameters(const Parameters& parameters);\r\n\r\n void readParameters(const std::vector& parameterVec);\r\n void writeParameters(std::vector& parameterVec) const;\r\n\r\n void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n std::string parametersToString(void) const;\r\n\r\nprivate:\r\n Parameters mParameters;\r\n\r\n double m_inv_scale;\r\n};\r\n\r\ntypedef boost::shared_ptr OCAMCameraPtr;\r\ntypedef boost::shared_ptr OCAMCameraConstPtr;\r\n\r\ntemplate \r\nvoid\r\nOCAMCamera::spaceToPlane(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& p)\r\n{\r\n T P_c[3];\r\n {\r\n T P_w[3];\r\n P_w[0] = T(P(0));\r\n P_w[1] = T(P(1));\r\n P_w[2] = T(P(2));\r\n\r\n // Convert quaternion from Eigen convention (x, y, z, w)\r\n // to Ceres convention (w, x, y, z)\r\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n P_c[0] += t[0];\r\n P_c[1] += t[1];\r\n P_c[2] += t[2];\r\n }\r\n\r\n T c = params[0];\r\n T d = params[1];\r\n T e = params[2];\r\n T xc[2] = { params[3], params[4] };\r\n\r\n //T poly[SCARAMUZZA_POLY_SIZE];\r\n //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n // poly[i] = params[5+i];\r\n\r\n T inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];\r\n\r\n T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];\r\n T norm = T(0.0);\r\n if (norm_sqr > T(0.0))\r\n norm = sqrt(norm_sqr);\r\n\r\n T theta = atan2(-P_c[2], norm);\r\n T rho = T(0.0);\r\n T theta_i = T(1.0);\r\n\r\n for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n {\r\n rho += theta_i * inv_poly[i];\r\n theta_i *= theta;\r\n }\r\n\r\n T invNorm = T(1.0) / norm;\r\n T xn[2] = {\r\n P_c[0] * invNorm * rho,\r\n P_c[1] * invNorm * rho\r\n };\r\n\r\n p(0) = xn[0] * c + xn[1] * d + xc[0];\r\n p(1) = xn[0] * e + xn[1] + xc[1];\r\n}\r\n\r\ntemplate \r\nvoid\r\nOCAMCamera::spaceToSphere(const T* const params,\r\n const T* const q, const T* const t,\r\n const Eigen::Matrix& P,\r\n Eigen::Matrix& P_s)\r\n{\r\n T P_c[3];\r\n {\r\n T P_w[3];\r\n P_w[0] = T(P(0));\r\n P_w[1] = T(P(1));\r\n P_w[2] = T(P(2));\r\n\r\n // Convert quaternion from Eigen convention (x, y, z, w)\r\n // to Ceres convention (w, x, y, z)\r\n T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n P_c[0] += t[0];\r\n P_c[1] += t[1];\r\n P_c[2] += t[2];\r\n }\r\n\r\n //T poly[SCARAMUZZA_POLY_SIZE];\r\n //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n // poly[i] = params[5+i];\r\n\r\n T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2];\r\n T norm = T(0.0);\r\n if (norm_sqr > T(0.0))\r\n norm = sqrt(norm_sqr);\r\n\r\n P_s(0) = P_c[0] / norm;\r\n P_s(1) = P_c[1] / norm;\r\n P_s(2) = P_c[2] / norm;\r\n}\r\n\r\ntemplate \r\nvoid\r\nOCAMCamera::LiftToSphere(const T* const params,\r\n const Eigen::Matrix& p,\r\n Eigen::Matrix& P)\r\n{\r\n T c = params[0];\r\n T d = params[1];\r\n T e = params[2];\r\n T cc[2] = { params[3], params[4] };\r\n T poly[SCARAMUZZA_POLY_SIZE];\r\n for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n poly[i] = params[5+i];\r\n\r\n // Relative to Center\r\n T p_2d[2];\r\n p_2d[0] = T(p(0));\r\n p_2d[1] = T(p(1));\r\n\r\n T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]};\r\n\r\n T inv_scale = T(1.0) / (c - d * e);\r\n\r\n // Affine Transformation\r\n T xc_a[2];\r\n\r\n xc_a[0] = inv_scale * (xc[0] - d * xc[1]);\r\n xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]);\r\n\r\n T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1];\r\n T phi = sqrt(norm_sqr);\r\n T phi_i = T(1.0);\r\n T z = T(0.0);\r\n\r\n for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n {\r\n if (i!=1) {\r\n z += phi_i * poly[i];\r\n }\r\n phi_i *= phi;\r\n }\r\n\r\n T p_3d[3];\r\n p_3d[0] = xc[0];\r\n p_3d[1] = xc[1];\r\n p_3d[2] = -z;\r\n\r\n T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2];\r\n T p_3d_norm = sqrt(p_3d_norm_sqr);\r\n\r\n P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm;\r\n}\r\n\r\ntemplate \r\nvoid OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix& P,\r\n Eigen::Matrix& p) {\r\n T P_c[3];\r\n {\r\n P_c[0] = T(P(0));\r\n P_c[1] = T(P(1));\r\n P_c[2] = T(P(2));\r\n }\r\n\r\n T c = params[0];\r\n T d = params[1];\r\n T e = params[2];\r\n T xc[2] = {params[3], params[4]};\r\n\r\n T inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];\r\n\r\n T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];\r\n T norm = T(0.0);\r\n if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr);\r\n\r\n T theta = atan2(-P_c[2], norm);\r\n T rho = T(0.0);\r\n T theta_i = T(1.0);\r\n\r\n for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) {\r\n rho += theta_i * inv_poly[i];\r\n theta_i *= theta;\r\n }\r\n\r\n T invNorm = T(1.0) / norm;\r\n T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho};\r\n\r\n p(0) = xn[0] * c + xn[1] * d + xc[0];\r\n p(1) = xn[0] * e + xn[1] + xc[1];\r\n}\r\n}\r\n\r\n#endif\r\n" }, { "path": "src/utils/camodocal/include/camodocal/chessboard/Chessboard.h", "content": "#ifndef CHESSBOARD_H\n#define CHESSBOARD_H\n\n#include \n#include \n\nnamespace camodocal\n{\n\n// forward declarations\nclass ChessboardCorner;\ntypedef boost::shared_ptr ChessboardCornerPtr;\nclass ChessboardQuad;\ntypedef boost::shared_ptr ChessboardQuadPtr;\n\nclass Chessboard\n{\npublic:\n Chessboard(cv::Size boardSize, cv::Mat& image);\n\n void findCorners(bool useOpenCV = false);\n const std::vector& getCorners(void) const;\n bool cornersFound(void) const;\n\n const cv::Mat& getImage(void) const;\n const cv::Mat& getSketch(void) const;\n\nprivate:\n bool findChessboardCorners(const cv::Mat& image,\n const cv::Size& patternSize,\n std::vector& corners,\n int flags, bool useOpenCV);\n\n bool findChessboardCornersImproved(const cv::Mat& image,\n const cv::Size& patternSize,\n std::vector& corners,\n int flags);\n\n void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize);\n\n void findConnectedQuads(std::vector& quads,\n std::vector& group,\n int group_idx, int dilation);\n\n// int checkQuadGroup(std::vector& quadGroup,\n// std::vector& outCorners,\n// cv::Size patternSize);\n\n void labelQuadGroup(std::vector& quad_group,\n cv::Size patternSize, bool firstRun);\n\n void findQuadNeighbors(std::vector& quads, int dilation);\n\n int augmentBestRun(std::vector& candidateQuads, int candidateDilation,\n std::vector& existingQuads, int existingDilation);\n\n void generateQuads(std::vector& quads,\n cv::Mat& image, int flags,\n int dilation, bool firstRun);\n\n bool checkQuadGroup(std::vector& quads,\n std::vector& corners,\n cv::Size patternSize);\n\n void getQuadrangleHypotheses(const std::vector< std::vector >& contours,\n std::vector< std::pair >& quads,\n int classId) const;\n\n bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const;\n\n bool checkBoardMonotony(std::vector& corners,\n cv::Size patternSize);\n\n bool matchCorners(ChessboardQuadPtr& quad1, int corner1,\n ChessboardQuadPtr& quad2, int corner2) const;\n\n cv::Mat mImage;\n cv::Mat mSketch;\n std::vector mCorners;\n cv::Size mBoardSize;\n bool mCornersFound;\n};\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/chessboard/ChessboardCorner.h", "content": "#ifndef CHESSBOARDCORNER_H\n#define CHESSBOARDCORNER_H\n\n#include \n#include \n\nnamespace camodocal\n{\n\nclass ChessboardCorner;\ntypedef boost::shared_ptr ChessboardCornerPtr;\n\nclass ChessboardCorner\n{\npublic:\n ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {}\n\n float meanDist(int &n) const\n {\n float sum = 0;\n n = 0;\n for (int i = 0; i < 4; ++i)\n {\n if (neighbors[i].get())\n {\n float dx = neighbors[i]->pt.x - pt.x;\n float dy = neighbors[i]->pt.y - pt.y;\n sum += sqrt(dx*dx + dy*dy);\n n++;\n }\n }\n return sum / std::max(n, 1);\n }\n\n cv::Point2f pt; // X and y coordinates\n int row; // Row and column of the corner\n int column; // in the found pattern\n bool needsNeighbor; // Does the corner require a neighbor?\n int count; // number of corner neighbors\n ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors\n};\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/chessboard/ChessboardQuad.h", "content": "#ifndef CHESSBOARDQUAD_H\n#define CHESSBOARDQUAD_H\n\n#include \n\n#include \"camodocal/chessboard/ChessboardCorner.h\"\n\nnamespace camodocal\n{\n\nclass ChessboardQuad;\ntypedef boost::shared_ptr ChessboardQuadPtr;\n\nclass ChessboardQuad\n{\npublic:\n ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {}\n\n int count; // Number of quad neighbors\n int group_idx; // Quad group ID\n float edge_len; // Smallest side length^2\n ChessboardCornerPtr corners[4]; // Coordinates of quad corners\n ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors\n bool labeled; // Has this corner been labeled?\n};\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/chessboard/Spline.h", "content": "/* dynamo:- Event driven molecular dynamics simulator\n http://www.marcusbannerman.co.uk/dynamo\n Copyright (C) 2011 Marcus N Campbell Bannerman \n\n This program is free software: you can redistribute it and/or\n modify it under the terms of the GNU General Public License\n version 3 as published by the Free Software Foundation.\n\n This program is distributed in the hope that it will be useful,\n but WITHOUT ANY WARRANTY; without even the implied warranty of\n MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n GNU General Public License for more details.\n\n You should have received a copy of the GNU General Public License\n along with this program. If not, see .\n*/\n\n#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace ublas = boost::numeric::ublas;\n\nclass Spline : private std::vector >\n{\npublic:\n //The boundary conditions available\n enum BC_type {\n\tFIXED_1ST_DERIV_BC,\n\tFIXED_2ND_DERIV_BC,\n\tPARABOLIC_RUNOUT_BC\n };\n\n enum Spline_type {\n\tLINEAR,\n\tCUBIC\n };\n\n //Constructor takes the boundary conditions as arguments, this\n //sets the first derivative (gradient) at the lower and upper\n //end points\n Spline():\n\t_valid(false),\n\t_BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC),\n\t_BCLowVal(0), _BCHighVal(0),\n\t_type(CUBIC)\n {}\n\n typedef std::vector > base;\n typedef base::const_iterator const_iterator;\n\n //Standard STL read-only container stuff\n const_iterator begin() const { return base::begin(); }\n const_iterator end() const { return base::end(); }\n void clear() { _valid = false; base::clear(); _data.clear(); }\n size_t size() const { return base::size(); }\n size_t max_size() const { return base::max_size(); }\n size_t capacity() const { return base::capacity(); }\n bool empty() const { return base::empty(); }\n\n //Add a point to the spline, and invalidate it so its\n //recalculated on the next access\n inline void addPoint(double x, double y)\n {\n\t_valid = false;\n\tbase::push_back(std::pair(x,y));\n }\n\n //Reset the boundary conditions\n inline void setLowBC(BC_type BC, double val = 0)\n { _BCLow = BC; _BCLowVal = val; _valid = false; }\n\n inline void setHighBC(BC_type BC, double val = 0)\n { _BCHigh = BC; _BCHighVal = val; _valid = false; }\n\n void setType(Spline_type type) { _type = type; _valid = false; }\n\n //Check if the spline has been calculated, then generate the\n //spline interpolated value\n double operator()(double xval)\n {\n\tif (!_valid) generate();\n\n\t//Special cases when we're outside the range of the spline points\n\tif (xval <= x(0)) return lowCalc(xval);\n\tif (xval >= x(size()-1)) return highCalc(xval);\n\n\t//Check all intervals except the last one\n\tfor (std::vector::const_iterator iPtr = _data.begin();\n\t\t iPtr != _data.end()-1; ++iPtr)\n\t\tif ((xval >= iPtr->x) && (xval <= (iPtr+1)->x))\n\t\t return splineCalc(iPtr, xval);\n\n\treturn splineCalc(_data.end() - 1, xval);\n }\n\nprivate:\n\n ///////PRIVATE DATA MEMBERS\n struct SplineData { double x,a,b,c,d; };\n //vector of calculated spline data\n std::vector _data;\n //Second derivative at each point\n ublas::vector _ddy;\n //Tracks whether the spline parameters have been calculated for\n //the current set of points\n bool _valid;\n //The boundary conditions\n BC_type _BCLow, _BCHigh;\n //The values of the boundary conditions\n double _BCLowVal, _BCHighVal;\n\n Spline_type _type;\n\n ///////PRIVATE FUNCTIONS\n //Function to calculate the value of a given spline at a point xval\n inline double splineCalc(std::vector::const_iterator i, double xval)\n {\n\tconst double lx = xval - i->x;\n\treturn ((i->a * lx + i->b) * lx + i->c) * lx + i->d;\n }\n\n inline double lowCalc(double xval)\n {\n\tconst double lx = xval - x(0);\n\n\tif (_type == LINEAR)\n\t return lx * _BCHighVal + y(0);\n\n\tconst double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6;\n\n\tswitch(_BCLow)\n\t {\n\t case FIXED_1ST_DERIV_BC:\n\t\treturn lx * _BCLowVal + y(0);\n\t case FIXED_2ND_DERIV_BC:\n\t\t return lx * lx * _BCLowVal + firstDeriv * lx + y(0);\n\t case PARABOLIC_RUNOUT_BC:\n\t\treturn lx * lx * _ddy[0] + lx * firstDeriv + y(0);\n\t }\n\tthrow std::runtime_error(\"Unknown BC\");\n }\n\n inline double highCalc(double xval)\n {\n\tconst double lx = xval - x(size() - 1);\n\n\tif (_type == LINEAR)\n\t return lx * _BCHighVal + y(size() - 1);\n\n\tconst double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2);\n\n\tswitch(_BCHigh)\n\t {\n\t case FIXED_1ST_DERIV_BC:\n\t\treturn lx * _BCHighVal + y(size() - 1);\n\t case FIXED_2ND_DERIV_BC:\n\t\treturn lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1);\n\t case PARABOLIC_RUNOUT_BC:\n\t\treturn lx * lx * _ddy[size()-1] + lx * firstDeriv + y(size() - 1);\n\t }\n\tthrow std::runtime_error(\"Unknown BC\");\n }\n\n //These just provide access to the point data in a clean way\n inline double x(size_t i) const { return operator[](i).first; }\n inline double y(size_t i) const { return operator[](i).second; }\n inline double h(size_t i) const { return x(i+1) - x(i); }\n\n //Invert a arbitrary matrix using the boost ublas library\n template\n bool InvertMatrix(ublas::matrix A,\n\t\tublas::matrix& inverse)\n {\n\tusing namespace ublas;\n\n\t// create a permutation matrix for the LU-factorization\n\tpermutation_matrix pm(A.size1());\n\n\t// perform LU-factorization\n\tint res = lu_factorize(A,pm);\n\t\tif( res != 0 ) return false;\n\n\t// create identity matrix of \"inverse\"\n\tinverse.assign(ublas::identity_matrix(A.size1()));\n\n\t// backsubstitute to get the inverse\n\tlu_substitute(A, pm, inverse);\n\n\treturn true;\n }\n\n //This function will recalculate the spline parameters and store\n //them in _data, ready for spline interpolation\n void generate()\n {\n\tif (size() < 2)\n\t throw std::runtime_error(\"Spline requires at least 2 points\");\n\n\t//If any spline points are at the same x location, we have to\n\t//just slightly seperate them\n\t{\n\t bool testPassed(false);\n\t while (!testPassed)\n\t\t{\n\t\t testPassed = true;\n\t\t std::sort(base::begin(), base::end());\n\n\t\t for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr)\n\t\tif (iPtr->first == (iPtr+1)->first)\n\t\t {\n\t\t\tif ((iPtr+1)->first != 0)\n\t\t\t (iPtr+1)->first += (iPtr+1)->first\n\t\t\t* std::numeric_limits::epsilon() * 10;\n\t\t\telse\n\t\t\t (iPtr+1)->first = std::numeric_limits::epsilon() * 10;\n\t\t\ttestPassed = false;\n\t\t\tbreak;\n\t\t }\n\t\t}\n\t}\n\n\tconst size_t e = size() - 1;\n\n\tswitch (_type)\n\t {\n\t case LINEAR:\n\t\t{\n\t\t _data.resize(e);\n\t\t for (size_t i(0); i < e; ++i)\n\t\t{\n\t\t _data[i].x = x(i);\n\t\t _data[i].a = 0;\n\t\t _data[i].b = 0;\n\t\t _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i));\n\t\t _data[i].d = y(i);\n\t\t}\n\t\t break;\n\t\t}\n\t case CUBIC:\n\t\t{\n\t\t ublas::matrix A(size(), size());\n\t\t for (size_t yv(0); yv <= e; ++yv)\n\t\tfor (size_t xv(0); xv <= e; ++xv)\n\t\t A(xv,yv) = 0;\n\n\t\t for (size_t i(1); i < e; ++i)\n\t\t{\n\t\t A(i-1,i) = h(i-1);\n\t\t A(i,i) = 2 * (h(i-1) + h(i));\n\t\t A(i+1,i) = h(i);\n\t\t}\n\n\t\t ublas::vector C(size());\n\t\t for (size_t xv(0); xv <= e; ++xv)\n\t\tC(xv) = 0;\n\n\t\t for (size_t i(1); i < e; ++i)\n\t\tC(i) = 6 *\n\t\t ((y(i+1) - y(i)) / h(i)\n\t\t - (y(i) - y(i-1)) / h(i-1));\n\n\t\t //Boundary conditions\n\t\t switch(_BCLow)\n\t\t{\n\t\tcase FIXED_1ST_DERIV_BC:\n\t\t C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal);\n\t\t A(0,0) = 2 * h(0);\n\t\t A(1,0) = h(0);\n\t\t break;\n\t\tcase FIXED_2ND_DERIV_BC:\n\t\t C(0) = _BCLowVal;\n\t\t A(0,0) = 1;\n\t\t break;\n\t\tcase PARABOLIC_RUNOUT_BC:\n\t\t C(0) = 0; A(0,0) = 1; A(1,0) = -1;\n\t\t break;\n\t\t}\n\n\t\t switch(_BCHigh)\n\t\t{\n\t\tcase FIXED_1ST_DERIV_BC:\n\t\t C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1));\n\t\t A(e,e) = 2 * h(e - 1);\n\t\t A(e-1,e) = h(e - 1);\n\t\t break;\n\t\tcase FIXED_2ND_DERIV_BC:\n\t\t C(e) = _BCHighVal;\n\t\t A(e,e) = 1;\n\t\t break;\n\t\tcase PARABOLIC_RUNOUT_BC:\n\t\t C(e) = 0; A(e,e) = 1; A(e-1,e) = -1;\n\t\t break;\n\t\t}\n\n\t\t ublas::matrix AInv(size(), size());\n\t\t InvertMatrix(A,AInv);\n\n\t\t _ddy = ublas::prod(C, AInv);\n\n\t\t _data.resize(size()-1);\n\t\t for (size_t i(0); i < e; ++i)\n\t\t{\n\t\t _data[i].x = x(i);\n\t\t _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i));\n\t\t _data[i].b = _ddy(i) / 2;\n\t\t _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3;\n\t\t _data[i].d = y(i);\n\t\t}\n\t\t}\n\t }\n\t_valid = true;\n }\n};\n" }, { "path": "src/utils/camodocal/include/camodocal/gpl/EigenQuaternionParameterization.h", "content": "#ifndef EIGENQUATERNIONPARAMETERIZATION_H\n#define EIGENQUATERNIONPARAMETERIZATION_H\n\n#include \"ceres/local_parameterization.h\"\n\nnamespace camodocal\n{\n\nclass EigenQuaternionParameterization : public ceres::LocalParameterization\n{\npublic:\n virtual ~EigenQuaternionParameterization() {}\n virtual bool Plus(const double* x,\n const double* delta,\n double* x_plus_delta) const;\n virtual bool ComputeJacobian(const double* x,\n double* jacobian) const;\n virtual int GlobalSize() const { return 4; }\n virtual int LocalSize() const { return 3; }\n\nprivate:\n template\n void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const;\n};\n\n\ntemplate\nvoid\nEigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const\n{\n zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1];\n zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0];\n zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3];\n zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2];\n}\n\n}\n\n#endif\n\n" }, { "path": "src/utils/camodocal/include/camodocal/gpl/EigenUtils.h", "content": "#ifndef EIGENUTILS_H\n#define EIGENUTILS_H\n\n#include \n\n#include \"ceres/rotation.h\"\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\n// Returns the 3D cross product skew symmetric matrix of a given 3D vector\ntemplate\nEigen::Matrix skew(const Eigen::Matrix& vec)\n{\n return (Eigen::Matrix() << T(0), -vec(2), vec(1),\n vec(2), T(0), -vec(0),\n -vec(1), vec(0), T(0)).finished();\n}\n\ntemplate\ntypename Eigen::MatrixBase::PlainObject sqrtm(const Eigen::MatrixBase& A)\n{\n Eigen::SelfAdjointEigenSolver es(A);\n\n return es.operatorSqrt();\n}\n\ntemplate\nEigen::Matrix AngleAxisToRotationMatrix(const Eigen::Matrix& rvec)\n{\n T angle = rvec.norm();\n if (angle == T(0))\n {\n return Eigen::Matrix::Identity();\n }\n\n Eigen::Matrix axis;\n axis = rvec.normalized();\n\n Eigen::Matrix rmat;\n rmat = Eigen::AngleAxis(angle, axis);\n\n return rmat;\n}\n\ntemplate\nEigen::Quaternion AngleAxisToQuaternion(const Eigen::Matrix& rvec)\n{\n Eigen::Matrix rmat = AngleAxisToRotationMatrix(rvec);\n\n return Eigen::Quaternion(rmat);\n}\n\ntemplate\nvoid AngleAxisToQuaternion(const Eigen::Matrix& rvec, T* q)\n{\n Eigen::Quaternion quat = AngleAxisToQuaternion(rvec);\n\n q[0] = quat.x();\n q[1] = quat.y();\n q[2] = quat.z();\n q[3] = quat.w();\n}\n\ntemplate\nEigen::Matrix RotationToAngleAxis(const Eigen::Matrix & rmat)\n{\n Eigen::AngleAxis angleaxis; \n angleaxis.fromRotationMatrix(rmat); \n return angleaxis.angle() * angleaxis.axis(); \n \n}\n\ntemplate\nvoid QuaternionToAngleAxis(const T* const q, Eigen::Matrix& rvec)\n{\n Eigen::Quaternion quat(q[3], q[0], q[1], q[2]);\n\n Eigen::Matrix rmat = quat.toRotationMatrix();\n\n Eigen::AngleAxis angleaxis;\n angleaxis.fromRotationMatrix(rmat);\n\n rvec = angleaxis.angle() * angleaxis.axis();\n}\n\ntemplate\nEigen::Matrix QuaternionToRotation(const T* const q)\n{\n T R[9];\n ceres::QuaternionToRotation(q, R);\n\n Eigen::Matrix rmat;\n for (int i = 0; i < 3; ++i)\n {\n for (int j = 0; j < 3; ++j)\n {\n rmat(i,j) = R[i * 3 + j];\n }\n }\n\n return rmat;\n}\n\ntemplate\nvoid QuaternionToRotation(const T* const q, T* rot)\n{\n ceres::QuaternionToRotation(q, rot);\n}\n\ntemplate\nEigen::Matrix QuaternionMultMatLeft(const Eigen::Quaternion& q)\n{\n return (Eigen::Matrix() << q.w(), -q.z(), q.y(), q.x(),\n q.z(), q.w(), -q.x(), q.y(),\n -q.y(), q.x(), q.w(), q.z(),\n -q.x(), -q.y(), -q.z(), q.w()).finished();\n}\n\ntemplate\nEigen::Matrix QuaternionMultMatRight(const Eigen::Quaternion& q)\n{\n return (Eigen::Matrix() << q.w(), q.z(), -q.y(), q.x(),\n -q.z(), q.w(), q.x(), q.y(),\n q.y(), -q.x(), q.w(), q.z(),\n -q.x(), -q.y(), -q.z(), q.w()).finished();\n}\n\n/// @param theta - rotation about screw axis\n/// @param d - projection of tvec on the rotation axis\n/// @param l - screw axis direction\n/// @param m - screw axis moment\ntemplate\nvoid AngleAxisAndTranslationToScrew(const Eigen::Matrix& rvec,\n const Eigen::Matrix& tvec,\n T& theta, T& d,\n Eigen::Matrix& l,\n Eigen::Matrix& m)\n{\n\n theta = rvec.norm();\n if (theta == 0)\n {\n l.setZero(); \n m.setZero(); \n std::cout << \"Warning: Undefined screw! Returned 0. \" << std::endl; \n }\n\n l = rvec.normalized();\n\n Eigen::Matrix t = tvec;\n\n d = t.transpose() * l;\n\n // point on screw axis - projection of origin on screw axis\n Eigen::Matrix c;\n c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t));\n\n // c and hence the screw axis is not defined if theta is either 0 or M_PI\n m = c.cross(l);\n}\n\ntemplate\nEigen::Matrix RPY2mat(T roll, T pitch, T yaw)\n{\n Eigen::Matrix m;\n\n T cr = cos(roll);\n T sr = sin(roll);\n T cp = cos(pitch);\n T sp = sin(pitch);\n T cy = cos(yaw);\n T sy = sin(yaw);\n\n m(0,0) = cy * cp;\n m(0,1) = cy * sp * sr - sy * cr;\n m(0,2) = cy * sp * cr + sy * sr;\n m(1,0) = sy * cp;\n m(1,1) = sy * sp * sr + cy * cr;\n m(1,2) = sy * sp * cr - cy * sr;\n m(2,0) = - sp;\n m(2,1) = cp * sr;\n m(2,2) = cp * cr;\n return m; \n}\n\ntemplate\nvoid mat2RPY(const Eigen::Matrix& m, T& roll, T& pitch, T& yaw)\n{\n roll = atan2(m(2,1), m(2,2));\n pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2)));\n yaw = atan2(m(1,0), m(0,0));\n}\n\ntemplate\nEigen::Matrix homogeneousTransform(const Eigen::Matrix& R, const Eigen::Matrix& t)\n{\n Eigen::Matrix H;\n H.setIdentity();\n\n H.block(0,0,3,3) = R;\n H.block(0,3,3,1) = t;\n\n return H;\n}\n\ntemplate\nEigen::Matrix poseWithCartesianTranslation(const T* const q, const T* const p)\n{\n Eigen::Matrix pose = Eigen::Matrix::Identity();\n\n T rotation[9];\n ceres::QuaternionToRotation(q, rotation);\n for (int i = 0; i < 3; ++i)\n {\n for (int j = 0; j < 3; ++j)\n {\n pose(i,j) = rotation[i * 3 + j];\n }\n }\n\n pose(0,3) = p[0];\n pose(1,3) = p[1];\n pose(2,3) = p[2];\n\n return pose;\n}\n\ntemplate\nEigen::Matrix poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0))\n{\n Eigen::Matrix pose = Eigen::Matrix::Identity();\n\n T rotation[9];\n ceres::QuaternionToRotation(q, rotation);\n for (int i = 0; i < 3; ++i)\n {\n for (int j = 0; j < 3; ++j)\n {\n pose(i,j) = rotation[i * 3 + j];\n }\n }\n\n T theta = p[0];\n T phi = p[1];\n pose(0,3) = sin(theta) * cos(phi) * scale;\n pose(1,3) = sin(theta) * sin(phi) * scale;\n pose(2,3) = cos(theta) * scale;\n\n return pose;\n}\n\n// Returns the Sampson error of a given essential matrix and 2 image points\ntemplate\nT sampsonError(const Eigen::Matrix& E,\n const Eigen::Matrix& p1,\n const Eigen::Matrix& p2)\n{\n Eigen::Matrix Ex1 = E * p1;\n Eigen::Matrix Etx2 = E.transpose() * p2;\n\n T x2tEx1 = p2.dot(Ex1);\n\n // compute Sampson error\n T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));\n\n return err;\n}\n\n// Returns the Sampson error of a given rotation/translation and 2 image points\ntemplate\nT sampsonError(const Eigen::Matrix& R,\n const Eigen::Matrix& t,\n const Eigen::Matrix& p1,\n const Eigen::Matrix& p2)\n{\n // construct essential matrix\n Eigen::Matrix E = skew(t) * R;\n\n Eigen::Matrix Ex1 = E * p1;\n Eigen::Matrix Etx2 = E.transpose() * p2;\n\n T x2tEx1 = p2.dot(Ex1);\n\n // compute Sampson error\n T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));\n\n return err;\n}\n\n// Returns the Sampson error of a given rotation/translation and 2 image points\ntemplate\nT sampsonError(const Eigen::Matrix& H,\n const Eigen::Matrix& p1,\n const Eigen::Matrix& p2)\n{\n Eigen::Matrix R = H.block(0, 0, 3, 3);\n Eigen::Matrix t = H.block(0, 3, 3, 1);\n\n return sampsonError(R, t, p1, p2);\n}\n\ntemplate\nEigen::Matrix\ntransformPoint(const Eigen::Matrix& H, const Eigen::Matrix& P)\n{\n Eigen::Matrix P_trans = H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1);\n\n return P_trans;\n}\n\ntemplate\nEigen::Matrix\nestimate3DRigidTransform(const std::vector, Eigen::aligned_allocator > >& points1,\n const std::vector, Eigen::aligned_allocator > >& points2)\n{\n // compute centroids\n Eigen::Matrix c1, c2;\n c1.setZero(); c2.setZero();\n\n for (size_t i = 0; i < points1.size(); ++i)\n {\n c1 += points1.at(i);\n c2 += points2.at(i);\n }\n\n c1 /= points1.size();\n c2 /= points1.size();\n\n Eigen::Matrix X(3, points1.size());\n Eigen::Matrix Y(3, points1.size());\n for (size_t i = 0; i < points1.size(); ++i)\n {\n X.col(i) = points1.at(i) - c1;\n Y.col(i) = points2.at(i) - c2;\n }\n\n Eigen::Matrix H = X * Y.transpose();\n\n Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);\n\n Eigen::Matrix U = svd.matrixU();\n Eigen::Matrix V = svd.matrixV();\n if (U.determinant() * V.determinant() < 0.0)\n {\n V.col(2) *= -1.0;\n }\n\n Eigen::Matrix R = V * U.transpose();\n Eigen::Matrix t = c2 - R * c1;\n\n return homogeneousTransform(R, t);\n}\n\ntemplate\nEigen::Matrix\nestimate3DRigidSimilarityTransform(const std::vector, Eigen::aligned_allocator > >& points1,\n const std::vector, Eigen::aligned_allocator > >& points2)\n{\n // compute centroids\n Eigen::Matrix c1, c2;\n c1.setZero(); c2.setZero();\n\n for (size_t i = 0; i < points1.size(); ++i)\n {\n c1 += points1.at(i);\n c2 += points2.at(i);\n }\n\n c1 /= points1.size();\n c2 /= points1.size();\n\n Eigen::Matrix X(3, points1.size());\n Eigen::Matrix Y(3, points1.size());\n for (size_t i = 0; i < points1.size(); ++i)\n {\n X.col(i) = points1.at(i) - c1;\n Y.col(i) = points2.at(i) - c2;\n }\n\n Eigen::Matrix H = X * Y.transpose();\n\n Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);\n\n Eigen::Matrix U = svd.matrixU();\n Eigen::Matrix V = svd.matrixV();\n if (U.determinant() * V.determinant() < 0.0)\n {\n V.col(2) *= -1.0;\n }\n\n Eigen::Matrix R = V * U.transpose();\n\n std::vector, Eigen::aligned_allocator > > rotatedPoints1(points1.size());\n for (size_t i = 0; i < points1.size(); ++i)\n {\n rotatedPoints1.at(i) = R * (points1.at(i) - c1);\n }\n\n double sum_ss = 0.0, sum_tt = 0.0;\n for (size_t i = 0; i < points1.size(); ++i)\n {\n sum_ss += (points1.at(i) - c1).squaredNorm();\n sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i));\n }\n\n double scale = sum_tt / sum_ss;\n\n Eigen::Matrix sR = scale * R;\n Eigen::Matrix t = c2 - sR * c1;\n\n return homogeneousTransform(sR, t);\n}\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/include/camodocal/gpl/gpl.h", "content": "#ifndef GPL_H\r\n#define GPL_H\r\n\r\n#include \r\n#include \r\n#include \r\n\r\nnamespace camodocal\r\n{\r\n\r\ntemplate\r\nconst T clamp(const T& v, const T& a, const T& b)\r\n{\r\n\treturn std::min(b, std::max(a, v));\r\n}\r\n\r\ndouble hypot3(double x, double y, double z);\r\nfloat hypot3f(float x, float y, float z);\r\n\r\ntemplate\r\nconst T normalizeTheta(const T& theta)\r\n{\r\n\tT normTheta = theta;\r\n\r\n\twhile (normTheta < - M_PI)\r\n\t{\r\n\t\tnormTheta += 2.0 * M_PI;\r\n\t}\r\n\twhile (normTheta > M_PI)\r\n\t{\r\n\t\tnormTheta -= 2.0 * M_PI;\r\n\t}\r\n\r\n\treturn normTheta;\r\n}\r\n\r\ndouble d2r(double deg);\r\nfloat d2r(float deg);\r\ndouble r2d(double rad);\r\nfloat r2d(float rad);\r\n\r\ndouble sinc(double theta);\r\n\r\ntemplate\r\nconst T square(const T& x)\r\n{\r\n\treturn x * x;\r\n}\r\n\r\ntemplate\r\nconst T cube(const T& x)\r\n{\r\n\treturn x * x * x;\r\n}\r\n\r\ntemplate\r\nconst T random(const T& a, const T& b)\r\n{\r\n\treturn static_cast(rand()) / RAND_MAX * (b - a) + a;\r\n}\r\n\r\ntemplate\r\nconst T randomNormal(const T& sigma)\r\n{\r\n T x1, x2, w;\r\n\r\n do\r\n {\r\n x1 = 2.0 * random(0.0, 1.0) - 1.0;\r\n x2 = 2.0 * random(0.0, 1.0) - 1.0;\r\n w = x1 * x1 + x2 * x2;\r\n }\r\n while (w >= 1.0 || w == 0.0);\r\n\r\n w = sqrt((-2.0 * log(w)) / w);\r\n\r\n return x1 * w * sigma;\r\n}\r\n\r\nunsigned long long timeInMicroseconds(void);\r\n\r\ndouble timeInSeconds(void);\r\n\r\nvoid colorDepthImage(cv::Mat& imgDepth,\r\n cv::Mat& imgColoredDepth,\r\n float minRange, float maxRange);\r\n\r\nbool colormap(const std::string& name, unsigned char idx,\r\n float& r, float& g, float& b);\r\n\r\nstd::vector bresLine(int x0, int y0, int x1, int y1);\r\nstd::vector bresCircle(int x0, int y0, int r);\r\n\r\nvoid fitCircle(const std::vector& points,\r\n double& centerX, double& centerY, double& radius);\r\n\r\nstd::vector intersectCircles(double x1, double y1, double r1,\r\n double x2, double y2, double r2);\r\n\r\nvoid LLtoUTM(double latitude, double longitude,\r\n double& utmNorthing, double& utmEasting,\r\n std::string& utmZone);\r\nvoid UTMtoLL(double utmNorthing, double utmEasting,\r\n const std::string& utmZone,\r\n double& latitude, double& longitude);\r\n\r\nlong int timestampDiff(uint64_t t1, uint64_t t2);\r\n\r\n}\r\n\r\n#endif\r\n" }, { "path": "src/utils/camodocal/include/camodocal/sparse_graph/Transform.h", "content": "#ifndef TRANSFORM_H\n#define TRANSFORM_H\n\n#include \n#include \n#include \n\nnamespace camodocal\n{\n\nclass Transform\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Transform();\n Transform(const Eigen::Matrix4d& H);\n\n Eigen::Quaterniond& rotation(void);\n const Eigen::Quaterniond& rotation(void) const;\n double* rotationData(void);\n const double* const rotationData(void) const;\n\n Eigen::Vector3d& translation(void);\n const Eigen::Vector3d& translation(void) const;\n double* translationData(void);\n const double* const translationData(void) const;\n\n Eigen::Matrix4d toMatrix(void) const;\n\nprivate:\n Eigen::Quaterniond m_q;\n Eigen::Vector3d m_t;\n};\n\n}\n\n#endif\n" }, { "path": "src/utils/camodocal/src/calib/CameraCalibration.cc", "content": "#include \"camodocal/calib/CameraCalibration.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/sparse_graph/Transform.h\"\n#include \"camodocal/gpl/EigenQuaternionParameterization.h\"\n#include \"camodocal/gpl/EigenUtils.h\"\n#include \"camodocal/camera_models/CostFunctionFactory.h\"\n\n#include \"ceres/ceres.h\"\nnamespace camodocal\n{\n\nCameraCalibration::CameraCalibration()\n : m_boardSize(cv::Size(0,0))\n , m_squareSize(0.0f)\n , m_verbose(false)\n{\n\n}\n\nCameraCalibration::CameraCalibration(const Camera::ModelType modelType,\n const std::string& cameraName,\n const cv::Size& imageSize,\n const cv::Size& boardSize,\n float squareSize)\n : m_boardSize(boardSize)\n , m_squareSize(squareSize)\n , m_verbose(false)\n{\n m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, imageSize);\n}\n\nvoid\nCameraCalibration::clear(void)\n{\n m_imagePoints.clear();\n m_scenePoints.clear();\n}\n\nvoid\nCameraCalibration::addChessboardData(const std::vector& corners)\n{\n m_imagePoints.push_back(corners);\n\n std::vector scenePointsInView;\n for (int i = 0; i < m_boardSize.height; ++i)\n {\n for (int j = 0; j < m_boardSize.width; ++j)\n {\n scenePointsInView.push_back(cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0));\n }\n }\n m_scenePoints.push_back(scenePointsInView);\n}\n\nbool\nCameraCalibration::calibrate(void)\n{\n int imageCount = m_imagePoints.size();\n\n // compute intrinsic camera parameters and extrinsic parameters for each of the views\n std::vector rvecs;\n std::vector tvecs;\n bool ret = calibrateHelper(m_camera, rvecs, tvecs);\n\n m_cameraPoses = cv::Mat(imageCount, 6, CV_64F);\n for (int i = 0; i < imageCount; ++i)\n {\n m_cameraPoses.at(i,0) = rvecs.at(i).at(0);\n m_cameraPoses.at(i,1) = rvecs.at(i).at(1);\n m_cameraPoses.at(i,2) = rvecs.at(i).at(2);\n m_cameraPoses.at(i,3) = tvecs.at(i).at(0);\n m_cameraPoses.at(i,4) = tvecs.at(i).at(1);\n m_cameraPoses.at(i,5) = tvecs.at(i).at(2);\n }\n\n // Compute measurement covariance.\n std::vector > errVec(m_imagePoints.size());\n Eigen::Vector2d errSum = Eigen::Vector2d::Zero();\n size_t errCount = 0;\n for (size_t i = 0; i < m_imagePoints.size(); ++i)\n {\n std::vector estImagePoints;\n m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),\n estImagePoints);\n\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n cv::Point2f pObs = m_imagePoints.at(i).at(j);\n cv::Point2f pEst = estImagePoints.at(j);\n\n cv::Point2f err = pObs - pEst;\n\n errVec.at(i).push_back(err);\n\n errSum += Eigen::Vector2d(err.x, err.y);\n }\n\n errCount += m_imagePoints.at(i).size();\n }\n\n Eigen::Vector2d errMean = errSum / static_cast(errCount);\n\n Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero();\n for (size_t i = 0; i < errVec.size(); ++i)\n {\n for (size_t j = 0; j < errVec.at(i).size(); ++j)\n {\n cv::Point2f err = errVec.at(i).at(j);\n double d0 = err.x - errMean(0);\n double d1 = err.y - errMean(1);\n\n measurementCovariance(0,0) += d0 * d0;\n measurementCovariance(0,1) += d0 * d1;\n measurementCovariance(1,1) += d1 * d1;\n }\n }\n measurementCovariance /= static_cast(errCount);\n measurementCovariance(1,0) = measurementCovariance(0,1);\n\n m_measurementCovariance = measurementCovariance;\n\n return ret;\n}\n\nint\nCameraCalibration::sampleCount(void) const\n{\n return m_imagePoints.size();\n}\n\nstd::vector >&\nCameraCalibration::imagePoints(void)\n{\n return m_imagePoints;\n}\n\nconst std::vector >&\nCameraCalibration::imagePoints(void) const\n{\n return m_imagePoints;\n}\n\nstd::vector >&\nCameraCalibration::scenePoints(void)\n{\n return m_scenePoints;\n}\n\nconst std::vector >&\nCameraCalibration::scenePoints(void) const\n{\n return m_scenePoints;\n}\n\nCameraPtr&\nCameraCalibration::camera(void)\n{\n return m_camera;\n}\n\nconst CameraConstPtr\nCameraCalibration::camera(void) const\n{\n return m_camera;\n}\n\nEigen::Matrix2d&\nCameraCalibration::measurementCovariance(void)\n{\n return m_measurementCovariance;\n}\n\nconst Eigen::Matrix2d&\nCameraCalibration::measurementCovariance(void) const\n{\n return m_measurementCovariance;\n}\n\ncv::Mat&\nCameraCalibration::cameraPoses(void)\n{\n return m_cameraPoses;\n}\n\nconst cv::Mat&\nCameraCalibration::cameraPoses(void) const\n{\n return m_cameraPoses;\n}\n\nvoid\nCameraCalibration::drawResults(std::vector& images) const\n{\n std::vector rvecs, tvecs;\n\n for (size_t i = 0; i < images.size(); ++i)\n {\n cv::Mat rvec(3, 1, CV_64F);\n rvec.at(0) = m_cameraPoses.at(i,0);\n rvec.at(1) = m_cameraPoses.at(i,1);\n rvec.at(2) = m_cameraPoses.at(i,2);\n\n cv::Mat tvec(3, 1, CV_64F);\n tvec.at(0) = m_cameraPoses.at(i,3);\n tvec.at(1) = m_cameraPoses.at(i,4);\n tvec.at(2) = m_cameraPoses.at(i,5);\n\n rvecs.push_back(rvec);\n tvecs.push_back(tvec);\n }\n\n int drawShiftBits = 4;\n int drawMultiplier = 1 << drawShiftBits;\n\n cv::Scalar green(0, 255, 0);\n cv::Scalar red(0, 0, 255);\n\n for (size_t i = 0; i < images.size(); ++i)\n {\n cv::Mat& image = images.at(i);\n if (image.channels() == 1)\n {\n cv::cvtColor(image, image, CV_GRAY2RGB);\n }\n\n std::vector estImagePoints;\n m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),\n estImagePoints);\n\n float errorSum = 0.0f;\n float errorMax = std::numeric_limits::min();\n\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n cv::Point2f pObs = m_imagePoints.at(i).at(j);\n cv::Point2f pEst = estImagePoints.at(j);\n\n cv::circle(image,\n cv::Point(cvRound(pObs.x * drawMultiplier),\n cvRound(pObs.y * drawMultiplier)),\n 5, green, 2, CV_AA, drawShiftBits);\n\n cv::circle(image,\n cv::Point(cvRound(pEst.x * drawMultiplier),\n cvRound(pEst.y * drawMultiplier)),\n 5, red, 2, CV_AA, drawShiftBits);\n\n float error = cv::norm(pObs - pEst);\n\n errorSum += error;\n if (error > errorMax)\n {\n errorMax = error;\n }\n }\n\n std::ostringstream oss;\n oss << \"Reprojection error: avg = \" << errorSum / m_imagePoints.at(i).size()\n << \" max = \" << errorMax;\n\n cv::putText(image, oss.str(), cv::Point(10, image.rows - 10),\n cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),\n 1, CV_AA);\n }\n}\n\nvoid\nCameraCalibration::writeParams(const std::string& filename) const\n{\n m_camera->writeParametersToYamlFile(filename);\n}\n\nbool\nCameraCalibration::writeChessboardData(const std::string& filename) const\n{\n std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary);\n if (!ofs.is_open())\n {\n return false;\n }\n\n writeData(ofs, m_boardSize.width);\n writeData(ofs, m_boardSize.height);\n writeData(ofs, m_squareSize);\n\n writeData(ofs, m_measurementCovariance(0,0));\n writeData(ofs, m_measurementCovariance(0,1));\n writeData(ofs, m_measurementCovariance(1,0));\n writeData(ofs, m_measurementCovariance(1,1));\n\n writeData(ofs, m_cameraPoses.rows);\n writeData(ofs, m_cameraPoses.cols);\n writeData(ofs, m_cameraPoses.type());\n for (int i = 0; i < m_cameraPoses.rows; ++i)\n {\n for (int j = 0; j < m_cameraPoses.cols; ++j)\n {\n writeData(ofs, m_cameraPoses.at(i,j));\n }\n }\n\n writeData(ofs, m_imagePoints.size());\n for (size_t i = 0; i < m_imagePoints.size(); ++i)\n {\n writeData(ofs, m_imagePoints.at(i).size());\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n const cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n\n writeData(ofs, ipt.x);\n writeData(ofs, ipt.y);\n }\n }\n\n writeData(ofs, m_scenePoints.size());\n for (size_t i = 0; i < m_scenePoints.size(); ++i)\n {\n writeData(ofs, m_scenePoints.at(i).size());\n for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j)\n {\n const cv::Point3f& spt = m_scenePoints.at(i).at(j);\n\n writeData(ofs, spt.x);\n writeData(ofs, spt.y);\n writeData(ofs, spt.z);\n }\n }\n\n return true;\n}\n\nbool\nCameraCalibration::readChessboardData(const std::string& filename)\n{\n std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary);\n if (!ifs.is_open())\n {\n return false;\n }\n\n readData(ifs, m_boardSize.width);\n readData(ifs, m_boardSize.height);\n readData(ifs, m_squareSize);\n\n readData(ifs, m_measurementCovariance(0,0));\n readData(ifs, m_measurementCovariance(0,1));\n readData(ifs, m_measurementCovariance(1,0));\n readData(ifs, m_measurementCovariance(1,1));\n\n int rows, cols, type;\n readData(ifs, rows);\n readData(ifs, cols);\n readData(ifs, type);\n m_cameraPoses = cv::Mat(rows, cols, type);\n\n for (int i = 0; i < m_cameraPoses.rows; ++i)\n {\n for (int j = 0; j < m_cameraPoses.cols; ++j)\n {\n readData(ifs, m_cameraPoses.at(i,j));\n }\n }\n\n size_t nImagePointSets;\n readData(ifs, nImagePointSets);\n\n m_imagePoints.clear();\n m_imagePoints.resize(nImagePointSets);\n for (size_t i = 0; i < m_imagePoints.size(); ++i)\n {\n size_t nImagePoints;\n readData(ifs, nImagePoints);\n m_imagePoints.at(i).resize(nImagePoints);\n\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n readData(ifs, ipt.x);\n readData(ifs, ipt.y);\n }\n }\n\n size_t nScenePointSets;\n readData(ifs, nScenePointSets);\n\n m_scenePoints.clear();\n m_scenePoints.resize(nScenePointSets);\n for (size_t i = 0; i < m_scenePoints.size(); ++i)\n {\n size_t nScenePoints;\n readData(ifs, nScenePoints);\n m_scenePoints.at(i).resize(nScenePoints);\n\n for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j)\n {\n cv::Point3f& spt = m_scenePoints.at(i).at(j);\n readData(ifs, spt.x);\n readData(ifs, spt.y);\n readData(ifs, spt.z);\n }\n }\n\n return true;\n}\n\nvoid\nCameraCalibration::setVerbose(bool verbose)\n{\n m_verbose = verbose;\n}\n\nbool\nCameraCalibration::calibrateHelper(CameraPtr& camera,\n std::vector& rvecs, std::vector& tvecs) const\n{\n rvecs.assign(m_scenePoints.size(), cv::Mat());\n tvecs.assign(m_scenePoints.size(), cv::Mat());\n\n // STEP 1: Estimate intrinsics\n camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints);\n\n // STEP 2: Estimate extrinsics\n for (size_t i = 0; i < m_scenePoints.size(); ++i)\n {\n camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i));\n }\n\n if (m_verbose)\n {\n std::cout << \"[\" << camera->cameraName() << \"] \"\n << \"# INFO: \" << \"Initial reprojection error: \"\n << std::fixed << std::setprecision(3)\n << camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs)\n << \" pixels\" << std::endl;\n }\n\n // STEP 3: optimization using ceres\n optimize(camera, rvecs, tvecs);\n\n if (m_verbose)\n {\n double err = camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs);\n std::cout << \"[\" << camera->cameraName() << \"] \" << \"# INFO: Final reprojection error: \"\n << err << \" pixels\" << std::endl;\n std::cout << \"[\" << camera->cameraName() << \"] \" << \"# INFO: \"\n << camera->parametersToString() << std::endl;\n }\n\n return true;\n}\n\nvoid\nCameraCalibration::optimize(CameraPtr& camera,\n std::vector& rvecs, std::vector& tvecs) const\n{\n // Use ceres to do optimization\n ceres::Problem problem;\n\n std::vector > transformVec(rvecs.size());\n for (size_t i = 0; i < rvecs.size(); ++i)\n {\n Eigen::Vector3d rvec;\n cv::cv2eigen(rvecs.at(i), rvec);\n\n transformVec.at(i).rotation() = Eigen::AngleAxisd(rvec.norm(), rvec.normalized());\n transformVec.at(i).translation() << tvecs[i].at(0),\n tvecs[i].at(1),\n tvecs[i].at(2);\n }\n\n std::vector intrinsicCameraParams;\n m_camera->writeParameters(intrinsicCameraParams);\n\n // create residuals for each observation\n for (size_t i = 0; i < m_imagePoints.size(); ++i)\n {\n for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n {\n const cv::Point3f& spt = m_scenePoints.at(i).at(j);\n const cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n\n ceres::CostFunction* costFunction =\n CostFunctionFactory::instance()->generateCostFunction(camera,\n Eigen::Vector3d(spt.x, spt.y, spt.z),\n Eigen::Vector2d(ipt.x, ipt.y),\n CAMERA_INTRINSICS | CAMERA_POSE);\n\n ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0);\n problem.AddResidualBlock(costFunction, lossFunction,\n intrinsicCameraParams.data(),\n transformVec.at(i).rotationData(),\n transformVec.at(i).translationData());\n }\n\n ceres::LocalParameterization* quaternionParameterization =\n new EigenQuaternionParameterization;\n\n problem.SetParameterization(transformVec.at(i).rotationData(),\n quaternionParameterization);\n }\n\n std::cout << \"begin ceres\" << std::endl;\n ceres::Solver::Options options;\n options.max_num_iterations = 1000;\n options.num_threads = 1;\n\n if (m_verbose)\n {\n options.minimizer_progress_to_stdout = true;\n }\n\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n std::cout << \"end ceres\" << std::endl;\n\n if (m_verbose)\n {\n std::cout << summary.FullReport() << std::endl;\n }\n\n camera->readParameters(intrinsicCameraParams);\n\n for (size_t i = 0; i < rvecs.size(); ++i)\n {\n Eigen::AngleAxisd aa(transformVec.at(i).rotation());\n\n Eigen::Vector3d rvec = aa.angle() * aa.axis();\n cv::eigen2cv(rvec, rvecs.at(i));\n\n cv::Mat& tvec = tvecs.at(i);\n tvec.at(0) = transformVec.at(i).translation()(0);\n tvec.at(1) = transformVec.at(i).translation()(1);\n tvec.at(2) = transformVec.at(i).translation()(2);\n }\n}\n\ntemplate\nvoid\nCameraCalibration::readData(std::ifstream& ifs, T& data) const\n{\n char* buffer = new char[sizeof(T)];\n\n ifs.read(buffer, sizeof(T));\n\n data = *(reinterpret_cast(buffer));\n\n delete[] buffer;\n}\n\ntemplate\nvoid\nCameraCalibration::writeData(std::ofstream& ofs, T data) const\n{\n char* pData = reinterpret_cast(&data);\n\n ofs.write(pData, sizeof(T));\n}\n\n}\n" }, { "path": "src/utils/camodocal/src/camera_models/Camera.cc", "content": "#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include \n\nnamespace camodocal\n{\n\nCamera::Parameters::Parameters(ModelType modelType)\n : m_modelType(modelType)\n , m_imageWidth(0)\n , m_imageHeight(0)\n{\n switch (modelType)\n {\n case KANNALA_BRANDT:\n m_nIntrinsics = 8;\n break;\n case PINHOLE:\n m_nIntrinsics = 8;\n break;\n case SCARAMUZZA:\n m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;\n break;\n case MEI:\n default:\n m_nIntrinsics = 9;\n }\n}\n\nCamera::Parameters::Parameters(ModelType modelType,\n const std::string& cameraName,\n int w, int h)\n : m_modelType(modelType)\n , m_cameraName(cameraName)\n , m_imageWidth(w)\n , m_imageHeight(h)\n{\n switch (modelType)\n {\n case KANNALA_BRANDT:\n m_nIntrinsics = 8;\n break;\n case PINHOLE:\n m_nIntrinsics = 8;\n break;\n case SCARAMUZZA:\n m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;\n break;\n case MEI:\n default:\n m_nIntrinsics = 9;\n }\n}\n\nCamera::ModelType&\nCamera::Parameters::modelType(void)\n{\n return m_modelType;\n}\n\nstd::string&\nCamera::Parameters::cameraName(void)\n{\n return m_cameraName;\n}\n\nint&\nCamera::Parameters::imageWidth(void)\n{\n return m_imageWidth;\n}\n\nint&\nCamera::Parameters::imageHeight(void)\n{\n return m_imageHeight;\n}\n\nCamera::ModelType\nCamera::Parameters::modelType(void) const\n{\n return m_modelType;\n}\n\nconst std::string&\nCamera::Parameters::cameraName(void) const\n{\n return m_cameraName;\n}\n\nint\nCamera::Parameters::imageWidth(void) const\n{\n return m_imageWidth;\n}\n\nint\nCamera::Parameters::imageHeight(void) const\n{\n return m_imageHeight;\n}\n\nint\nCamera::Parameters::nIntrinsics(void) const\n{\n return m_nIntrinsics;\n}\n\ncv::Mat&\nCamera::mask(void)\n{\n return m_mask;\n}\n\nconst cv::Mat&\nCamera::mask(void) const\n{\n return m_mask;\n}\n\nvoid\nCamera::estimateExtrinsics(const std::vector& objectPoints,\n const std::vector& imagePoints,\n cv::Mat& rvec, cv::Mat& tvec) const\n{\n std::vector Ms(imagePoints.size());\n for (size_t i = 0; i < Ms.size(); ++i)\n {\n Eigen::Vector3d P;\n liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);\n\n P /= P(2);\n\n Ms.at(i).x = P(0);\n Ms.at(i).y = P(1);\n }\n\n // assume unit focal length, zero principal point, and zero distortion\n cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);\n}\n\ndouble\nCamera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const\n{\n Eigen::Vector2d p1, p2;\n\n spaceToPlane(P1, p1);\n spaceToPlane(P2, p2);\n\n return (p1 - p2).norm();\n}\n\ndouble\nCamera::reprojectionError(const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints,\n const std::vector& rvecs,\n const std::vector& tvecs,\n cv::OutputArray _perViewErrors) const\n{\n int imageCount = objectPoints.size();\n size_t pointsSoFar = 0;\n double totalErr = 0.0;\n\n bool computePerViewErrors = _perViewErrors.needed();\n cv::Mat perViewErrors;\n if (computePerViewErrors)\n {\n _perViewErrors.create(imageCount, 1, CV_64F);\n perViewErrors = _perViewErrors.getMat();\n }\n\n for (int i = 0; i < imageCount; ++i)\n {\n size_t pointCount = imagePoints.at(i).size();\n\n pointsSoFar += pointCount;\n\n std::vector estImagePoints;\n projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i),\n estImagePoints);\n\n double err = 0.0;\n for (size_t j = 0; j < imagePoints.at(i).size(); ++j)\n {\n err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j));\n }\n\n if (computePerViewErrors)\n {\n perViewErrors.at(i) = err / pointCount;\n }\n\n totalErr += err;\n }\n\n return totalErr / pointsSoFar;\n}\n\ndouble\nCamera::reprojectionError(const Eigen::Vector3d& P,\n const Eigen::Quaterniond& camera_q,\n const Eigen::Vector3d& camera_t,\n const Eigen::Vector2d& observed_p) const\n{\n Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t;\n\n Eigen::Vector2d p;\n spaceToPlane(P_cam, p);\n\n return (p - observed_p).norm();\n}\n\nvoid\nCamera::projectPoints(const std::vector& objectPoints,\n const cv::Mat& rvec,\n const cv::Mat& tvec,\n std::vector& imagePoints) const\n{\n // project 3D object points to the image plane\n imagePoints.reserve(objectPoints.size());\n\n //double\n cv::Mat R0;\n cv::Rodrigues(rvec, R0);\n\n Eigen::MatrixXd R(3,3);\n R << R0.at(0,0), R0.at(0,1), R0.at(0,2),\n R0.at(1,0), R0.at(1,1), R0.at(1,2),\n R0.at(2,0), R0.at(2,1), R0.at(2,2);\n\n Eigen::Vector3d t;\n t << tvec.at(0), tvec.at(1), tvec.at(2);\n\n for (size_t i = 0; i < objectPoints.size(); ++i)\n {\n const cv::Point3f& objectPoint = objectPoints.at(i);\n\n // Rotate and translate\n Eigen::Vector3d P;\n P << objectPoint.x, objectPoint.y, objectPoint.z;\n\n P = R * P + t;\n\n Eigen::Vector2d p;\n spaceToPlane(P, p);\n\n imagePoints.push_back(cv::Point2f(p(0), p(1)));\n }\n}\n\n}\n" }, { "path": "src/utils/camodocal/src/camera_models/CameraFactory.cc", "content": "#include \"camodocal/camera_models/CameraFactory.h\"\n\n#include \n\n\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include \"ceres/ceres.h\"\n\nnamespace camodocal\n{\n\nboost::shared_ptr CameraFactory::m_instance;\n\nCameraFactory::CameraFactory()\n{\n\n}\n\nboost::shared_ptr\nCameraFactory::instance(void)\n{\n if (m_instance.get() == 0)\n {\n m_instance.reset(new CameraFactory);\n }\n\n return m_instance;\n}\n\nCameraPtr\nCameraFactory::generateCamera(Camera::ModelType modelType,\n const std::string& cameraName,\n cv::Size imageSize) const\n{\n switch (modelType)\n {\n case Camera::KANNALA_BRANDT:\n {\n EquidistantCameraPtr camera(new EquidistantCamera);\n\n EquidistantCamera::Parameters params = camera->getParameters();\n params.cameraName() = cameraName;\n params.imageWidth() = imageSize.width;\n params.imageHeight() = imageSize.height;\n camera->setParameters(params);\n return camera;\n }\n case Camera::PINHOLE:\n {\n PinholeCameraPtr camera(new PinholeCamera);\n\n PinholeCamera::Parameters params = camera->getParameters();\n params.cameraName() = cameraName;\n params.imageWidth() = imageSize.width;\n params.imageHeight() = imageSize.height;\n camera->setParameters(params);\n return camera;\n }\n case Camera::SCARAMUZZA:\n {\n OCAMCameraPtr camera(new OCAMCamera);\n\n OCAMCamera::Parameters params = camera->getParameters();\n params.cameraName() = cameraName;\n params.imageWidth() = imageSize.width;\n params.imageHeight() = imageSize.height;\n camera->setParameters(params);\n return camera;\n }\n case Camera::MEI:\n default:\n {\n CataCameraPtr camera(new CataCamera);\n\n CataCamera::Parameters params = camera->getParameters();\n params.cameraName() = cameraName;\n params.imageWidth() = imageSize.width;\n params.imageHeight() = imageSize.height;\n camera->setParameters(params);\n return camera;\n }\n }\n}\n\nCameraPtr\nCameraFactory::generateCameraFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return CameraPtr();\n }\n\n Camera::ModelType modelType = Camera::MEI;\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (boost::iequals(sModelType, \"kannala_brandt\"))\n {\n modelType = Camera::KANNALA_BRANDT;\n }\n else if (boost::iequals(sModelType, \"mei\"))\n {\n modelType = Camera::MEI;\n }\n else if (boost::iequals(sModelType, \"scaramuzza\"))\n {\n modelType = Camera::SCARAMUZZA;\n }\n else if (boost::iequals(sModelType, \"pinhole\"))\n {\n modelType = Camera::PINHOLE;\n }\n else\n {\n std::cerr << \"# ERROR: Unknown camera model: \" << sModelType << std::endl;\n return CameraPtr();\n }\n }\n\n switch (modelType)\n {\n case Camera::KANNALA_BRANDT:\n {\n EquidistantCameraPtr camera(new EquidistantCamera);\n\n EquidistantCamera::Parameters params = camera->getParameters();\n params.readFromYamlFile(filename);\n camera->setParameters(params);\n return camera;\n }\n case Camera::PINHOLE:\n {\n PinholeCameraPtr camera(new PinholeCamera);\n\n PinholeCamera::Parameters params = camera->getParameters();\n params.readFromYamlFile(filename);\n camera->setParameters(params);\n return camera;\n }\n case Camera::SCARAMUZZA:\n {\n OCAMCameraPtr camera(new OCAMCamera);\n\n OCAMCamera::Parameters params = camera->getParameters();\n params.readFromYamlFile(filename);\n camera->setParameters(params);\n return camera;\n }\n case Camera::MEI:\n default:\n {\n CataCameraPtr camera(new CataCamera);\n\n CataCamera::Parameters params = camera->getParameters();\n params.readFromYamlFile(filename);\n camera->setParameters(params);\n return camera;\n }\n }\n\n return CameraPtr();\n}\n\n}\n\n" }, { "path": "src/utils/camodocal/src/camera_models/CataCamera.cc", "content": "#include \"camodocal/camera_models/CataCamera.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nCataCamera::Parameters::Parameters()\n : Camera::Parameters(MEI)\n , m_xi(0.0)\n , m_k1(0.0)\n , m_k2(0.0)\n , m_p1(0.0)\n , m_p2(0.0)\n , m_gamma1(0.0)\n , m_gamma2(0.0)\n , m_u0(0.0)\n , m_v0(0.0)\n{\n\n}\n\nCataCamera::Parameters::Parameters(const std::string& cameraName,\n int w, int h,\n double xi,\n double k1, double k2,\n double p1, double p2,\n double gamma1, double gamma2,\n double u0, double v0)\n : Camera::Parameters(MEI, cameraName, w, h)\n , m_xi(xi)\n , m_k1(k1)\n , m_k2(k2)\n , m_p1(p1)\n , m_p2(p2)\n , m_gamma1(gamma1)\n , m_gamma2(gamma2)\n , m_u0(u0)\n , m_v0(v0)\n{\n}\n\ndouble&\nCataCamera::Parameters::xi(void)\n{\n return m_xi;\n}\n\ndouble&\nCataCamera::Parameters::k1(void)\n{\n return m_k1;\n}\n\ndouble&\nCataCamera::Parameters::k2(void)\n{\n return m_k2;\n}\n\ndouble&\nCataCamera::Parameters::p1(void)\n{\n return m_p1;\n}\n\ndouble&\nCataCamera::Parameters::p2(void)\n{\n return m_p2;\n}\n\ndouble&\nCataCamera::Parameters::gamma1(void)\n{\n return m_gamma1;\n}\n\ndouble&\nCataCamera::Parameters::gamma2(void)\n{\n return m_gamma2;\n}\n\ndouble&\nCataCamera::Parameters::u0(void)\n{\n return m_u0;\n}\n\ndouble&\nCataCamera::Parameters::v0(void)\n{\n return m_v0;\n}\n\ndouble\nCataCamera::Parameters::xi(void) const\n{\n return m_xi;\n}\n\ndouble\nCataCamera::Parameters::k1(void) const\n{\n return m_k1;\n}\n\ndouble\nCataCamera::Parameters::k2(void) const\n{\n return m_k2;\n}\n\ndouble\nCataCamera::Parameters::p1(void) const\n{\n return m_p1;\n}\n\ndouble\nCataCamera::Parameters::p2(void) const\n{\n return m_p2;\n}\n\ndouble\nCataCamera::Parameters::gamma1(void) const\n{\n return m_gamma1;\n}\n\ndouble\nCataCamera::Parameters::gamma2(void) const\n{\n return m_gamma2;\n}\n\ndouble\nCataCamera::Parameters::u0(void) const\n{\n return m_u0;\n}\n\ndouble\nCataCamera::Parameters::v0(void) const\n{\n return m_v0;\n}\n\nbool\nCataCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return false;\n }\n\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (sModelType.compare(\"MEI\") != 0)\n {\n return false;\n }\n }\n\n m_modelType = MEI;\n fs[\"camera_name\"] >> m_cameraName;\n m_imageWidth = static_cast(fs[\"image_width\"]);\n m_imageHeight = static_cast(fs[\"image_height\"]);\n\n cv::FileNode n = fs[\"mirror_parameters\"];\n m_xi = static_cast(n[\"xi\"]);\n\n n = fs[\"distortion_parameters\"];\n m_k1 = static_cast(n[\"k1\"]);\n m_k2 = static_cast(n[\"k2\"]);\n m_p1 = static_cast(n[\"p1\"]);\n m_p2 = static_cast(n[\"p2\"]);\n\n n = fs[\"projection_parameters\"];\n m_gamma1 = static_cast(n[\"gamma1\"]);\n m_gamma2 = static_cast(n[\"gamma2\"]);\n m_u0 = static_cast(n[\"u0\"]);\n m_v0 = static_cast(n[\"v0\"]);\n\n return true;\n}\n\nvoid\nCataCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n fs << \"model_type\" << \"MEI\";\n fs << \"camera_name\" << m_cameraName;\n fs << \"image_width\" << m_imageWidth;\n fs << \"image_height\" << m_imageHeight;\n\n // mirror: xi\n fs << \"mirror_parameters\";\n fs << \"{\" << \"xi\" << m_xi << \"}\";\n\n // radial distortion: k1, k2\n // tangential distortion: p1, p2\n fs << \"distortion_parameters\";\n fs << \"{\" << \"k1\" << m_k1\n << \"k2\" << m_k2\n << \"p1\" << m_p1\n << \"p2\" << m_p2 << \"}\";\n\n // projection: gamma1, gamma2, u0, v0\n fs << \"projection_parameters\";\n fs << \"{\" << \"gamma1\" << m_gamma1\n << \"gamma2\" << m_gamma2\n << \"u0\" << m_u0\n << \"v0\" << m_v0 << \"}\";\n\n fs.release();\n}\n\nCataCamera::Parameters&\nCataCamera::Parameters::operator=(const CataCamera::Parameters& other)\n{\n if (this != &other)\n {\n m_modelType = other.m_modelType;\n m_cameraName = other.m_cameraName;\n m_imageWidth = other.m_imageWidth;\n m_imageHeight = other.m_imageHeight;\n m_xi = other.m_xi;\n m_k1 = other.m_k1;\n m_k2 = other.m_k2;\n m_p1 = other.m_p1;\n m_p2 = other.m_p2;\n m_gamma1 = other.m_gamma1;\n m_gamma2 = other.m_gamma2;\n m_u0 = other.m_u0;\n m_v0 = other.m_v0;\n }\n\n return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const CataCamera::Parameters& params)\n{\n out << \"Camera Parameters:\" << std::endl;\n out << \" model_type \" << \"MEI\" << std::endl;\n out << \" camera_name \" << params.m_cameraName << std::endl;\n out << \" image_width \" << params.m_imageWidth << std::endl;\n out << \" image_height \" << params.m_imageHeight << std::endl;\n\n out << \"Mirror Parameters\" << std::endl;\n out << std::fixed << std::setprecision(10);\n out << \" xi \" << params.m_xi << std::endl;\n\n // radial distortion: k1, k2\n // tangential distortion: p1, p2\n out << \"Distortion Parameters\" << std::endl;\n out << \" k1 \" << params.m_k1 << std::endl\n << \" k2 \" << params.m_k2 << std::endl\n << \" p1 \" << params.m_p1 << std::endl\n << \" p2 \" << params.m_p2 << std::endl;\n\n // projection: gamma1, gamma2, u0, v0\n out << \"Projection Parameters\" << std::endl;\n out << \" gamma1 \" << params.m_gamma1 << std::endl\n << \" gamma2 \" << params.m_gamma2 << std::endl\n << \" u0 \" << params.m_u0 << std::endl\n << \" v0 \" << params.m_v0 << std::endl;\n\n return out;\n}\n\nCataCamera::CataCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n , m_noDistortion(true)\n{\n\n}\n\nCataCamera::CataCamera(const std::string& cameraName,\n int imageWidth, int imageHeight,\n double xi, double k1, double k2, double p1, double p2,\n double gamma1, double gamma2, double u0, double v0)\n : mParameters(cameraName, imageWidth, imageHeight,\n xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0)\n{\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.gamma1();\n m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n m_inv_K22 = 1.0 / mParameters.gamma2();\n m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nCataCamera::CataCamera(const CataCamera::Parameters& params)\n : mParameters(params)\n{\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.gamma1();\n m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n m_inv_K22 = 1.0 / mParameters.gamma2();\n m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nCamera::ModelType\nCataCamera::modelType(void) const\n{\n return mParameters.modelType();\n}\n\nconst std::string&\nCataCamera::cameraName(void) const\n{\n return mParameters.cameraName();\n}\n\nint\nCataCamera::imageWidth(void) const\n{\n return mParameters.imageWidth();\n}\n\nint\nCataCamera::imageHeight(void) const\n{\n return mParameters.imageHeight();\n}\n\nvoid\nCataCamera::estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints)\n{\n Parameters params = getParameters();\n\n double u0 = params.imageWidth() / 2.0;\n double v0 = params.imageHeight() / 2.0;\n\n double gamma0 = 0.0;\n double minReprojErr = std::numeric_limits::max();\n\n std::vector rvecs, tvecs;\n rvecs.assign(objectPoints.size(), cv::Mat());\n tvecs.assign(objectPoints.size(), cv::Mat());\n\n params.xi() = 1.0;\n params.k1() = 0.0;\n params.k2() = 0.0;\n params.p1() = 0.0;\n params.p2() = 0.0;\n params.u0() = u0;\n params.v0() = v0;\n\n // Initialize gamma (focal length)\n // Use non-radial line image and xi = 1\n for (size_t i = 0; i < imagePoints.size(); ++i)\n {\n for (int r = 0; r < boardSize.height; ++r)\n {\n cv::Mat P(boardSize.width, 4, CV_64F);\n for (int c = 0; c < boardSize.width; ++c)\n {\n const cv::Point2f& imagePoint = imagePoints.at(i).at(r * boardSize.width + c);\n\n double u = imagePoint.x - u0;\n double v = imagePoint.y - v0;\n\n P.at(c, 0) = u;\n P.at(c, 1) = v;\n P.at(c, 2) = 0.5;\n P.at(c, 3) = -0.5 * (square(u) + square(v));\n }\n\n cv::Mat C;\n cv::SVD::solveZ(P, C);\n\n double t = square(C.at(0)) + square(C.at(1)) + C.at(2) * C.at(3);\n if (t < 0.0)\n {\n continue;\n }\n\n // check that line image is not radial\n double d = sqrt(1.0 / t);\n double nx = C.at(0) * d;\n double ny = C.at(1) * d;\n if (hypot(nx, ny) > 0.95)\n {\n continue;\n }\n\n double gamma = sqrt(C.at(2) / C.at(3));\n\n params.gamma1() = gamma;\n params.gamma2() = gamma;\n setParameters(params);\n\n for (size_t j = 0; j < objectPoints.size(); ++j)\n {\n estimateExtrinsics(objectPoints.at(j), imagePoints.at(j), rvecs.at(j), tvecs.at(j));\n }\n\n double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray());\n\n if (reprojErr < minReprojErr)\n {\n minReprojErr = reprojErr;\n gamma0 = gamma;\n }\n }\n }\n\n if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits::max())\n {\n std::cout << \"[\" << params.cameraName() << \"] \"\n << \"# INFO: CataCamera model fails with given data. \" << std::endl;\n\n return;\n }\n\n params.gamma1() = gamma0;\n params.gamma2() = gamma0;\n setParameters(params);\n}\n\n/** \n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nCataCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n double lambda;\n\n // Lift points to normalised plane\n mx_d = m_inv_K11 * p(0) + m_inv_K13;\n my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n if (m_noDistortion)\n {\n mx_u = mx_d;\n my_u = my_d;\n }\n else\n {\n // Apply inverse distortion model\n if (0)\n {\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n // Inverse distortion model\n // proposed by Heikkila\n mx2_d = mx_d*mx_d;\n my2_d = my_d*my_d;\n mxy_d = mx_d*my_d;\n rho2_d = mx2_d+my2_d;\n rho4_d = rho2_d*rho2_d;\n radDist_d = k1*rho2_d+k2*rho4_d;\n Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n mx_u = mx_d - inv_denom_d*Dx_d;\n my_u = my_d - inv_denom_d*Dy_d;\n }\n else\n {\n // Recursive distortion model\n int n = 6;\n Eigen::Vector2d d_u;\n distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n // Approximate value\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n\n for (int i = 1; i < n; ++i)\n {\n distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n }\n }\n }\n\n // Lift normalised points to the sphere (inv_hslash)\n double xi = mParameters.xi();\n if (xi == 1.0)\n {\n lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0);\n P << lambda * mx_u, lambda * my_u, lambda - 1.0;\n }\n else\n {\n lambda = (xi + sqrt(1.0 + (1.0 - xi * xi) * (mx_u * mx_u + my_u * my_u))) / (1.0 + mx_u * mx_u + my_u * my_u);\n P << lambda * mx_u, lambda * my_u, lambda - xi;\n }\n}\n\n/** \n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nCataCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n //double lambda;\n\n // Lift points to normalised plane\n mx_d = m_inv_K11 * p(0) + m_inv_K13;\n my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n if (m_noDistortion)\n {\n mx_u = mx_d;\n my_u = my_d;\n }\n else\n {\n if (0)\n {\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n // Apply inverse distortion model\n // proposed by Heikkila\n mx2_d = mx_d*mx_d;\n my2_d = my_d*my_d;\n mxy_d = mx_d*my_d;\n rho2_d = mx2_d+my2_d;\n rho4_d = rho2_d*rho2_d;\n radDist_d = k1*rho2_d+k2*rho4_d;\n Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n mx_u = mx_d - inv_denom_d*Dx_d;\n my_u = my_d - inv_denom_d*Dy_d;\n }\n else\n {\n // Recursive distortion model\n int n = 8;\n Eigen::Vector2d d_u;\n distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n // Approximate value\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n\n for (int i = 1; i < n; ++i)\n {\n distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n }\n }\n }\n\n // Obtain a projective ray\n double xi = mParameters.xi();\n if (xi == 1.0)\n {\n P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0;\n }\n else\n {\n // Reuse variable\n rho2_d = mx_u * mx_u + my_u * my_u;\n P << mx_u, my_u, 1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d));\n }\n}\n\n\n/** \n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nCataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n Eigen::Vector2d p_u, p_d;\n\n // Project points to the normalised plane\n double z = P(2) + mParameters.xi() * P.norm();\n p_u << P(0) / z, P(1) / z;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n // Apply generalised projection matrix\n p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\n#if 0\n/** \n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nCataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n Eigen::Matrix& J) const\n{\n double xi = mParameters.xi();\n\n Eigen::Vector2d p_u, p_d;\n double norm, inv_denom;\n double dxdmx, dydmx, dxdmy, dydmy;\n\n norm = P.norm();\n // Project points to the normalised plane\n inv_denom = 1.0 / (P(2) + xi * norm);\n p_u << inv_denom * P(0), inv_denom * P(1);\n\n // Calculate jacobian\n inv_denom = inv_denom * inv_denom / norm;\n double dudx = inv_denom * (norm * P(2) + xi * (P(1) * P(1) + P(2) * P(2)));\n double dvdx = -inv_denom * xi * P(0) * P(1);\n double dudy = dvdx;\n double dvdy = inv_denom * (norm * P(2) + xi * (P(0) * P(0) + P(2) * P(2)));\n inv_denom = inv_denom * (-xi * P(2) - norm); // reuse variable\n double dudz = P(0) * inv_denom;\n double dvdz = P(1) * inv_denom;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n double gamma1 = mParameters.gamma1();\n double gamma2 = mParameters.gamma2();\n\n // Make the product of the jacobians\n // and add projection matrix jacobian\n inv_denom = gamma1 * (dudx * dxdmx + dvdx * dxdmy); // reuse\n dvdx = gamma2 * (dudx * dydmx + dvdx * dydmy);\n dudx = inv_denom;\n\n inv_denom = gamma1 * (dudy * dxdmx + dvdy * dxdmy); // reuse\n dvdy = gamma2 * (dudy * dydmx + dvdy * dydmy);\n dudy = inv_denom;\n\n inv_denom = gamma1 * (dudz * dxdmx + dvdz * dxdmy); // reuse\n dvdz = gamma2 * (dudz * dydmx + dvdz * dydmy);\n dudz = inv_denom;\n \n // Apply generalised projection matrix\n p << gamma1 * p_d(0) + mParameters.u0(),\n gamma2 * p_d(1) + mParameters.v0();\n\n J << dudx, dudy, dudz,\n dvdx, dvdy, dvdz;\n}\n#endif\n\n/** \n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nCataCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n Eigen::Vector2d p_d;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n // Apply generalised projection matrix\n p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\n/** \n * \\brief Apply distortion to input point (from the normalised plane)\n * \n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nCataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const\n{\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n mx2_u = p_u(0) * p_u(0);\n my2_u = p_u(1) * p_u(1);\n mxy_u = p_u(0) * p_u(1);\n rho2_u = mx2_u + my2_u;\n rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n}\n\n/** \n * \\brief Apply distortion to input point (from the normalised plane)\n * and calculate Jacobian\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nCataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n Eigen::Matrix2d& J) const\n{\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n mx2_u = p_u(0) * p_u(0);\n my2_u = p_u(1) * p_u(1);\n mxy_u = p_u(0) * p_u(1);\n rho2_u = mx2_u + my2_u;\n rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n\n double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0);\n double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1);\n double dxdmy = dydmx;\n double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0);\n\n J << dxdmx, dxdmy,\n dydmx, dydmy;\n}\n\nvoid\nCataCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n double xi = mParameters.xi();\n double d2 = mx_u * mx_u + my_u * my_u;\n\n Eigen::Vector3d P;\n P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));\n\n Eigen::Vector2d p;\n spaceToPlane(P, p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nCataCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx, float fy,\n cv::Size imageSize,\n float cx, float cy,\n cv::Mat rmat) const\n{\n if (imageSize == cv::Size(0, 0))\n {\n imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n }\n\n cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n Eigen::Matrix3f K_rect;\n\n if (cx == -1.0f && cy == -1.0f)\n {\n K_rect << fx, 0, imageSize.width / 2,\n 0, fy, imageSize.height / 2,\n 0, 0, 1;\n }\n else\n {\n K_rect << fx, 0, cx,\n 0, fy, cy,\n 0, 0, 1;\n }\n\n if (fx == -1.0f || fy == -1.0f)\n {\n K_rect(0,0) = mParameters.gamma1();\n K_rect(1,1) = mParameters.gamma2();\n }\n\n Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n Eigen::Matrix3f R, R_inv;\n cv::cv2eigen(rmat, R);\n R_inv = R.inverse();\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n Eigen::Vector3f xo;\n xo << u, v, 1;\n\n Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n Eigen::Vector2d p;\n spaceToPlane(uo.cast(), p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n cv::Mat K_rect_cv;\n cv::eigen2cv(K_rect, K_rect_cv);\n return K_rect_cv;\n}\n\nint\nCataCamera::parameterCount(void) const\n{\n return 9;\n}\n\nconst CataCamera::Parameters&\nCataCamera::getParameters(void) const\n{\n return mParameters;\n}\n\nvoid\nCataCamera::setParameters(const CataCamera::Parameters& parameters)\n{\n mParameters = parameters;\n\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n m_inv_K11 = 1.0 / mParameters.gamma1();\n m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n m_inv_K22 = 1.0 / mParameters.gamma2();\n m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nvoid\nCataCamera::readParameters(const std::vector& parameterVec)\n{\n if ((int)parameterVec.size() != parameterCount())\n {\n return;\n }\n\n Parameters params = getParameters();\n\n params.xi() = parameterVec.at(0);\n params.k1() = parameterVec.at(1);\n params.k2() = parameterVec.at(2);\n params.p1() = parameterVec.at(3);\n params.p2() = parameterVec.at(4);\n params.gamma1() = parameterVec.at(5);\n params.gamma2() = parameterVec.at(6);\n params.u0() = parameterVec.at(7);\n params.v0() = parameterVec.at(8);\n\n setParameters(params);\n}\n\nvoid\nCataCamera::writeParameters(std::vector& parameterVec) const\n{\n parameterVec.resize(parameterCount());\n parameterVec.at(0) = mParameters.xi();\n parameterVec.at(1) = mParameters.k1();\n parameterVec.at(2) = mParameters.k2();\n parameterVec.at(3) = mParameters.p1();\n parameterVec.at(4) = mParameters.p2();\n parameterVec.at(5) = mParameters.gamma1();\n parameterVec.at(6) = mParameters.gamma2();\n parameterVec.at(7) = mParameters.u0();\n parameterVec.at(8) = mParameters.v0();\n}\n\nvoid\nCataCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nCataCamera::parametersToString(void) const\n{\n std::ostringstream oss;\n oss << mParameters;\n\n return oss.str();\n}\n\n}\n" }, { "path": "src/utils/camodocal/src/camera_models/CostFunctionFactory.cc", "content": "#include \"camodocal/camera_models/CostFunctionFactory.h\"\n\n#include \"ceres/ceres.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\nnamespace camodocal\n{\n\ntemplate\nvoid\nworldToCameraTransform(const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const p_odo, const T* const att_odo,\n T* q, T* t, bool optimize_cam_odo_z = true)\n{\n Eigen::Quaternion q_z_inv(cos(att_odo[0] / T(2)), T(0), T(0), -sin(att_odo[0] / T(2)));\n Eigen::Quaternion q_y_inv(cos(att_odo[1] / T(2)), T(0), -sin(att_odo[1] / T(2)), T(0));\n Eigen::Quaternion q_x_inv(cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), T(0), T(0));\n\n Eigen::Quaternion q_zyx_inv = q_x_inv * q_y_inv * q_z_inv;\n\n T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()};\n\n T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]};\n\n T q0[4];\n ceres::QuaternionProduct(q_odo_cam, q_odo, q0);\n\n T t0[3];\n T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]};\n\n ceres::QuaternionRotatePoint(q_odo, t_odo, t0);\n\n t0[0] += t_cam_odo[0];\n t0[1] += t_cam_odo[1];\n\n if (optimize_cam_odo_z)\n {\n t0[2] += t_cam_odo[2];\n }\n\n ceres::QuaternionRotatePoint(q_odo_cam, t0, t);\n t[0] = -t[0];\n t[1] = -t[1];\n t[2] = -t[2];\n\n // Convert quaternion from Ceres convention (w, x, y, z)\n // to Eigen convention (x, y, z, w)\n q[0] = q0[1];\n q[1] = q0[2];\n q[2] = q0[3];\n q[3] = q0[0];\n}\n\ntemplate\nclass ReprojectionError1\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ReprojectionError1(const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p)\n : m_observed_P(observed_P), m_observed_p(observed_p)\n , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {}\n\n ReprojectionError1(const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat)\n : m_observed_P(observed_P), m_observed_p(observed_p)\n , m_sqrtPrecisionMat(sqrtPrecisionMat) {}\n\n ReprojectionError1(const std::vector& intrinsic_params,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p)\n : m_intrinsic_params(intrinsic_params)\n , m_observed_P(observed_P), m_observed_p(observed_p) {}\n\n // variables: camera intrinsics and camera extrinsics\n template \n bool operator()(const T* const intrinsic_params,\n const T* const q,\n const T* const t,\n T* residuals) const\n {\n Eigen::Matrix P = m_observed_P.cast();\n\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n Eigen::Matrix e = predicted_p - m_observed_p.cast();\n\n Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e;\n\n residuals[0] = e_weighted(0);\n residuals[1] = e_weighted(1);\n\n return true;\n }\n\n // variables: camera-odometry transforms and odometry poses\n template \n bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const p_odo, const T* const att_odo,\n T* residuals) const\n {\n T q[4], t[3];\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t);\n\n Eigen::Matrix P = m_observed_P.cast();\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n residuals[0] = predicted_p(0) - T(m_observed_p(0));\n residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n return true;\n }\n\n//private:\n // camera intrinsics\n std::vector m_intrinsic_params;\n\n // observed 3D point\n Eigen::Vector3d m_observed_P;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p;\n\n // square root of precision matrix\n Eigen::Matrix2d m_sqrtPrecisionMat;\n};\n\n// variables: camera extrinsics, 3D point\ntemplate\nclass ReprojectionError2\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ReprojectionError2(const std::vector& intrinsic_params,\n const Eigen::Vector2d& observed_p)\n : m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {}\n\n template \n bool operator()(const T* const q, const T* const t,\n const T* const point, T* residuals) const\n {\n Eigen::Matrix P;\n P(0) = T(point[0]);\n P(1) = T(point[1]);\n P(2) = T(point[2]);\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n residuals[0] = predicted_p(0) - T(m_observed_p(0));\n residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n return true;\n }\n\nprivate:\n // camera intrinsics\n std::vector m_intrinsic_params;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p;\n};\n\ntemplate\nclass ReprojectionError3\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ReprojectionError3(const Eigen::Vector2d& observed_p)\n : m_observed_p(observed_p)\n , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity())\n , m_optimize_cam_odo_z(true) {}\n\n ReprojectionError3(const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat)\n : m_observed_p(observed_p)\n , m_sqrtPrecisionMat(sqrtPrecisionMat)\n , m_optimize_cam_odo_z(true) {}\n\n ReprojectionError3(const std::vector& intrinsic_params,\n const Eigen::Vector2d& observed_p)\n : m_intrinsic_params(intrinsic_params)\n , m_observed_p(observed_p)\n , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity())\n , m_optimize_cam_odo_z(true) {}\n\n ReprojectionError3(const std::vector& intrinsic_params,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat)\n : m_intrinsic_params(intrinsic_params)\n , m_observed_p(observed_p)\n , m_sqrtPrecisionMat(sqrtPrecisionMat)\n , m_optimize_cam_odo_z(true) {}\n\n\n ReprojectionError3(const std::vector& intrinsic_params,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n bool optimize_cam_odo_z)\n : m_intrinsic_params(intrinsic_params)\n , m_odo_pos(odo_pos), m_odo_att(odo_att)\n , m_observed_p(observed_p)\n , m_optimize_cam_odo_z(optimize_cam_odo_z) {}\n\n ReprojectionError3(const std::vector& intrinsic_params,\n const Eigen::Quaterniond& cam_odo_q,\n const Eigen::Vector3d& cam_odo_t,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p)\n : m_intrinsic_params(intrinsic_params)\n , m_cam_odo_q(cam_odo_q), m_cam_odo_t(cam_odo_t)\n , m_odo_pos(odo_pos), m_odo_att(odo_att)\n , m_observed_p(observed_p)\n , m_optimize_cam_odo_z(true) {}\n\n // variables: camera intrinsics, camera-to-odometry transform,\n // odometry extrinsics, 3D point\n template \n bool operator()(const T* const intrinsic_params,\n const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const p_odo, const T* const att_odo,\n const T* const point, T* residuals) const\n {\n T q[4], t[3];\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n Eigen::Matrix P(point[0], point[1], point[2]);\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n Eigen::Matrix err = predicted_p - m_observed_p.cast();\n Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err;\n\n residuals[0] = err_weighted(0);\n residuals[1] = err_weighted(1);\n\n return true;\n }\n\n // variables: camera-to-odometry transform, 3D point\n template \n bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const point, T* residuals) const\n {\n T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};\n T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};\n T q[4], t[3];\n\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n Eigen::Matrix P(point[0], point[1], point[2]);\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n residuals[0] = predicted_p(0) - T(m_observed_p(0));\n residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n return true;\n }\n\n // variables: camera-to-odometry transform, odometry extrinsics, 3D point\n template \n bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n const T* const p_odo, const T* const att_odo,\n const T* const point, T* residuals) const\n {\n T q[4], t[3];\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n Eigen::Matrix P(point[0], point[1], point[2]);\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n Eigen::Matrix err = predicted_p - m_observed_p.cast();\n Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err;\n\n residuals[0] = err_weighted(0);\n residuals[1] = err_weighted(1);\n\n return true;\n }\n\n // variables: 3D point\n template \n bool operator()(const T* const point, T* residuals) const\n {\n T q_cam_odo[4] = {T(m_cam_odo_q.coeffs()(0)), T(m_cam_odo_q.coeffs()(1)), T(m_cam_odo_q.coeffs()(2)), T(m_cam_odo_q.coeffs()(3))};\n T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))};\n T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};\n T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};\n T q[4], t[3];\n\n worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n Eigen::Matrix P(point[0], point[1], point[2]);\n\n // project 3D object point to the image plane\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n residuals[0] = predicted_p(0) - T(m_observed_p(0));\n residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n return true;\n }\n\nprivate:\n // camera intrinsics\n std::vector m_intrinsic_params;\n\n // observed camera-odometry transform\n Eigen::Quaterniond m_cam_odo_q;\n Eigen::Vector3d m_cam_odo_t;\n\n // observed odometry\n Eigen::Vector3d m_odo_pos;\n Eigen::Vector3d m_odo_att;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p;\n\n Eigen::Matrix2d m_sqrtPrecisionMat;\n\n bool m_optimize_cam_odo_z;\n};\n\n// variables: camera intrinsics and camera extrinsics\ntemplate\nclass StereoReprojectionError\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n StereoReprojectionError(const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p_l,\n const Eigen::Vector2d& observed_p_r)\n : m_observed_P(observed_P)\n , m_observed_p_l(observed_p_l)\n , m_observed_p_r(observed_p_r)\n {\n\n }\n\n template \n bool operator()(const T* const intrinsic_params_l,\n const T* const intrinsic_params_r,\n const T* const q_l,\n const T* const t_l,\n const T* const q_l_r,\n const T* const t_l_r,\n T* residuals) const\n {\n Eigen::Matrix P;\n P(0) = T(m_observed_P(0));\n P(1) = T(m_observed_P(1));\n P(2) = T(m_observed_P(2));\n\n Eigen::Matrix predicted_p_l;\n CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l);\n\n Eigen::Quaternion q_r = Eigen::Quaternion(q_l_r) * Eigen::Quaternion(q_l);\n\n Eigen::Matrix t_r;\n t_r(0) = t_l[0];\n t_r(1) = t_l[1];\n t_r(2) = t_l[2];\n\n t_r = Eigen::Quaternion(q_l_r) * t_r;\n t_r(0) += t_l_r[0];\n t_r(1) += t_l_r[1];\n t_r(2) += t_l_r[2];\n\n Eigen::Matrix predicted_p_r;\n CameraT::spaceToPlane(intrinsic_params_r, q_r.coeffs().data(), t_r.data(), P, predicted_p_r);\n\n residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0));\n residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1));\n residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0));\n residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1));\n\n return true;\n }\n\nprivate:\n // observed 3D point\n Eigen::Vector3d m_observed_P;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p_l;\n Eigen::Vector2d m_observed_p_r;\n};\n\ntemplate \nclass ComprehensionError {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ComprehensionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p)\n : m_observed_P(observed_P),\n m_observed_p(observed_p),\n m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {\n }\n\n template \n bool operator()(const T* const intrinsic_params, const T* const q, const T* const t,\n T* residuals) const {\n {\n Eigen::Matrix p = m_observed_p.cast();\n \n Eigen::Matrix predicted_img_P;\n CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P);\n \n Eigen::Matrix predicted_p;\n CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p);\n\n Eigen::Matrix e = predicted_p - m_observed_p.cast();\n\n Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e;\n\n residuals[0] = e_weighted(0);\n residuals[1] = e_weighted(1);\n }\n\n {\n Eigen::Matrix P = m_observed_P.cast();\n\n Eigen::Matrix predicted_p;\n CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n Eigen::Matrix e = predicted_p - m_observed_p.cast();\n\n Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e;\n\n residuals[2] = e_weighted(0);\n residuals[3] = e_weighted(1);\n }\n\n return true;\n }\n\n // private:\n // camera intrinsics\n // std::vector m_intrinsic_params;\n\n // observed 3D point\n Eigen::Vector3d m_observed_P;\n\n // observed 2D point\n Eigen::Vector2d m_observed_p;\n\n // square root of precision matrix\n Eigen::Matrix2d m_sqrtPrecisionMat;\n};\n\nboost::shared_ptr CostFunctionFactory::m_instance;\n\nCostFunctionFactory::CostFunctionFactory()\n{\n\n}\n\nboost::shared_ptr\nCostFunctionFactory::instance(void)\n{\n if (m_instance.get() == 0)\n {\n m_instance.reset(new CostFunctionFactory);\n }\n\n return m_instance;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n int flags) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_INTRINSICS | CAMERA_POSE:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3>(\n new ReprojectionError1(observed_P, observed_p));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3>(\n new ReprojectionError1(observed_P, observed_p));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3>(\n new ReprojectionError1(observed_P, observed_p));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n new ComprehensionError(observed_P, observed_p));\n // new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n // new ReprojectionError1(observed_P, observed_p));\n break;\n }\n break;\n case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>(\n new ReprojectionError1(intrinsic_params, observed_P, observed_p));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>(\n new ReprojectionError1(intrinsic_params, observed_P, observed_p));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>(\n new ReprojectionError1(intrinsic_params, observed_P, observed_p));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>(\n new ReprojectionError1(intrinsic_params, observed_P, observed_p));\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat,\n int flags) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_INTRINSICS | CAMERA_POSE:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3>(\n new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3>(\n new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3>(\n new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat));\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector2d& observed_p,\n int flags, bool optimize_cam_odo_z) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError2(intrinsic_params, observed_p));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError2(intrinsic_params, observed_p));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError2(intrinsic_params, observed_p));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError2(intrinsic_params, observed_p));\n break;\n }\n break;\n case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n }\n break;\n case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p));\n }\n break;\n }\n break;\n case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 2, 1, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n }\n break;\n case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p));\n }\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector2d& observed_p,\n const Eigen::Matrix2d& sqrtPrecisionMat,\n int flags, bool optimize_cam_odo_z) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>(\n new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat));\n }\n break;\n }\n break;\n case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(\n new ReprojectionError3(observed_p, sqrtPrecisionMat));\n }\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n int flags, bool optimize_cam_odo_z) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case CAMERA_ODOMETRY_TRANSFORM | POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n break;\n case Camera::PINHOLE:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n break;\n case Camera::MEI:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n break;\n case Camera::SCARAMUZZA:\n if (optimize_cam_odo_z)\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 3, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n else\n {\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 4, 2, 3>(\n new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n }\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n const Eigen::Quaterniond& cam_odo_q,\n const Eigen::Vector3d& cam_odo_t,\n const Eigen::Vector3d& odo_pos,\n const Eigen::Vector3d& odo_att,\n const Eigen::Vector2d& observed_p,\n int flags) const\n{\n ceres::CostFunction* costFunction = 0;\n\n std::vector intrinsic_params;\n camera->writeParameters(intrinsic_params);\n\n switch (flags)\n {\n case POINT_3D:\n switch (camera->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 3>(\n new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 3>(\n new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 3>(\n new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 2, 3>(\n new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n break;\n }\n break;\n }\n\n return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& cameraL,\n const CameraConstPtr& cameraR,\n const Eigen::Vector3d& observed_P,\n const Eigen::Vector2d& observed_p_l,\n const Eigen::Vector2d& observed_p_r) const\n{\n ceres::CostFunction* costFunction = 0;\n\n if (cameraL->modelType() != cameraR->modelType())\n {\n return costFunction;\n }\n\n switch (cameraL->modelType())\n {\n case Camera::KANNALA_BRANDT:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, 8, 8, 4, 3, 4, 3>(\n new StereoReprojectionError(observed_P, observed_p_l, observed_p_r));\n break;\n case Camera::PINHOLE:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, 8, 8, 4, 3, 4, 3>(\n new StereoReprojectionError(observed_P, observed_p_l, observed_p_r));\n break;\n case Camera::MEI:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, 9, 9, 4, 3, 4, 3>(\n new StereoReprojectionError(observed_P, observed_p_l, observed_p_r));\n break;\n case Camera::SCARAMUZZA:\n costFunction =\n new ceres::AutoDiffCostFunction, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>(\n new StereoReprojectionError(observed_P, observed_p_l, observed_p_r));\n break;\n }\n\n return costFunction;\n}\n\n}\n\n" }, { "path": "src/utils/camodocal/src/camera_models/EquidistantCamera.cc", "content": "#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nEquidistantCamera::Parameters::Parameters()\n : Camera::Parameters(KANNALA_BRANDT)\n , m_k2(0.0)\n , m_k3(0.0)\n , m_k4(0.0)\n , m_k5(0.0)\n , m_mu(0.0)\n , m_mv(0.0)\n , m_u0(0.0)\n , m_v0(0.0)\n{\n\n}\n\nEquidistantCamera::Parameters::Parameters(const std::string& cameraName,\n int w, int h,\n double k2, double k3, double k4, double k5,\n double mu, double mv,\n double u0, double v0)\n : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h)\n , m_k2(k2)\n , m_k3(k3)\n , m_k4(k4)\n , m_k5(k5)\n , m_mu(mu)\n , m_mv(mv)\n , m_u0(u0)\n , m_v0(v0)\n{\n\n}\n\ndouble&\nEquidistantCamera::Parameters::k2(void)\n{\n return m_k2;\n}\n\ndouble&\nEquidistantCamera::Parameters::k3(void)\n{\n return m_k3;\n}\n\ndouble&\nEquidistantCamera::Parameters::k4(void)\n{\n return m_k4;\n}\n\ndouble&\nEquidistantCamera::Parameters::k5(void)\n{\n return m_k5;\n}\n\ndouble&\nEquidistantCamera::Parameters::mu(void)\n{\n return m_mu;\n}\n\ndouble&\nEquidistantCamera::Parameters::mv(void)\n{\n return m_mv;\n}\n\ndouble&\nEquidistantCamera::Parameters::u0(void)\n{\n return m_u0;\n}\n\ndouble&\nEquidistantCamera::Parameters::v0(void)\n{\n return m_v0;\n}\n\ndouble\nEquidistantCamera::Parameters::k2(void) const\n{\n return m_k2;\n}\n\ndouble\nEquidistantCamera::Parameters::k3(void) const\n{\n return m_k3;\n}\n\ndouble\nEquidistantCamera::Parameters::k4(void) const\n{\n return m_k4;\n}\n\ndouble\nEquidistantCamera::Parameters::k5(void) const\n{\n return m_k5;\n}\n\ndouble\nEquidistantCamera::Parameters::mu(void) const\n{\n return m_mu;\n}\n\ndouble\nEquidistantCamera::Parameters::mv(void) const\n{\n return m_mv;\n}\n\ndouble\nEquidistantCamera::Parameters::u0(void) const\n{\n return m_u0;\n}\n\ndouble\nEquidistantCamera::Parameters::v0(void) const\n{\n return m_v0;\n}\n\nbool\nEquidistantCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return false;\n }\n\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (sModelType.compare(\"KANNALA_BRANDT\") != 0)\n {\n return false;\n }\n }\n\n m_modelType = KANNALA_BRANDT;\n fs[\"camera_name\"] >> m_cameraName;\n m_imageWidth = static_cast(fs[\"image_width\"]);\n m_imageHeight = static_cast(fs[\"image_height\"]);\n\n cv::FileNode n = fs[\"projection_parameters\"];\n m_k2 = static_cast(n[\"k2\"]);\n m_k3 = static_cast(n[\"k3\"]);\n m_k4 = static_cast(n[\"k4\"]);\n m_k5 = static_cast(n[\"k5\"]);\n m_mu = static_cast(n[\"mu\"]);\n m_mv = static_cast(n[\"mv\"]);\n m_u0 = static_cast(n[\"u0\"]);\n m_v0 = static_cast(n[\"v0\"]);\n\n return true;\n}\n\nvoid\nEquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n fs << \"model_type\" << \"KANNALA_BRANDT\";\n fs << \"camera_name\" << m_cameraName;\n fs << \"image_width\" << m_imageWidth;\n fs << \"image_height\" << m_imageHeight;\n\n // projection: k2, k3, k4, k5, mu, mv, u0, v0\n fs << \"projection_parameters\";\n fs << \"{\" << \"k2\" << m_k2\n << \"k3\" << m_k3\n << \"k4\" << m_k4\n << \"k5\" << m_k5\n << \"mu\" << m_mu\n << \"mv\" << m_mv\n << \"u0\" << m_u0\n << \"v0\" << m_v0 << \"}\";\n\n fs.release();\n}\n\nEquidistantCamera::Parameters&\nEquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other)\n{\n if (this != &other)\n {\n m_modelType = other.m_modelType;\n m_cameraName = other.m_cameraName;\n m_imageWidth = other.m_imageWidth;\n m_imageHeight = other.m_imageHeight;\n m_k2 = other.m_k2;\n m_k3 = other.m_k3;\n m_k4 = other.m_k4;\n m_k5 = other.m_k5;\n m_mu = other.m_mu;\n m_mv = other.m_mv;\n m_u0 = other.m_u0;\n m_v0 = other.m_v0;\n }\n\n return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const EquidistantCamera::Parameters& params)\n{\n out << \"Camera Parameters:\" << std::endl;\n out << \" model_type \" << \"KANNALA_BRANDT\" << std::endl;\n out << \" camera_name \" << params.m_cameraName << std::endl;\n out << \" image_width \" << params.m_imageWidth << std::endl;\n out << \" image_height \" << params.m_imageHeight << std::endl;\n\n // projection: k2, k3, k4, k5, mu, mv, u0, v0\n out << \"Projection Parameters\" << std::endl;\n out << \" k2 \" << params.m_k2 << std::endl\n << \" k3 \" << params.m_k3 << std::endl\n << \" k4 \" << params.m_k4 << std::endl\n << \" k5 \" << params.m_k5 << std::endl\n << \" mu \" << params.m_mu << std::endl\n << \" mv \" << params.m_mv << std::endl\n << \" u0 \" << params.m_u0 << std::endl\n << \" v0 \" << params.m_v0 << std::endl;\n\n return out;\n}\n\nEquidistantCamera::EquidistantCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n{\n\n}\n\nEquidistantCamera::EquidistantCamera(const std::string& cameraName,\n int imageWidth, int imageHeight,\n double k2, double k3, double k4, double k5,\n double mu, double mv,\n double u0, double v0)\n : mParameters(cameraName, imageWidth, imageHeight,\n k2, k3, k4, k5, mu, mv, u0, v0)\n{\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.mu();\n m_inv_K13 = -mParameters.u0() / mParameters.mu();\n m_inv_K22 = 1.0 / mParameters.mv();\n m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nEquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params)\n : mParameters(params)\n{\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.mu();\n m_inv_K13 = -mParameters.u0() / mParameters.mu();\n m_inv_K22 = 1.0 / mParameters.mv();\n m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nCamera::ModelType\nEquidistantCamera::modelType(void) const\n{\n return mParameters.modelType();\n}\n\nconst std::string&\nEquidistantCamera::cameraName(void) const\n{\n return mParameters.cameraName();\n}\n\nint\nEquidistantCamera::imageWidth(void) const\n{\n return mParameters.imageWidth();\n}\n\nint\nEquidistantCamera::imageHeight(void) const\n{\n return mParameters.imageHeight();\n}\n\nvoid\nEquidistantCamera::estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints)\n{\n Parameters params = getParameters();\n\n double u0 = params.imageWidth() / 2.0;\n double v0 = params.imageHeight() / 2.0;\n\n double minReprojErr = std::numeric_limits::max();\n\n std::vector rvecs, tvecs;\n rvecs.assign(objectPoints.size(), cv::Mat());\n tvecs.assign(objectPoints.size(), cv::Mat());\n\n params.k2() = 0.0;\n params.k3() = 0.0;\n params.k4() = 0.0;\n params.k5() = 0.0;\n params.u0() = u0;\n params.v0() = v0;\n\n // Initialize focal length\n // C. Hughes, P. Denny, M. Glavin, and E. Jones,\n // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point\n // Extraction, PAMI 2010\n // Find circles from rows of chessboard corners, and for each pair\n // of circles, find vanishing points: v1 and v2.\n // f = ||v1 - v2|| / PI;\n double f0 = 0.0;\n for (size_t i = 0; i < imagePoints.size(); ++i)\n {\n std::vector center(boardSize.height);\n double radius[boardSize.height];\n for (int r = 0; r < boardSize.height; ++r)\n {\n std::vector circle;\n for (int c = 0; c < boardSize.width; ++c)\n {\n circle.push_back(imagePoints.at(i).at(r * boardSize.width + c));\n }\n\n fitCircle(circle, center[r](0), center[r](1), radius[r]);\n }\n\n for (int j = 0; j < boardSize.height; ++j)\n {\n for (int k = j + 1; k < boardSize.height; ++k)\n {\n // find distance between pair of vanishing points which\n // correspond to intersection points of 2 circles\n std::vector ipts;\n ipts = intersectCircles(center[j](0), center[j](1), radius[j],\n center[k](0), center[k](1), radius[k]);\n\n if (ipts.size() < 2)\n {\n continue;\n }\n\n double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;\n\n params.mu() = f;\n params.mv() = f;\n\n setParameters(params);\n\n for (size_t l = 0; l < objectPoints.size(); ++l)\n {\n estimateExtrinsics(objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l));\n }\n\n double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray());\n\n if (reprojErr < minReprojErr)\n {\n minReprojErr = reprojErr;\n f0 = f;\n }\n }\n }\n }\n\n if (f0 <= 0.0 && minReprojErr >= std::numeric_limits::max())\n {\n std::cout << \"[\" << params.cameraName() << \"] \"\n << \"# INFO: kannala-Brandt model fails with given data. \" << std::endl;\n\n return;\n }\n\n params.mu() = f0;\n params.mv() = f0;\n\n setParameters(params);\n}\n\n/**\n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nEquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n liftProjective(p, P);\n}\n\n/** \n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nEquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n // Lift points to normalised plane\n Eigen::Vector2d p_u;\n p_u << m_inv_K11 * p(0) + m_inv_K13,\n m_inv_K22 * p(1) + m_inv_K23;\n\n // Obtain a projective ray\n double theta, phi;\n backprojectSymmetric(p_u, theta, phi);\n\n P(0) = sin(theta) * cos(phi);\n P(1) = sin(theta) * sin(phi);\n P(2) = cos(theta);\n}\n\n/** \n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nEquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n double theta = acos(P(2) / P.norm());\n double phi = atan2(P(1), P(0));\n\n Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));\n\n // Apply generalised projection matrix\n p << mParameters.mu() * p_u(0) + mParameters.u0(),\n mParameters.mv() * p_u(1) + mParameters.v0();\n}\n\n\n/** \n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nEquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n Eigen::Matrix& J) const\n{\n double theta = acos(P(2) / P.norm());\n double phi = atan2(P(1), P(0));\n\n Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));\n\n // Apply generalised projection matrix\n p << mParameters.mu() * p_u(0) + mParameters.u0(),\n mParameters.mv() * p_u(1) + mParameters.v0();\n}\n\n/** \n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nEquidistantCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n// Eigen::Vector2d p_d;\n//\n// if (m_noDistortion)\n// {\n// p_d = p_u;\n// }\n// else\n// {\n// // Apply distortion\n// Eigen::Vector2d d_u;\n// distortion(p_u, d_u);\n// p_d = p_u + d_u;\n// }\n//\n// // Apply generalised projection matrix\n// p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n// mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\nvoid\nEquidistantCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n double theta, phi;\n backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi);\n\n Eigen::Vector3d P;\n P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta);\n\n Eigen::Vector2d p;\n spaceToPlane(P, p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nEquidistantCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx, float fy,\n cv::Size imageSize,\n float cx, float cy,\n cv::Mat rmat) const\n{\n if (imageSize == cv::Size(0, 0))\n {\n imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n }\n\n cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n Eigen::Matrix3f K_rect;\n\n if (cx == -1.0f && cy == -1.0f)\n {\n K_rect << fx, 0, imageSize.width / 2,\n 0, fy, imageSize.height / 2,\n 0, 0, 1;\n }\n else\n {\n K_rect << fx, 0, cx,\n 0, fy, cy,\n 0, 0, 1;\n }\n\n if (fx == -1.0f || fy == -1.0f)\n {\n K_rect(0,0) = mParameters.mu();\n K_rect(1,1) = mParameters.mv();\n }\n\n Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n Eigen::Matrix3f R, R_inv;\n cv::cv2eigen(rmat, R);\n R_inv = R.inverse();\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n Eigen::Vector3f xo;\n xo << u, v, 1;\n\n Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n Eigen::Vector2d p;\n spaceToPlane(uo.cast(), p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n cv::Mat K_rect_cv;\n cv::eigen2cv(K_rect, K_rect_cv);\n return K_rect_cv;\n}\n\nint\nEquidistantCamera::parameterCount(void) const\n{\n return 8;\n}\n\nconst EquidistantCamera::Parameters&\nEquidistantCamera::getParameters(void) const\n{\n return mParameters;\n}\n\nvoid\nEquidistantCamera::setParameters(const EquidistantCamera::Parameters& parameters)\n{\n mParameters = parameters;\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.mu();\n m_inv_K13 = -mParameters.u0() / mParameters.mu();\n m_inv_K22 = 1.0 / mParameters.mv();\n m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nvoid\nEquidistantCamera::readParameters(const std::vector& parameterVec)\n{\n if (parameterVec.size() != parameterCount())\n {\n return;\n }\n\n Parameters params = getParameters();\n\n params.k2() = parameterVec.at(0);\n params.k3() = parameterVec.at(1);\n params.k4() = parameterVec.at(2);\n params.k5() = parameterVec.at(3);\n params.mu() = parameterVec.at(4);\n params.mv() = parameterVec.at(5);\n params.u0() = parameterVec.at(6);\n params.v0() = parameterVec.at(7);\n\n setParameters(params);\n}\n\nvoid\nEquidistantCamera::writeParameters(std::vector& parameterVec) const\n{\n parameterVec.resize(parameterCount());\n parameterVec.at(0) = mParameters.k2();\n parameterVec.at(1) = mParameters.k3();\n parameterVec.at(2) = mParameters.k4();\n parameterVec.at(3) = mParameters.k5();\n parameterVec.at(4) = mParameters.mu();\n parameterVec.at(5) = mParameters.mv();\n parameterVec.at(6) = mParameters.u0();\n parameterVec.at(7) = mParameters.v0();\n}\n\nvoid\nEquidistantCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nEquidistantCamera::parametersToString(void) const\n{\n std::ostringstream oss;\n oss << mParameters;\n\n return oss.str();\n}\n\nvoid\nEquidistantCamera::fitOddPoly(const std::vector& x, const std::vector& y,\n int n, std::vector& coeffs) const\n{\n std::vector pows;\n for (int i = 1; i <= n; i += 2)\n {\n pows.push_back(i);\n }\n\n Eigen::MatrixXd X(x.size(), pows.size());\n Eigen::MatrixXd Y(y.size(), 1);\n for (size_t i = 0; i < x.size(); ++i)\n {\n for (size_t j = 0; j < pows.size(); ++j)\n {\n X(i,j) = pow(x.at(i), pows.at(j));\n }\n Y(i,0) = y.at(i);\n }\n\n Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y;\n\n coeffs.resize(A.rows());\n for (int i = 0; i < A.rows(); ++i)\n {\n coeffs.at(i) = A(i,0);\n }\n}\n\nvoid\nEquidistantCamera::backprojectSymmetric(const Eigen::Vector2d& p_u,\n double& theta, double& phi) const\n{\n double tol = 1e-10;\n double p_u_norm = p_u.norm();\n\n if (p_u_norm < 1e-10)\n {\n phi = 0.0;\n }\n else\n {\n phi = atan2(p_u(1), p_u(0));\n }\n\n int npow = 9;\n if (mParameters.k5() == 0.0)\n {\n npow -= 2;\n }\n if (mParameters.k4() == 0.0)\n {\n npow -= 2;\n }\n if (mParameters.k3() == 0.0)\n {\n npow -= 2;\n }\n if (mParameters.k2() == 0.0)\n {\n npow -= 2;\n }\n\n Eigen::MatrixXd coeffs(npow + 1, 1);\n coeffs.setZero();\n coeffs(0) = -p_u_norm;\n coeffs(1) = 1.0;\n\n if (npow >= 3)\n {\n coeffs(3) = mParameters.k2();\n }\n if (npow >= 5)\n {\n coeffs(5) = mParameters.k3();\n }\n if (npow >= 7)\n {\n coeffs(7) = mParameters.k4();\n }\n if (npow >= 9)\n {\n coeffs(9) = mParameters.k5();\n }\n\n if (npow == 1)\n {\n theta = p_u_norm;\n }\n else\n {\n // Get eigenvalues of companion matrix corresponding to polynomial.\n // Eigenvalues correspond to roots of polynomial.\n Eigen::MatrixXd A(npow, npow);\n A.setZero();\n A.block(1, 0, npow - 1, npow - 1).setIdentity();\n A.col(npow - 1) = - coeffs.block(0, 0, npow, 1) / coeffs(npow);\n\n Eigen::EigenSolver es(A);\n Eigen::MatrixXcd eigval = es.eigenvalues();\n\n std::vector thetas;\n for (int i = 0; i < eigval.rows(); ++i)\n {\n if (fabs(eigval(i).imag()) > tol)\n {\n continue;\n }\n\n double t = eigval(i).real();\n\n if (t < -tol)\n {\n continue;\n }\n else if (t < 0.0)\n {\n t = 0.0;\n }\n\n thetas.push_back(t);\n }\n\n if (thetas.empty())\n {\n theta = p_u_norm;\n }\n else\n {\n theta = *std::min_element(thetas.begin(), thetas.end());\n }\n }\n}\n\n}\n" }, { "path": "src/utils/camodocal/src/camera_models/PinholeCamera.cc", "content": "#include \"camodocal/camera_models/PinholeCamera.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nPinholeCamera::Parameters::Parameters()\n : Camera::Parameters(PINHOLE)\n , m_k1(0.0)\n , m_k2(0.0)\n , m_p1(0.0)\n , m_p2(0.0)\n , m_fx(0.0)\n , m_fy(0.0)\n , m_cx(0.0)\n , m_cy(0.0)\n{\n\n}\n\nPinholeCamera::Parameters::Parameters(const std::string& cameraName,\n int w, int h,\n double k1, double k2,\n double p1, double p2,\n double fx, double fy,\n double cx, double cy)\n : Camera::Parameters(PINHOLE, cameraName, w, h)\n , m_k1(k1)\n , m_k2(k2)\n , m_p1(p1)\n , m_p2(p2)\n , m_fx(fx)\n , m_fy(fy)\n , m_cx(cx)\n , m_cy(cy)\n{\n}\n\ndouble&\nPinholeCamera::Parameters::k1(void)\n{\n return m_k1;\n}\n\ndouble&\nPinholeCamera::Parameters::k2(void)\n{\n return m_k2;\n}\n\ndouble&\nPinholeCamera::Parameters::p1(void)\n{\n return m_p1;\n}\n\ndouble&\nPinholeCamera::Parameters::p2(void)\n{\n return m_p2;\n}\n\ndouble&\nPinholeCamera::Parameters::fx(void)\n{\n return m_fx;\n}\n\ndouble&\nPinholeCamera::Parameters::fy(void)\n{\n return m_fy;\n}\n\ndouble&\nPinholeCamera::Parameters::cx(void)\n{\n return m_cx;\n}\n\ndouble&\nPinholeCamera::Parameters::cy(void)\n{\n return m_cy;\n}\n\ndouble\nPinholeCamera::Parameters::k1(void) const\n{\n return m_k1;\n}\n\ndouble\nPinholeCamera::Parameters::k2(void) const\n{\n return m_k2;\n}\n\ndouble\nPinholeCamera::Parameters::p1(void) const\n{\n return m_p1;\n}\n\ndouble\nPinholeCamera::Parameters::p2(void) const\n{\n return m_p2;\n}\n\ndouble\nPinholeCamera::Parameters::fx(void) const\n{\n return m_fx;\n}\n\ndouble\nPinholeCamera::Parameters::fy(void) const\n{\n return m_fy;\n}\n\ndouble\nPinholeCamera::Parameters::cx(void) const\n{\n return m_cx;\n}\n\ndouble\nPinholeCamera::Parameters::cy(void) const\n{\n return m_cy;\n}\n\nbool\nPinholeCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return false;\n }\n\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (sModelType.compare(\"PINHOLE\") != 0)\n {\n return false;\n }\n }\n\n m_modelType = PINHOLE;\n fs[\"camera_name\"] >> m_cameraName;\n m_imageWidth = static_cast(fs[\"image_width\"]);\n m_imageHeight = static_cast(fs[\"image_height\"]);\n\n cv::FileNode n = fs[\"distortion_parameters\"];\n m_k1 = static_cast(n[\"k1\"]);\n m_k2 = static_cast(n[\"k2\"]);\n m_p1 = static_cast(n[\"p1\"]);\n m_p2 = static_cast(n[\"p2\"]);\n\n n = fs[\"projection_parameters\"];\n m_fx = static_cast(n[\"fx\"]);\n m_fy = static_cast(n[\"fy\"]);\n m_cx = static_cast(n[\"cx\"]);\n m_cy = static_cast(n[\"cy\"]);\n\n return true;\n}\n\nvoid\nPinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n fs << \"model_type\" << \"PINHOLE\";\n fs << \"camera_name\" << m_cameraName;\n fs << \"image_width\" << m_imageWidth;\n fs << \"image_height\" << m_imageHeight;\n\n // radial distortion: k1, k2\n // tangential distortion: p1, p2\n fs << \"distortion_parameters\";\n fs << \"{\" << \"k1\" << m_k1\n << \"k2\" << m_k2\n << \"p1\" << m_p1\n << \"p2\" << m_p2 << \"}\";\n\n // projection: fx, fy, cx, cy\n fs << \"projection_parameters\";\n fs << \"{\" << \"fx\" << m_fx\n << \"fy\" << m_fy\n << \"cx\" << m_cx\n << \"cy\" << m_cy << \"}\";\n\n fs.release();\n}\n\nPinholeCamera::Parameters&\nPinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other)\n{\n if (this != &other)\n {\n m_modelType = other.m_modelType;\n m_cameraName = other.m_cameraName;\n m_imageWidth = other.m_imageWidth;\n m_imageHeight = other.m_imageHeight;\n m_k1 = other.m_k1;\n m_k2 = other.m_k2;\n m_p1 = other.m_p1;\n m_p2 = other.m_p2;\n m_fx = other.m_fx;\n m_fy = other.m_fy;\n m_cx = other.m_cx;\n m_cy = other.m_cy;\n }\n\n return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const PinholeCamera::Parameters& params)\n{\n out << \"Camera Parameters:\" << std::endl;\n out << \" model_type \" << \"PINHOLE\" << std::endl;\n out << \" camera_name \" << params.m_cameraName << std::endl;\n out << \" image_width \" << params.m_imageWidth << std::endl;\n out << \" image_height \" << params.m_imageHeight << std::endl;\n\n // radial distortion: k1, k2\n // tangential distortion: p1, p2\n out << \"Distortion Parameters\" << std::endl;\n out << \" k1 \" << params.m_k1 << std::endl\n << \" k2 \" << params.m_k2 << std::endl\n << \" p1 \" << params.m_p1 << std::endl\n << \" p2 \" << params.m_p2 << std::endl;\n\n // projection: fx, fy, cx, cy\n out << \"Projection Parameters\" << std::endl;\n out << \" fx \" << params.m_fx << std::endl\n << \" fy \" << params.m_fy << std::endl\n << \" cx \" << params.m_cx << std::endl\n << \" cy \" << params.m_cy << std::endl;\n\n return out;\n}\n\nPinholeCamera::PinholeCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n , m_noDistortion(true)\n{\n\n}\n\nPinholeCamera::PinholeCamera(const std::string& cameraName,\n int imageWidth, int imageHeight,\n double k1, double k2, double p1, double p2,\n double fx, double fy, double cx, double cy)\n : mParameters(cameraName, imageWidth, imageHeight,\n k1, k2, p1, p2, fx, fy, cx, cy)\n{\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.fx();\n m_inv_K13 = -mParameters.cx() / mParameters.fx();\n m_inv_K22 = 1.0 / mParameters.fy();\n m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nPinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params)\n : mParameters(params)\n{\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n // Inverse camera projection matrix parameters\n m_inv_K11 = 1.0 / mParameters.fx();\n m_inv_K13 = -mParameters.cx() / mParameters.fx();\n m_inv_K22 = 1.0 / mParameters.fy();\n m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nCamera::ModelType\nPinholeCamera::modelType(void) const\n{\n return mParameters.modelType();\n}\n\nconst std::string&\nPinholeCamera::cameraName(void) const\n{\n return mParameters.cameraName();\n}\n\nint\nPinholeCamera::imageWidth(void) const\n{\n return mParameters.imageWidth();\n}\n\nint\nPinholeCamera::imageHeight(void) const\n{\n return mParameters.imageHeight();\n}\n\nvoid\nPinholeCamera::estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints)\n{\n // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000\n\n Parameters params = getParameters();\n\n params.k1() = 0.0;\n params.k2() = 0.0;\n params.p1() = 0.0;\n params.p2() = 0.0;\n\n double cx = params.imageWidth() / 2.0;\n double cy = params.imageHeight() / 2.0;\n params.cx() = cx;\n params.cy() = cy;\n\n size_t nImages = imagePoints.size();\n\n cv::Mat A(nImages * 2, 2, CV_64F);\n cv::Mat b(nImages * 2, 1, CV_64F);\n\n for (size_t i = 0; i < nImages; ++i)\n {\n const std::vector& oPoints = objectPoints.at(i);\n\n std::vector M(oPoints.size());\n for (size_t j = 0; j < M.size(); ++j)\n {\n M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y);\n }\n\n cv::Mat H = cv::findHomography(M, imagePoints.at(i));\n\n H.at(0,0) -= H.at(2,0) * cx;\n H.at(0,1) -= H.at(2,1) * cx;\n H.at(0,2) -= H.at(2,2) * cx;\n H.at(1,0) -= H.at(2,0) * cy;\n H.at(1,1) -= H.at(2,1) * cy;\n H.at(1,2) -= H.at(2,2) * cy;\n\n double h[3], v[3], d1[3], d2[3];\n double n[4] = {0,0,0,0};\n\n for (int j = 0; j < 3; ++j)\n {\n double t0 = H.at(j,0);\n double t1 = H.at(j,1);\n h[j] = t0; v[j] = t1;\n d1[j] = (t0 + t1) * 0.5;\n d2[j] = (t0 - t1) * 0.5;\n n[0] += t0 * t0; n[1] += t1 * t1;\n n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j];\n }\n\n for (int j = 0; j < 4; ++j)\n {\n n[j] = 1.0 / sqrt(n[j]);\n }\n\n for (int j = 0; j < 3; ++j)\n {\n h[j] *= n[0]; v[j] *= n[1];\n d1[j] *= n[2]; d2[j] *= n[3];\n }\n\n A.at(i * 2, 0) = h[0] * v[0];\n A.at(i * 2, 1) = h[1] * v[1];\n A.at(i * 2 + 1, 0) = d1[0] * d2[0];\n A.at(i * 2 + 1, 1) = d1[1] * d2[1];\n b.at(i * 2, 0) = -h[2] * v[2];\n b.at(i * 2 + 1, 0) = -d1[2] * d2[2];\n }\n\n cv::Mat f(2, 1, CV_64F);\n cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU);\n\n params.fx() = sqrt(fabs(1.0 / f.at(0)));\n params.fy() = sqrt(fabs(1.0 / f.at(1)));\n\n setParameters(params);\n}\n\n/**\n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nPinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n liftProjective(p, P);\n\n P.normalize();\n}\n\n/**\n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nPinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n //double lambda;\n\n // Lift points to normalised plane\n mx_d = m_inv_K11 * p(0) + m_inv_K13;\n my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n if (m_noDistortion)\n {\n mx_u = mx_d;\n my_u = my_d;\n }\n else\n {\n if (0)\n {\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n // Apply inverse distortion model\n // proposed by Heikkila\n mx2_d = mx_d*mx_d;\n my2_d = my_d*my_d;\n mxy_d = mx_d*my_d;\n rho2_d = mx2_d+my2_d;\n rho4_d = rho2_d*rho2_d;\n radDist_d = k1*rho2_d+k2*rho4_d;\n Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n mx_u = mx_d - inv_denom_d*Dx_d;\n my_u = my_d - inv_denom_d*Dy_d;\n }\n else\n {\n // Recursive distortion model\n int n = 8;\n Eigen::Vector2d d_u;\n distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n // Approximate value\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n\n for (int i = 1; i < n; ++i)\n {\n distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n mx_u = mx_d - d_u(0);\n my_u = my_d - d_u(1);\n }\n }\n }\n\n // Obtain a projective ray\n P << mx_u, my_u, 1.0;\n}\n\n\n/**\n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nPinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n Eigen::Vector2d p_u, p_d;\n\n // Project points to the normalised plane\n p_u << P(0) / P(2), P(1) / P(2);\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n // Apply generalised projection matrix\n p << mParameters.fx() * p_d(0) + mParameters.cx(),\n mParameters.fy() * p_d(1) + mParameters.cy();\n}\n\n#if 0\n/**\n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nPinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n Eigen::Matrix& J) const\n{\n Eigen::Vector2d p_u, p_d;\n double norm, inv_denom;\n double dxdmx, dydmx, dxdmy, dydmy;\n\n norm = P.norm();\n // Project points to the normalised plane\n inv_denom = 1.0 / P(2);\n p_u << inv_denom * P(0), inv_denom * P(1);\n\n // Calculate jacobian\n double dudx = inv_denom;\n double dvdx = 0.0;\n double dudy = 0.0;\n double dvdy = inv_denom;\n inv_denom = - inv_denom * inv_denom;\n double dudz = P(0) * inv_denom;\n double dvdz = P(1) * inv_denom;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n double fx = mParameters.fx();\n double fy = mParameters.fy();\n\n // Make the product of the jacobians\n // and add projection matrix jacobian\n inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse\n dvdx = fy * (dudx * dydmx + dvdx * dydmy);\n dudx = inv_denom;\n\n inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse\n dvdy = fy * (dudy * dydmx + dvdy * dydmy);\n dudy = inv_denom;\n\n inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse\n dvdz = fy * (dudz * dydmx + dvdz * dydmy);\n dudz = inv_denom;\n\n // Apply generalised projection matrix\n p << fx * p_d(0) + mParameters.cx(),\n fy * p_d(1) + mParameters.cy();\n\n J << dudx, dudy, dudz,\n dvdx, dvdy, dvdz;\n}\n#endif\n\n/**\n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nPinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n Eigen::Vector2d p_d;\n\n if (m_noDistortion)\n {\n p_d = p_u;\n }\n else\n {\n // Apply distortion\n Eigen::Vector2d d_u;\n distortion(p_u, d_u);\n p_d = p_u + d_u;\n }\n\n // Apply generalised projection matrix\n p << mParameters.fx() * p_d(0) + mParameters.cx(),\n mParameters.fy() * p_d(1) + mParameters.cy();\n}\n\n/**\n * \\brief Apply distortion to input point (from the normalised plane)\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nPinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const\n{\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n mx2_u = p_u(0) * p_u(0);\n my2_u = p_u(1) * p_u(1);\n mxy_u = p_u(0) * p_u(1);\n rho2_u = mx2_u + my2_u;\n rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n}\n\n/**\n * \\brief Apply distortion to input point (from the normalised plane)\n * and calculate Jacobian\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nPinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n Eigen::Matrix2d& J) const\n{\n double k1 = mParameters.k1();\n double k2 = mParameters.k2();\n double p1 = mParameters.p1();\n double p2 = mParameters.p2();\n\n double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n mx2_u = p_u(0) * p_u(0);\n my2_u = p_u(1) * p_u(1);\n mxy_u = p_u(0) * p_u(1);\n rho2_u = mx2_u + my2_u;\n rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n\n double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0);\n double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1);\n double dxdmy = dydmx;\n double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0);\n\n J << dxdmx, dxdmy,\n dydmx, dydmy;\n}\n\nvoid\nPinholeCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n Eigen::Vector3d P;\n P << mx_u, my_u, 1.0;\n\n Eigen::Vector2d p;\n spaceToPlane(P, p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nPinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx, float fy,\n cv::Size imageSize,\n float cx, float cy,\n cv::Mat rmat) const\n{\n if (imageSize == cv::Size(0, 0))\n {\n imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n }\n\n cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n Eigen::Matrix3f R, R_inv;\n cv::cv2eigen(rmat, R);\n R_inv = R.inverse();\n\n // assume no skew\n Eigen::Matrix3f K_rect;\n\n if (cx == -1.0f || cy == -1.0f)\n {\n K_rect << fx, 0, imageSize.width / 2,\n 0, fy, imageSize.height / 2,\n 0, 0, 1;\n }\n else\n {\n K_rect << fx, 0, cx,\n 0, fy, cy,\n 0, 0, 1;\n }\n\n if (fx == -1.0f || fy == -1.0f)\n {\n K_rect(0,0) = mParameters.fx();\n K_rect(1,1) = mParameters.fy();\n }\n\n Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n Eigen::Vector3f xo;\n xo << u, v, 1;\n\n Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n Eigen::Vector2d p;\n spaceToPlane(uo.cast(), p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n cv::Mat K_rect_cv;\n cv::eigen2cv(K_rect, K_rect_cv);\n return K_rect_cv;\n}\n\nint\nPinholeCamera::parameterCount(void) const\n{\n return 8;\n}\n\nconst PinholeCamera::Parameters&\nPinholeCamera::getParameters(void) const\n{\n return mParameters;\n}\n\nvoid\nPinholeCamera::setParameters(const PinholeCamera::Parameters& parameters)\n{\n mParameters = parameters;\n\n if ((mParameters.k1() == 0.0) &&\n (mParameters.k2() == 0.0) &&\n (mParameters.p1() == 0.0) &&\n (mParameters.p2() == 0.0))\n {\n m_noDistortion = true;\n }\n else\n {\n m_noDistortion = false;\n }\n\n m_inv_K11 = 1.0 / mParameters.fx();\n m_inv_K13 = -mParameters.cx() / mParameters.fx();\n m_inv_K22 = 1.0 / mParameters.fy();\n m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nvoid\nPinholeCamera::readParameters(const std::vector& parameterVec)\n{\n if ((int)parameterVec.size() != parameterCount())\n {\n return;\n }\n\n Parameters params = getParameters();\n\n params.k1() = parameterVec.at(0);\n params.k2() = parameterVec.at(1);\n params.p1() = parameterVec.at(2);\n params.p2() = parameterVec.at(3);\n params.fx() = parameterVec.at(4);\n params.fy() = parameterVec.at(5);\n params.cx() = parameterVec.at(6);\n params.cy() = parameterVec.at(7);\n\n setParameters(params);\n}\n\nvoid\nPinholeCamera::writeParameters(std::vector& parameterVec) const\n{\n parameterVec.resize(parameterCount());\n parameterVec.at(0) = mParameters.k1();\n parameterVec.at(1) = mParameters.k2();\n parameterVec.at(2) = mParameters.p1();\n parameterVec.at(3) = mParameters.p2();\n parameterVec.at(4) = mParameters.fx();\n parameterVec.at(5) = mParameters.fy();\n parameterVec.at(6) = mParameters.cx();\n parameterVec.at(7) = mParameters.cy();\n}\n\nvoid\nPinholeCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nPinholeCamera::parametersToString(void) const\n{\n std::ostringstream oss;\n oss << mParameters;\n\n return oss.str();\n}\n\n}\n" }, { "path": "src/utils/camodocal/src/camera_models/ScaramuzzaCamera.cc", "content": "#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/gpl/gpl.h\"\n\n\nEigen::VectorXd polyfit(Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, int poly_order) {\n assert(poly_order > 0);\n assert(xVec.size() > poly_order);\n assert(xVec.size() == yVec.size());\n\n Eigen::MatrixXd A(xVec.size(), poly_order+1);\n Eigen::VectorXd B(xVec.size());\n\n for(int i = 0; i < xVec.size(); ++i) {\n const double x = xVec(i);\n const double y = yVec(i);\n\n double x_pow_k = 1.0;\n\n for(int k=0; k<=poly_order; ++k) {\n A(i,k) = x_pow_k;\n x_pow_k *= x;\n }\n\n B(i) = y;\n }\n\n Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);\n Eigen::VectorXd x = svd.solve(B);\n\n return x;\n}\n\nnamespace camodocal\n{\n\nOCAMCamera::Parameters::Parameters()\n : Camera::Parameters(SCARAMUZZA)\n , m_C(0.0)\n , m_D(0.0)\n , m_E(0.0)\n , m_center_x(0.0)\n , m_center_y(0.0)\n{\n memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE);\n memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);\n}\n\n\n\nbool\nOCAMCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n if (!fs.isOpened())\n {\n return false;\n }\n\n if (!fs[\"model_type\"].isNone())\n {\n std::string sModelType;\n fs[\"model_type\"] >> sModelType;\n\n if (!boost::iequals(sModelType, \"scaramuzza\"))\n {\n return false;\n }\n }\n\n m_modelType = SCARAMUZZA;\n fs[\"camera_name\"] >> m_cameraName;\n m_imageWidth = static_cast(fs[\"image_width\"]);\n m_imageHeight = static_cast(fs[\"image_height\"]);\n\n cv::FileNode n = fs[\"poly_parameters\"];\n for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n m_poly[i] = static_cast(n[std::string(\"p\") + boost::lexical_cast(i)]);\n\n n = fs[\"inv_poly_parameters\"];\n for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n m_inv_poly[i] = static_cast(n[std::string(\"p\") + boost::lexical_cast(i)]);\n\n n = fs[\"affine_parameters\"];\n m_C = static_cast(n[\"ac\"]);\n m_D = static_cast(n[\"ad\"]);\n m_E = static_cast(n[\"ae\"]);\n\n m_center_x = static_cast(n[\"cx\"]);\n m_center_y = static_cast(n[\"cy\"]);\n\n return true;\n}\n\nvoid\nOCAMCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n fs << \"model_type\" << \"scaramuzza\";\n fs << \"camera_name\" << m_cameraName;\n fs << \"image_width\" << m_imageWidth;\n fs << \"image_height\" << m_imageHeight;\n\n fs << \"poly_parameters\";\n fs << \"{\";\n for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n fs << std::string(\"p\") + boost::lexical_cast(i) << m_poly[i];\n fs << \"}\";\n\n fs << \"inv_poly_parameters\";\n fs << \"{\";\n for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n fs << std::string(\"p\") + boost::lexical_cast(i) << m_inv_poly[i];\n fs << \"}\";\n\n fs << \"affine_parameters\";\n fs << \"{\" << \"ac\" << m_C\n << \"ad\" << m_D\n << \"ae\" << m_E\n << \"cx\" << m_center_x\n << \"cy\" << m_center_y << \"}\";\n\n fs.release();\n}\n\nOCAMCamera::Parameters&\nOCAMCamera::Parameters::operator=(const OCAMCamera::Parameters& other)\n{\n if (this != &other)\n {\n m_modelType = other.m_modelType;\n m_cameraName = other.m_cameraName;\n m_imageWidth = other.m_imageWidth;\n m_imageHeight = other.m_imageHeight;\n m_C = other.m_C;\n m_D = other.m_D;\n m_E = other.m_E;\n m_center_x = other.m_center_x;\n m_center_y = other.m_center_y;\n\n memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE);\n memcpy(m_inv_poly, other.m_inv_poly, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);\n }\n\n return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const OCAMCamera::Parameters& params)\n{\n out << \"Camera Parameters:\" << std::endl;\n out << \" model_type \" << \"scaramuzza\" << std::endl;\n out << \" camera_name \" << params.m_cameraName << std::endl;\n out << \" image_width \" << params.m_imageWidth << std::endl;\n out << \" image_height \" << params.m_imageHeight << std::endl;\n\n out << std::fixed << std::setprecision(10);\n\n out << \"Poly Parameters\" << std::endl;\n for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n out << std::string(\"p\") + boost::lexical_cast(i) << \": \" << params.m_poly[i] << std::endl;\n\n out << \"Inverse Poly Parameters\" << std::endl;\n for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n out << std::string(\"p\") + boost::lexical_cast(i) << \": \" << params.m_inv_poly[i] << std::endl;\n\n out << \"Affine Parameters\" << std::endl;\n out << \" ac \" << params.m_C << std::endl\n << \" ad \" << params.m_D << std::endl\n << \" ae \" << params.m_E << std::endl;\n out << \" cx \" << params.m_center_x << std::endl\n << \" cy \" << params.m_center_y << std::endl;\n\n return out;\n}\n\nOCAMCamera::OCAMCamera()\n : m_inv_scale(0.0)\n{\n\n}\n\nOCAMCamera::OCAMCamera(const OCAMCamera::Parameters& params)\n : mParameters(params)\n{\n m_inv_scale = 1.0 / (params.C() - params.D() * params.E());\n}\n\nCamera::ModelType\nOCAMCamera::modelType(void) const\n{\n return mParameters.modelType();\n}\n\nconst std::string&\nOCAMCamera::cameraName(void) const\n{\n return mParameters.cameraName();\n}\n\nint\nOCAMCamera::imageWidth(void) const\n{\n return mParameters.imageWidth();\n}\n\nint\nOCAMCamera::imageHeight(void) const\n{\n return mParameters.imageHeight();\n}\n\nvoid\nOCAMCamera::estimateIntrinsics(const cv::Size& boardSize,\n const std::vector< std::vector >& objectPoints,\n const std::vector< std::vector >& imagePoints)\n{\n // std::cout << \"OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED\" << std::endl;\n // throw std::string(\"OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED\");\n\n // Reference: Page 30 of\n // \" Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion Estimation, ETH Zurich. Thesis no. 17635.\"\n // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf\n // Matlab code: calibrate.m\n\n // First, estimate every image's extrinsics parameters\n std::vector< Eigen::Matrix3d > RList;\n std::vector< Eigen::Vector3d > TList;\n\n RList.reserve(imagePoints.size());\n TList.reserve(imagePoints.size());\n\n // i-th image\n for (size_t image_index = 0; image_index < imagePoints.size(); ++image_index)\n {\n const std::vector& objPts = objectPoints.at(image_index);\n const std::vector& imgPts = imagePoints.at(image_index);\n\n assert(objPts.size() == imgPts.size());\n assert(objPts.size() == static_cast(boardSize.width * boardSize.height));\n\n Eigen::MatrixXd M(objPts.size(), 6);\n\n for(size_t corner_index = 0; corner_index < objPts.size(); ++corner_index) {\n double X = objPts.at(corner_index).x;\n double Y = objPts.at(corner_index).y;\n assert(objPts.at(corner_index).z==0.0);\n \n double u = imgPts.at(corner_index).x;\n double v = imgPts.at(corner_index).y;\n\n M(corner_index, 0) = -v * X;\n M(corner_index, 1) = -v * Y;\n M(corner_index, 2) = u * X;\n M(corner_index, 3) = u * Y;\n M(corner_index, 4) = -v;\n M(corner_index, 5) = u;\n }\n\n Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);\n assert(svd.matrixV().cols() == 6);\n Eigen::VectorXd h = -svd.matrixV().col(5);\n\n // scaled version of R and T\n const double sr11 = h(0);\n const double sr12 = h(1);\n const double sr21 = h(2);\n const double sr22 = h(3);\n const double st1 = h(4);\n const double st2 = h(5);\n\n const double AA = square(sr11*sr12 + sr21*sr22);\n const double BB = square(sr11) + square(sr21);\n const double CC = square(sr12) + square(sr22);\n\n const double sr32_squared_1 = (- (CC-BB) + sqrt(square(CC-BB) + 4.0 * AA)) / 2.0;\n const double sr32_squared_2 = (- (CC-BB) - sqrt(square(CC-BB) + 4.0 * AA)) / 2.0;\n\n// printf(\"rst = %.12f\\n\", sr32_squared_1*sr32_squared_1 + (CC-BB)*sr32_squared_1 - AA);\n\n std::vector sr32_squared_values;\n if (sr32_squared_1 > 0) sr32_squared_values.push_back(sr32_squared_1);\n if (sr32_squared_2 > 0) sr32_squared_values.push_back(sr32_squared_2);\n assert(!sr32_squared_values.empty());\n\n std::vector sr32_values;\n std::vector sr31_values;\n for (auto sr32_squared : sr32_squared_values) {\n for(int sign = -1; sign <= 1; sign += 2) {\n const double sr32 = static_cast(sign) * std::sqrt(sr32_squared);\n sr32_values.push_back( sr32 );\n if (sr32_squared == 0.0) {\n // sr31 can be calculated through norm equality, \n // but it has positive and negative posibilities\n // positive one\n sr31_values.push_back(std::sqrt(CC-BB));\n // negative one\n sr32_values.push_back( sr32 );\n sr31_values.push_back(-std::sqrt(CC-BB));\n \n break; // skip the same situation\n } else {\n // sr31 can be calculated throught dot product == 0\n sr31_values.push_back(- (sr11*sr12 + sr21*sr22) / sr32);\n }\n }\n }\n\n // std::cout << \"h= \" << std::setprecision(12) << h.transpose() << std::endl;\n // std::cout << \"length: \" << sr32_values.size() << \" & \" << sr31_values.size() << std::endl;\n\n assert(!sr31_values.empty());\n assert(sr31_values.size() == sr32_values.size());\n \n std::vector H_values;\n for(size_t i=0;i H_candidates;\n\n for (auto& H : H_values)\n {\n Eigen::MatrixXd A_mat(2 * imagePoints.at(image_index).size(), 4);\n Eigen::VectorXd B_vec(2 * imagePoints.at(image_index).size());\n A_mat.setZero();\n B_vec.setZero();\n\n size_t line_index = 0;\n\n // iterate images\n const double& r11 = H(0,0);\n const double& r12 = H(0,1);\n // const double& r13 = H(0,2);\n const double& r21 = H(1,0);\n const double& r22 = H(1,1);\n // const double& r23 = H(1,2);\n const double& r31 = H(2,0);\n const double& r32 = H(2,1);\n // const double& r33 = H(2,2);\n const double& t1 = H(0);\n const double& t2 = H(1);\n \n // iterate chessboard corners in the image\n for(size_t j=0; j(A_mat.rows()));\n\n // pseudo-inverse for polynomial parameters and all t3s\n {\n Eigen::JacobiSVD svd(A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);\n\n Eigen::VectorXd x = svd.solve(B_vec);\n\n // std::cout << \"x(poly and t3) = \" << x << std::endl;\n\n if (x(2) > 0 && x(3) > 0) {\n H_candidates.push_back(H);\n }\n }\n }\n\n // printf(\"H_candidates.size()=%zu\\n\", H_candidates.size());\n assert(H_candidates.size()==1);\n\n Eigen::Matrix3d& H = H_candidates.front();\n\n Eigen::Matrix3d R; \n R.col(0) = H.col(0); \n R.col(1) = H.col(1); \n R.col(2) = H.col(0).cross(H.col(1)); \n\n Eigen::Vector3d T = H.col(2);\n RList.push_back(R);\n TList.push_back(T);\n\n // std::cout << \"#\" << image_index << \" frame\" << \" R =\" << R << \" \\nT = \" << T.transpose() << std::endl;\n }\n\n // Second, estimate camera intrinsic parameters and all t3\n Eigen::MatrixXd A_mat(2 * imagePoints.size() * imagePoints.at(0).size(), SCARAMUZZA_POLY_SIZE-1 + imagePoints.size());\n Eigen::VectorXd B_vec(2 * imagePoints.size() * imagePoints.at(0).size());\n A_mat.setZero();\n B_vec.setZero();\n\n size_t line_index = 0;\n\n // iterate images\n for(size_t i = 0; i < imagePoints.size(); ++i) {\n const double& r11 = RList.at(i)(0,0);\n const double& r12 = RList.at(i)(0,1);\n // const double& r13 = RList.at(i)(0,2);\n const double& r21 = RList.at(i)(1,0);\n const double& r22 = RList.at(i)(1,1);\n // const double& r23 = RList.at(i)(1,2);\n const double& r31 = RList.at(i)(2,0);\n const double& r32 = RList.at(i)(2,1);\n // const double& r33 = RList.at(i)(2,2);\n const double& t1 = TList.at(i)(0);\n const double& t2 = TList.at(i)(1);\n \n // iterate chessboard corners in the image\n for(size_t j=0; j(A_mat.rows()));\n\n Eigen::Matrix poly_coeff;\n // pseudo-inverse for polynomial parameters and all t3s\n {\n Eigen::JacobiSVD svd(A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);\n\n Eigen::VectorXd x = svd.solve(B_vec);\n\n poly_coeff[0] = x(0);\n poly_coeff[1] = 0.0;\n for(int i=1;i(SCARAMUZZA_POLY_SIZE-1+TList.size()));\n }\n\n Parameters params = getParameters();\n\n // Affine matrix A is constructed as [C D; E 1]\n params.C() = 1.0;\n params.D() = 0.0;\n params.E() = 0.0;\n\n params.center_x() = params.imageWidth() / 2.0;\n params.center_y() = params.imageHeight() / 2.0;\n \n for(size_t i=0; i rou_vec;\n std::vector z_vec;\n for(double rou = 0.0; rou <= (params.imageWidth() + params.imageHeight())/2; rou += 0.1) {\n double rou_pow_k = 1.0;\n double z = 0.0;\n\n for (int k = 0; k < SCARAMUZZA_POLY_SIZE; k++)\n {\n z += rou_pow_k * params.poly(k);\n rou_pow_k *= rou;\n }\n\n rou_vec.push_back(rou);\n z_vec.push_back(z);\n }\n\n assert(rou_vec.size() == z_vec.size());\n Eigen::VectorXd xVec(rou_vec.size());\n Eigen::VectorXd yVec(rou_vec.size());\n\n for(size_t i=0; i(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n#endif\n\ncv::Mat\nOCAMCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n float fx, float fy,\n cv::Size imageSize,\n float cx, float cy,\n cv::Mat rmat) const\n{\n if (imageSize == cv::Size(0, 0))\n {\n imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n }\n\n cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n Eigen::Matrix3f K_rect;\n\n K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx,\n 0, fy, cy < 0 ? imageSize.height / 2 : cy,\n 0, 0, 1;\n\n if (fx < 0 || fy < 0)\n {\n throw std::string(std::string(__FUNCTION__) + \": Focal length must be specified\");\n }\n\n Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n Eigen::Matrix3f R, R_inv;\n cv::cv2eigen(rmat, R);\n R_inv = R.inverse();\n\n for (int v = 0; v < imageSize.height; ++v)\n {\n for (int u = 0; u < imageSize.width; ++u)\n {\n Eigen::Vector3f xo;\n xo << u, v, 1;\n\n Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n Eigen::Vector2d p;\n spaceToPlane(uo.cast(), p);\n\n mapX.at(v,u) = p(0);\n mapY.at(v,u) = p(1);\n }\n }\n\n cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n cv::Mat K_rect_cv;\n cv::eigen2cv(K_rect, K_rect_cv);\n return K_rect_cv;\n}\n\nint\nOCAMCamera::parameterCount(void) const\n{\n return SCARAMUZZA_CAMERA_NUM_PARAMS;\n}\n\nconst OCAMCamera::Parameters&\nOCAMCamera::getParameters(void) const\n{\n return mParameters;\n}\n\nvoid\nOCAMCamera::setParameters(const OCAMCamera::Parameters& parameters)\n{\n mParameters = parameters;\n\n m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E());\n}\n\nvoid\nOCAMCamera::readParameters(const std::vector& parameterVec)\n{\n if ((int)parameterVec.size() != parameterCount())\n {\n return;\n }\n\n Parameters params = getParameters();\n\n params.C() = parameterVec.at(0);\n params.D() = parameterVec.at(1);\n params.E() = parameterVec.at(2);\n params.center_x() = parameterVec.at(3);\n params.center_y() = parameterVec.at(4);\n for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n params.poly(i) = parameterVec.at(5+i);\n for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i);\n\n setParameters(params);\n}\n\nvoid\nOCAMCamera::writeParameters(std::vector& parameterVec) const\n{\n parameterVec.resize(parameterCount());\n parameterVec.at(0) = mParameters.C();\n parameterVec.at(1) = mParameters.D();\n parameterVec.at(2) = mParameters.E();\n parameterVec.at(3) = mParameters.center_x();\n parameterVec.at(4) = mParameters.center_y();\n for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n parameterVec.at(5+i) = mParameters.poly(i);\n for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i);\n}\n\nvoid\nOCAMCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nOCAMCamera::parametersToString(void) const\n{\n std::ostringstream oss;\n oss << mParameters;\n\n return oss.str();\n}\n\n}\n" }, { "path": "src/utils/camodocal/src/chessboard/Chessboard.cc", "content": "#include \"camodocal/chessboard/Chessboard.h\"\n\n#include \n#include \n\n#include \"camodocal/chessboard/ChessboardQuad.h\"\n#include \"camodocal/chessboard/Spline.h\"\n\n#define MAX_CONTOUR_APPROX 7\n\nnamespace camodocal\n{\n\nChessboard::Chessboard(cv::Size boardSize, cv::Mat& image)\n : mBoardSize(boardSize)\n , mCornersFound(false)\n{\n if (image.channels() == 1)\n {\n cv::cvtColor(image, mSketch, CV_GRAY2BGR);\n image.copyTo(mImage);\n }\n else\n {\n image.copyTo(mSketch);\n cv::cvtColor(image, mImage, CV_BGR2GRAY);\n }\n}\n\nvoid\nChessboard::findCorners(bool useOpenCV)\n{\n mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners,\n CV_CALIB_CB_ADAPTIVE_THRESH +\n CV_CALIB_CB_NORMALIZE_IMAGE +\n CV_CALIB_CB_FILTER_QUADS +\n CV_CALIB_CB_FAST_CHECK,\n useOpenCV);\n\n if (mCornersFound)\n {\n // draw chessboard corners\n cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound);\n }\n}\n\nconst std::vector&\nChessboard::getCorners(void) const\n{\n return mCorners;\n}\n\nbool\nChessboard::cornersFound(void) const\n{\n return mCornersFound;\n}\n\nconst cv::Mat&\nChessboard::getImage(void) const\n{\n return mImage;\n}\n\nconst cv::Mat&\nChessboard::getSketch(void) const\n{\n return mSketch;\n}\n\nbool\nChessboard::findChessboardCorners(const cv::Mat& image,\n const cv::Size& patternSize,\n std::vector& corners,\n int flags, bool useOpenCV)\n{\n if (useOpenCV)\n {\n return cv::findChessboardCorners(image, patternSize, corners, flags);\n }\n else\n {\n return findChessboardCornersImproved(image, patternSize, corners, flags);\n }\n}\n\nbool\nChessboard::findChessboardCornersImproved(const cv::Mat& image,\n const cv::Size& patternSize,\n std::vector& corners,\n int flags)\n{\n /************************************************************************************\\\n This is improved variant of chessboard corner detection algorithm that\n uses a graph of connected quads. It is based on the code contributed\n by Vladimir Vezhnevets and Philip Gruebele.\n Here is the copyright notice from the original Vladimir's code:\n ===============================================================\n\n The algorithms developed and implemented by Vezhnevets Vldimir\n aka Dead Moroz (vvp@graphics.cs.msu.ru)\n See http://graphics.cs.msu.su/en/research/calibration/opencv.html\n for detailed information.\n\n Reliability additions and modifications made by Philip Gruebele.\n pgruebele@cox.net\n\n Some improvements were made by:\n 1) Martin Rufli - increased chance of correct corner matching\n Rufli, M., Scaramuzza, D., and Siegwart, R. (2008),\n Automatic Detection of Checkerboards on Blurred and Distorted Images,\n Proceedings of the IEEE/RSJ International Conference on\n Intelligent Robots and Systems (IROS 2008), Nice, France, September 2008.\n 2) Lionel Heng - post-detection checks and better thresholding\n\n \\************************************************************************************/\n\n //int bestDilation = -1;\n const int minDilations = 0;\n const int maxDilations = 7;\n\n std::vector outputQuadGroup;\n\n if (image.depth() != CV_8U || image.channels() == 2)\n {\n return false;\n }\n\n if (patternSize.width < 2 || patternSize.height < 2)\n {\n return false;\n }\n\n if (patternSize.width > 127 || patternSize.height > 127)\n {\n return false;\n }\n\n cv::Mat img = image;\n\n // Image histogram normalization and\n // BGR to Grayscale image conversion (if applicable)\n // MARTIN: Set to \"false\"\n if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE))\n {\n cv::Mat norm_img(image.rows, image.cols, CV_8UC1);\n\n if (image.channels() != 1)\n {\n cv::cvtColor(image, norm_img, CV_BGR2GRAY);\n img = norm_img;\n }\n\n if (flags & CV_CALIB_CB_NORMALIZE_IMAGE)\n {\n cv::equalizeHist(image, norm_img);\n img = norm_img;\n }\n }\n\n if (flags & CV_CALIB_CB_FAST_CHECK)\n {\n if (!checkChessboard(img, patternSize))\n {\n return false;\n }\n }\n\n // PART 1: FIND LARGEST PATTERN\n //-----------------------------------------------------------------------\n // Checker patterns are tried to be found by dilating the background and\n // then applying a canny edge finder on the closed contours (checkers).\n // Try one dilation run, but if the pattern is not found, repeat until\n // max_dilations is reached.\n\n int prevSqrSize = 0;\n bool found = false;\n std::vector outputCorners;\n\n for (int k = 0; k < 6; ++k)\n {\n for (int dilations = minDilations; dilations <= maxDilations; ++dilations)\n {\n if (found)\n {\n break;\n }\n\n cv::Mat thresh_img;\n\n // convert the input grayscale image to binary (black-n-white)\n if (flags & CV_CALIB_CB_ADAPTIVE_THRESH)\n {\n int blockSize = lround(prevSqrSize == 0 ?\n std::min(img.cols,img.rows)*(k%2 == 0 ? 0.2 : 0.1): prevSqrSize*2)|1;\n\n // convert to binary\n cv::adaptiveThreshold(img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, blockSize, (k/2)*5);\n }\n else\n {\n // empiric threshold level\n double mean = (cv::mean(img))[0];\n int thresh_level = lround(mean - 10);\n thresh_level = std::max(thresh_level, 10);\n\n cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY);\n }\n\n // MARTIN's Code\n // Use both a rectangular and a cross kernel. In this way, a more\n // homogeneous dilation is performed, which is crucial for small,\n // distorted checkers. Use the CROSS kernel first, since its action\n // on the image is more subtle\n cv::Mat kernel1 = cv::getStructuringElement(CV_SHAPE_CROSS, cv::Size(3,3), cv::Point(1,1));\n cv::Mat kernel2 = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(3,3), cv::Point(1,1));\n\n if (dilations >= 1)\n cv::dilate(thresh_img, thresh_img, kernel1);\n if (dilations >= 2)\n cv::dilate(thresh_img, thresh_img, kernel2);\n if (dilations >= 3)\n cv::dilate(thresh_img, thresh_img, kernel1);\n if (dilations >= 4)\n cv::dilate(thresh_img, thresh_img, kernel2);\n if (dilations >= 5)\n cv::dilate(thresh_img, thresh_img, kernel1);\n if (dilations >= 6)\n cv::dilate(thresh_img, thresh_img, kernel2);\n\n // In order to find rectangles that go to the edge, we draw a white\n // line around the image edge. Otherwise FindContours will miss those\n // clipped rectangle contours. The border color will be the image mean,\n // because otherwise we risk screwing up filters like cvSmooth()\n cv::rectangle(thresh_img, cv::Point(0,0),\n cv::Point(thresh_img.cols - 1, thresh_img.rows - 1),\n CV_RGB(255,255,255), 3, 8);\n\n // Generate quadrangles in the following function\n std::vector quads;\n\n generateQuads(quads, thresh_img, flags, dilations, true);\n if (quads.empty())\n {\n continue;\n }\n\n // The following function finds and assigns neighbor quads to every\n // quadrangle in the immediate vicinity fulfilling certain\n // prerequisites\n findQuadNeighbors(quads, dilations);\n\n // The connected quads will be organized in groups. The following loop\n // increases a \"group_idx\" identifier.\n // The function \"findConnectedQuads assigns all connected quads\n // a unique group ID.\n // If more quadrangles were assigned to a given group (i.e. connected)\n // than are expected by the input variable \"patternSize\", the\n // function \"cleanFoundConnectedQuads\" erases the surplus\n // quadrangles by minimizing the convex hull of the remaining pattern.\n\n for (int group_idx = 0; ; ++group_idx)\n {\n std::vector quadGroup;\n\n findConnectedQuads(quads, quadGroup, group_idx, dilations);\n\n if (quadGroup.empty())\n {\n break;\n }\n\n cleanFoundConnectedQuads(quadGroup, patternSize);\n\n // The following function labels all corners of every quad\n // with a row and column entry.\n // \"count\" specifies the number of found quads in \"quad_group\"\n // with group identifier \"group_idx\"\n // The last parameter is set to \"true\", because this is the\n // first function call and some initializations need to be\n // made.\n labelQuadGroup(quadGroup, patternSize, true);\n\n found = checkQuadGroup(quadGroup, outputCorners, patternSize);\n\n float sumDist = 0;\n int total = 0;\n\n for (int i = 0; i < (int)outputCorners.size(); ++i)\n {\n int ni = 0;\n float avgi = outputCorners.at(i)->meanDist(ni);\n sumDist += avgi * ni;\n total += ni;\n }\n prevSqrSize = lround(sumDist / std::max(total, 1));\n\n if (found && !checkBoardMonotony(outputCorners, patternSize))\n {\n found = false;\n }\n }\n }\n }\n\n if (!found)\n {\n return false;\n }\n else\n {\n corners.clear();\n corners.reserve(outputCorners.size());\n for (size_t i = 0; i < outputCorners.size(); ++i)\n {\n corners.push_back(outputCorners.at(i)->pt);\n }\n\n cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1,-1),\n cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));\n\n return true;\n }\n}\n\n//===========================================================================\n// ERASE OVERHEAD\n//===========================================================================\n// If we found too many connected quads, remove those which probably do not\n// belong.\nvoid\nChessboard::cleanFoundConnectedQuads(std::vector& quadGroup,\n cv::Size patternSize)\n{\n cv::Point2f center(0.0f, 0.0f);\n\n // Number of quads this pattern should contain\n int count = ((patternSize.width + 1)*(patternSize.height + 1) + 1)/2;\n\n // If we have more quadrangles than we should, try to eliminate duplicates\n // or ones which don't belong to the pattern rectangle. Else go to the end\n // of the function\n if ((int)quadGroup.size() <= count)\n {\n return;\n }\n\n // Create an array of quadrangle centers\n std::vector centers;\n centers.resize(quadGroup.size());\n\n for (size_t i = 0; i < quadGroup.size(); ++i)\n {\n cv::Point2f ci(0.0f, 0.0f);\n ChessboardQuadPtr& q = quadGroup[i];\n\n for (int j = 0; j < 4; ++j)\n {\n ci += q->corners[j]->pt;\n }\n\n ci *= 0.25f;\n\n // Centers(i), is the geometric center of quad(i)\n // Center, is the center of all found quads\n centers[i] = ci;\n\n center += ci;\n }\n\n center *= 1.0f / quadGroup.size();\n\n // If we have more quadrangles than we should, we try to eliminate bad\n // ones based on minimizing the bounding box. We iteratively remove the\n // point which reduces the size of the bounding box of the blobs the most\n // (since we want the rectangle to be as small as possible) remove the\n // quadrange that causes the biggest reduction in pattern size until we\n // have the correct number\n while ((int)quadGroup.size() > count)\n {\n double minBoxArea = DBL_MAX;\n int minBoxAreaIndex = -1;\n\n // For each point, calculate box area without that point\n for (size_t skip = 0; skip < quadGroup.size(); ++skip)\n {\n // get bounding rectangle\n cv::Point2f temp = centers[skip];\n centers[skip] = center;\n\n std::vector hull;\n cv::convexHull(centers, hull, true, true);\n centers[skip] = temp;\n\n double hull_area = fabs(cv::contourArea(hull));\n\n // remember smallest box area\n if (hull_area < minBoxArea)\n {\n minBoxArea = hull_area;\n minBoxAreaIndex = skip;\n }\n }\n\n ChessboardQuadPtr& q0 = quadGroup[minBoxAreaIndex];\n\n // remove any references to this quad as a neighbor\n for (size_t i = 0; i < quadGroup.size(); ++i)\n {\n ChessboardQuadPtr& q = quadGroup.at(i);\n for (int j = 0; j < 4; ++j)\n {\n if (q->neighbors[j] == q0)\n {\n q->neighbors[j].reset();\n q->count--;\n for (int k = 0; k < 4; ++k)\n {\n if (q0->neighbors[k] == q)\n {\n q0->neighbors[k].reset();\n q0->count--;\n break;\n }\n }\n break;\n }\n }\n }\n // remove the quad\n quadGroup.at(minBoxAreaIndex) = quadGroup.back();\n centers.at(minBoxAreaIndex) = centers.back();\n quadGroup.pop_back();\n centers.pop_back();\n }\n}\n\n//===========================================================================\n// FIND COONECTED QUADS\n//===========================================================================\nvoid\nChessboard::findConnectedQuads(std::vector& quads,\n std::vector& group,\n int group_idx, int dilation)\n{\n ChessboardQuadPtr q;\n\n // Scan the array for a first unlabeled quad\n for (size_t i = 0; i < quads.size(); ++i)\n {\n ChessboardQuadPtr& quad = quads.at(i);\n\n if (quad->count > 0 && quad->group_idx < 0)\n {\n q = quad;\n break;\n }\n }\n\n if (q.get() == 0)\n {\n return;\n }\n\n // Recursively find a group of connected quads starting from the seed quad\n\n std::vector stack;\n stack.push_back(q);\n\n group.push_back(q);\n q->group_idx = group_idx;\n\n while (!stack.empty())\n {\n q = stack.back();\n stack.pop_back();\n\n for (int i = 0; i < 4; ++i)\n {\n ChessboardQuadPtr& neighbor = q->neighbors[i];\n\n // If he neighbor exists and the neighbor has more than 0\n // neighbors and the neighbor has not been classified yet.\n if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0)\n {\n stack.push_back(neighbor);\n group.push_back(neighbor);\n neighbor->group_idx = group_idx;\n }\n }\n }\n}\n\nvoid\nChessboard::labelQuadGroup(std::vector& quadGroup,\n cv::Size patternSize, bool firstRun)\n{\n // If this is the first function call, a seed quad needs to be selected\n if (firstRun)\n {\n // Search for the (first) quad with the maximum number of neighbors\n // (usually 4). This will be our starting point.\n int mark = -1;\n int maxNeighborCount = 0;\n for (size_t i = 0; i < quadGroup.size(); ++i)\n {\n ChessboardQuadPtr& q = quadGroup.at(i);\n if (q->count > maxNeighborCount)\n {\n mark = i;\n maxNeighborCount = q->count;\n\n if (maxNeighborCount == 4)\n {\n break;\n }\n }\n }\n\n // Mark the starting quad's (per definition) upper left corner with\n //(0,0) and then proceed clockwise\n // The following labeling sequence enshures a \"right coordinate system\"\n ChessboardQuadPtr& q = quadGroup.at(mark);\n\n q->labeled = true;\n\n q->corners[0]->row = 0;\n q->corners[0]->column = 0;\n q->corners[1]->row = 0;\n q->corners[1]->column = 1;\n q->corners[2]->row = 1;\n q->corners[2]->column = 1;\n q->corners[3]->row = 1;\n q->corners[3]->column = 0;\n }\n\n\n // Mark all other corners with their respective row and column\n bool flagChanged = true;\n while (flagChanged)\n {\n // First reset the flag to \"false\"\n flagChanged = false;\n\n // Go through all quads top down is faster, since unlabeled quads will\n // be inserted at the end of the list\n for (int i = quadGroup.size() - 1; i >= 0; --i)\n {\n ChessboardQuadPtr& quad = quadGroup.at(i);\n\n // Check whether quad \"i\" has been labeled already\n if (!quad->labeled)\n {\n // Check its neighbors, whether some of them have been labeled\n // already\n for (int j = 0; j < 4; j++)\n {\n // Check whether the neighbor exists (i.e. is not the NULL\n // pointer)\n if (quad->neighbors[j])\n {\n ChessboardQuadPtr& quadNeighbor = quad->neighbors[j];\n\n // Only proceed, if neighbor \"j\" was labeled\n if (quadNeighbor->labeled)\n {\n // For every quad it could happen to pass here\n // multiple times. We therefore \"break\" later.\n // Check whitch of the neighbors corners is\n // connected to the current quad\n int connectedNeighborCornerId = -1;\n for (int k = 0; k < 4; ++k)\n {\n if (quadNeighbor->neighbors[k] == quad)\n {\n connectedNeighborCornerId = k;\n\n // there is only one, therefore\n break;\n }\n }\n\n\n // For the following calculations we need the row\n // and column of the connected neighbor corner and\n // all other corners of the connected quad \"j\",\n // clockwise (CW)\n ChessboardCornerPtr& conCorner = quadNeighbor->corners[connectedNeighborCornerId];\n ChessboardCornerPtr& conCornerCW1 = quadNeighbor->corners[(connectedNeighborCornerId+1)%4];\n ChessboardCornerPtr& conCornerCW2 = quadNeighbor->corners[(connectedNeighborCornerId+2)%4];\n ChessboardCornerPtr& conCornerCW3 = quadNeighbor->corners[(connectedNeighborCornerId+3)%4];\n\n quad->corners[j]->row = conCorner->row;\n quad->corners[j]->column = conCorner->column;\n quad->corners[(j+1)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW3->row;\n quad->corners[(j+1)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW3->column;\n quad->corners[(j+2)%4]->row = conCorner->row + conCorner->row - conCornerCW2->row;\n quad->corners[(j+2)%4]->column = conCorner->column + conCorner->column - conCornerCW2->column;\n quad->corners[(j+3)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW1->row;\n quad->corners[(j+3)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW1->column;\n\n // Mark this quad as labeled\n quad->labeled = true;\n\n // Changes have taken place, set the flag\n flagChanged = true;\n\n // once is enough!\n break;\n }\n }\n }\n }\n }\n }\n\n\n // All corners are marked with row and column\n // Record the minimal and maximal row and column indices\n // It is unlikely that more than 8bit checkers are used per dimension, if there are\n // an error would have been thrown at the beginning of \"cvFindChessboardCorners2\"\n int min_row = 127;\n int max_row = -127;\n int min_column = 127;\n int max_column = -127;\n\n for (int i = 0; i < (int)quadGroup.size(); ++i)\n {\n ChessboardQuadPtr& q = quadGroup.at(i);\n\n for (int j = 0; j < 4; ++j)\n {\n ChessboardCornerPtr& c = q->corners[j];\n\n if (c->row > max_row)\n {\n max_row = c->row;\n }\n if (c->row < min_row)\n {\n min_row = c->row;\n }\n if (c->column > max_column)\n {\n max_column = c->column;\n }\n if (c->column < min_column)\n {\n min_column = c->column;\n }\n }\n }\n\n // Label all internal corners with \"needsNeighbor\" = false\n // Label all external corners with \"needsNeighbor\" = true,\n // except if in a given dimension the pattern size is reached\n for (int i = min_row; i <= max_row; ++i)\n {\n for (int j = min_column; j <= max_column; ++j)\n {\n // A flag that indicates, whether a row/column combination is\n // executed multiple times\n bool flag = false;\n\n // Remember corner and quad\n int cornerID;\n int quadID;\n\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n if ((q->corners[l]->row == i) && (q->corners[l]->column == j))\n {\n if (flag)\n {\n // Passed at least twice through here\n q->corners[l]->needsNeighbor = false;\n quadGroup[quadID]->corners[cornerID]->needsNeighbor = false;\n }\n else\n {\n // Mark with needs a neighbor, but note the\n // address\n q->corners[l]->needsNeighbor = true;\n cornerID = l;\n quadID = k;\n }\n\n // set the flag to true\n flag = true;\n }\n }\n }\n }\n }\n\n // Complete Linking:\n // sometimes not all corners were properly linked in \"findQuadNeighbors\",\n // but after labeling each corner with its respective row and column, it is\n // possible to match them anyway.\n for (int i = min_row; i <= max_row; ++i)\n {\n for (int j = min_column; j <= max_column; ++j)\n {\n // the following \"number\" indicates the number of corners which\n // correspond to the given (i,j) value\n // 1 is a border corner or a conrer which still needs a neighbor\n // 2 is a fully connected internal corner\n // >2 something went wrong during labeling, report a warning\n int number = 1;\n\n // remember corner and quad\n int cornerID;\n int quadID;\n\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n if ((q->corners[l]->row == i) && (q->corners[l]->column == j))\n {\n\n if (number == 1)\n {\n // First corner, note its ID\n cornerID = l;\n quadID = k;\n }\n\n else if (number == 2)\n {\n // Second corner, check wheter this and the\n // first one have equal coordinates, else\n // interpolate\n cv::Point2f delta = q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt;\n\n if (delta.x != 0.0f || delta.y != 0.0f)\n {\n // Interpolate\n q->corners[l]->pt -= delta * 0.5f;\n\n quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f;\n }\n }\n else if (number > 2)\n {\n // Something went wrong during row/column labeling\n // Report a Warning\n // ->Implemented in the function \"mrWriteCorners\"\n }\n\n // increase the number by one\n ++number;\n }\n }\n }\n }\n }\n\n\n // Bordercorners don't need any neighbors, if the pattern size in the\n // respective direction is reached\n // The only time we can make shure that the target pattern size is reached in a given\n // dimension, is when the larger side has reached the target size in the maximal\n // direction, or if the larger side is larger than the smaller target size and the\n // smaller side equals the smaller target size\n int largerDimPattern = std::max(patternSize.height,patternSize.width);\n int smallerDimPattern = std::min(patternSize.height,patternSize.width);\n bool flagSmallerDim1 = false;\n bool flagSmallerDim2 = false;\n\n if ((largerDimPattern + 1) == max_column - min_column)\n {\n flagSmallerDim1 = true;\n // We found out that in the column direction the target pattern size is reached\n // Therefore border column corners do not need a neighbor anymore\n // Go through all corners\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->column == min_column || c->column == max_column)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n\n if ((largerDimPattern + 1) == max_row - min_row)\n {\n flagSmallerDim2 = true;\n // We found out that in the column direction the target pattern size is reached\n // Therefore border column corners do not need a neighbor anymore\n // Go through all corners\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->row == min_row || c->row == max_row)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n\n\n // Check the two flags:\n // - If one is true and the other false, then the pattern target\n // size was reached in in one direction -> We can check, whether the target\n // pattern size is also reached in the other direction\n // - If both are set to true, then we deal with a square board -> do nothing\n // - If both are set to false -> There is a possibility that the larger side is\n // larger than the smaller target size -> Check and if true, then check whether\n // the other side has the same size as the smaller target size\n if ((flagSmallerDim1 == false && flagSmallerDim2 == true))\n {\n // Larger target pattern size is in row direction, check wheter smaller target\n // pattern size is reached in column direction\n if ((smallerDimPattern + 1) == max_column - min_column)\n {\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->column == min_column || c->column == max_column)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n }\n\n if ((flagSmallerDim1 == true && flagSmallerDim2 == false))\n {\n // Larger target pattern size is in column direction, check wheter smaller target\n // pattern size is reached in row direction\n if ((smallerDimPattern + 1) == max_row - min_row)\n {\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->row == min_row || c->row == max_row)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n }\n\n if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_column - min_column)\n {\n // Larger target pattern size is in column direction, check wheter smaller target\n // pattern size is reached in row direction\n if ((smallerDimPattern + 1) == max_row - min_row)\n {\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->row == min_row || c->row == max_row)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n }\n\n if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_row - min_row)\n {\n // Larger target pattern size is in row direction, check wheter smaller target\n // pattern size is reached in column direction\n if ((smallerDimPattern + 1) == max_column - min_column)\n {\n for (int k = 0; k < (int)quadGroup.size(); ++k)\n {\n ChessboardQuadPtr& q = quadGroup.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = q->corners[l];\n\n if (c->column == min_column || c->column == max_column)\n {\n // Needs no neighbor anymore\n c->needsNeighbor = false;\n }\n }\n }\n }\n }\n}\n\n//===========================================================================\n// GIVE A GROUP IDX\n//===========================================================================\nvoid\nChessboard::findQuadNeighbors(std::vector& quads, int dilation)\n{\n // Thresh dilation is used to counter the effect of dilation on the\n // distance between 2 neighboring corners. Since the distance below is\n // computed as its square, we do here the same. Additionally, we take the\n // conservative assumption that dilation was performed using the 3x3 CROSS\n // kernel, which coresponds to the 4-neighborhood.\n const float thresh_dilation = (float)(2*dilation+3)*(2*dilation+3)*2; // the \"*2\" is for the x and y component\n // the \"3\" is for initial corner mismatch\n\n // Find quad neighbors\n for (size_t idx = 0; idx < quads.size(); ++idx)\n {\n ChessboardQuadPtr& curQuad = quads.at(idx);\n\n // Go through all quadrangles and label them in groups\n // For each corner of this quadrangle\n for (int i = 0; i < 4; ++i)\n {\n float minDist = FLT_MAX;\n int closestCornerIdx = -1;\n ChessboardQuadPtr closestQuad;\n\n if (curQuad->neighbors[i])\n {\n continue;\n }\n\n cv::Point2f pt = curQuad->corners[i]->pt;\n\n // Find the closest corner in all other quadrangles\n for (size_t k = 0; k < quads.size(); ++k)\n {\n if (k == idx)\n {\n continue;\n }\n\n ChessboardQuadPtr& quad = quads.at(k);\n\n for (int j = 0; j < 4; ++j)\n {\n // If it already has a neighbor\n if (quad->neighbors[j])\n {\n continue;\n }\n\n cv::Point2f dp = pt - quad->corners[j]->pt;\n float dist = dp.dot(dp);\n\n // The following \"if\" checks, whether \"dist\" is the\n // shortest so far and smaller than the smallest\n // edge length of the current and target quads\n if (dist < minDist &&\n dist <= (curQuad->edge_len + thresh_dilation) &&\n dist <= (quad->edge_len + thresh_dilation) )\n {\n // Check whether conditions are fulfilled\n if (matchCorners(curQuad, i, quad, j))\n {\n closestCornerIdx = j;\n closestQuad = quad;\n minDist = dist;\n }\n }\n }\n }\n\n // Have we found a matching corner point?\n if (closestCornerIdx >= 0 && minDist < FLT_MAX)\n {\n ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];\n\n // Make sure that the closest quad does not have the current\n // quad as neighbor already\n bool valid = true;\n for (int j = 0; j < 4; ++j)\n {\n if (closestQuad->neighbors[j] == curQuad)\n {\n valid = false;\n break;\n }\n }\n if (!valid)\n {\n continue;\n }\n\n // We've found one more corner - remember it\n closestCorner->pt = (pt + closestCorner->pt) * 0.5f;\n\n curQuad->count++;\n curQuad->neighbors[i] = closestQuad;\n curQuad->corners[i] = closestCorner;\n\n closestQuad->count++;\n closestQuad->neighbors[closestCornerIdx] = curQuad;\n closestQuad->corners[closestCornerIdx] = closestCorner;\n }\n }\n }\n}\n\n\n\n//===========================================================================\n// AUGMENT PATTERN WITH ADDITIONAL QUADS\n//===========================================================================\n// The first part of the function is basically a copy of\n// \"findQuadNeighbors\"\n// The comparisons between two points and two lines could be computed in their\n// own function\nint\nChessboard::augmentBestRun(std::vector& candidateQuads, int candidateDilation,\n std::vector& existingQuads, int existingDilation)\n{\n // thresh dilation is used to counter the effect of dilation on the\n // distance between 2 neighboring corners. Since the distance below is\n // computed as its square, we do here the same. Additionally, we take the\n // conservative assumption that dilation was performed using the 3x3 CROSS\n // kernel, which coresponds to the 4-neighborhood.\n const float thresh_dilation = (2*candidateDilation+3)*(2*existingDilation+3)*2; // the \"*2\" is for the x and y component\n\n // Search all old quads which have a neighbor that needs to be linked\n for (size_t idx = 0; idx < existingQuads.size(); ++idx)\n {\n ChessboardQuadPtr& curQuad = existingQuads.at(idx);\n\n // For each corner of this quadrangle\n for (int i = 0; i < 4; ++i)\n {\n float minDist = FLT_MAX;\n int closestCornerIdx = -1;\n ChessboardQuadPtr closestQuad;\n\n // If curQuad corner[i] is already linked, continue\n if (!curQuad->corners[i]->needsNeighbor)\n {\n continue;\n }\n\n cv::Point2f pt = curQuad->corners[i]->pt;\n\n // Look for a match in all candidateQuads' corners\n for (size_t k = 0; k < candidateQuads.size(); ++k)\n {\n ChessboardQuadPtr& candidateQuad = candidateQuads.at(k);\n\n // Only look at unlabeled new quads\n if (candidateQuad->labeled)\n {\n continue;\n }\n\n for (int j = 0; j < 4; ++j)\n {\n // Only proceed if they are less than dist away from each\n // other\n cv::Point2f dp = pt - candidateQuad->corners[j]->pt;\n float dist = dp.dot(dp);\n\n if ((dist < minDist) &&\n dist <= (curQuad->edge_len + thresh_dilation) &&\n dist <= (candidateQuad->edge_len + thresh_dilation))\n {\n if (matchCorners(curQuad, i, candidateQuad, j))\n {\n closestCornerIdx = j;\n closestQuad = candidateQuad;\n minDist = dist;\n }\n }\n }\n }\n\n // Have we found a matching corner point?\n if (closestCornerIdx >= 0 && minDist < FLT_MAX)\n {\n ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];\n closestCorner->pt = (pt + closestCorner->pt) * 0.5f;\n\n // We've found one more corner - remember it\n // ATTENTION: write the corner x and y coordinates separately,\n // else the crucial row/column entries will be overwritten !!!\n curQuad->corners[i]->pt = closestCorner->pt;\n curQuad->neighbors[i] = closestQuad;\n closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt;\n\n // Label closest quad as labeled. In this way we exclude it\n // being considered again during the next loop iteration\n closestQuad->labeled = true;\n\n // We have a new member of the final pattern, copy it over\n ChessboardQuadPtr newQuad(new ChessboardQuad);\n newQuad->count = 1;\n newQuad->edge_len = closestQuad->edge_len;\n newQuad->group_idx = curQuad->group_idx; //the same as the current quad\n newQuad->labeled = false; //do it right afterwards\n\n curQuad->neighbors[i] = newQuad;\n\n // We only know one neighbor for sure\n newQuad->neighbors[closestCornerIdx] = curQuad;\n\n for (int j = 0; j < 4; j++)\n {\n newQuad->corners[j].reset(new ChessboardCorner);\n newQuad->corners[j]->pt = closestQuad->corners[j]->pt;\n }\n\n existingQuads.push_back(newQuad);\n\n // Start the function again\n return -1;\n }\n }\n }\n\n // Finished, don't start the function again\n return 1;\n}\n\n\n\n//===========================================================================\n// GENERATE QUADRANGLES\n//===========================================================================\nvoid\nChessboard::generateQuads(std::vector& quads,\n cv::Mat& image, int flags,\n int dilation, bool firstRun)\n{\n // Empirical lower bound for the size of allowable quadrangles.\n // MARTIN, modified: Added \"*0.1\" in order to find smaller quads.\n int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1);\n\n std::vector< std::vector > contours;\n std::vector hierarchy;\n\n // Initialize contour retrieving routine\n cv::findContours(image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);\n\n std::vector< std::vector > quadContours;\n\n for (size_t i = 0; i < contours.size(); ++i)\n {\n // Reject contours with a too small perimeter and contours which are\n // completely surrounded by another contour\n // MARTIN: If this function is called during PART 1, then the parameter \"first run\"\n // is set to \"true\". This guarantees, that only \"nice\" squares are detected.\n // During PART 2, we allow the polygonal approximation function below to\n // approximate more freely, which can result in recognized \"squares\" that are in\n // reality multiple blurred and sticked together squares.\n std::vector& contour = contours.at(i);\n\n if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize)\n {\n continue;\n }\n\n int min_approx_level = 1, max_approx_level;\n if (firstRun)\n {\n max_approx_level = 3;\n }\n else\n {\n max_approx_level = MAX_CONTOUR_APPROX;\n }\n\n std::vector approxContour;\n for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++)\n {\n cv::approxPolyDP(contour, approxContour, approx_level, true);\n\n if (approxContour.size() == 4)\n {\n break;\n }\n }\n\n // Reject non-quadrangles\n if (approxContour.size() == 4 && cv::isContourConvex(approxContour))\n {\n double p = cv::arcLength(approxContour, true);\n double area = cv::contourArea(approxContour);\n\n cv::Point pt[4];\n for (int i = 0; i < 4; i++)\n {\n pt[i] = approxContour[i];\n }\n\n cv::Point dp = pt[0] - pt[2];\n double d1 = sqrt(dp.dot(dp));\n\n dp = pt[1] - pt[3];\n double d2 = sqrt(dp.dot(dp));\n\n // PHILIPG: Only accept those quadrangles which are more\n // square than rectangular and which are big enough\n dp = pt[0] - pt[1];\n double d3 = sqrt(dp.dot(dp));\n dp = pt[1] - pt[2];\n double d4 = sqrt(dp.dot(dp));\n\n if (!(flags & CV_CALIB_CB_FILTER_QUADS) ||\n (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > minSize &&\n d1 >= 0.15 * p && d2 >= 0.15 * p))\n {\n quadContours.push_back(approxContour);\n }\n }\n }\n\n // Allocate quad & corner buffers\n quads.resize(quadContours.size());\n\n // Create array of quads structures\n for (size_t idx = 0; idx < quadContours.size(); ++idx)\n {\n ChessboardQuadPtr& q = quads.at(idx);\n std::vector& contour = quadContours.at(idx);\n\n // Reset group ID\n q.reset(new ChessboardQuad);\n assert(contour.size() == 4);\n\n for (int i = 0; i < 4; ++i)\n {\n cv::Point2f pt = contour.at(i);\n ChessboardCornerPtr corner(new ChessboardCorner);\n corner->pt = pt;\n q->corners[i] = corner;\n }\n\n for (int i = 0; i < 4; ++i)\n {\n cv::Point2f dp = q->corners[i]->pt - q->corners[(i+1)&3]->pt;\n float d = dp.dot(dp);\n if (q->edge_len > d)\n {\n q->edge_len = d;\n }\n }\n }\n}\n\nbool\nChessboard::checkQuadGroup(std::vector& quads,\n std::vector& corners,\n cv::Size patternSize)\n{\n // Initialize\n bool flagRow = false;\n bool flagColumn = false;\n int height = -1;\n int width = -1;\n\n // Compute the minimum and maximum row / column ID\n // (it is unlikely that more than 8bit checkers are used per dimension)\n int min_row = 127;\n int max_row = -127;\n int min_col = 127;\n int max_col = -127;\n\n for (size_t i = 0; i < quads.size(); ++i)\n {\n ChessboardQuadPtr& q = quads.at(i);\n\n for (int j = 0; j < 4; ++j)\n {\n ChessboardCornerPtr& c = q->corners[j];\n\n if (c->row > max_row)\n {\n max_row = c->row;\n }\n if (c->row < min_row)\n {\n min_row = c->row;\n }\n if (c->column > max_col)\n {\n max_col = c->column;\n }\n if (c->column < min_col)\n {\n min_col = c->column;\n }\n }\n }\n\n // If in a given direction the target pattern size is reached, we know exactly how\n // the checkerboard is oriented.\n // Else we need to prepare enough \"dummy\" corners for the worst case.\n for (size_t i = 0; i < quads.size(); ++i)\n {\n ChessboardQuadPtr& q = quads.at(i);\n\n for (int j = 0; j < 4; ++j)\n {\n ChessboardCornerPtr& c = q->corners[j];\n\n if (c->column == max_col && c->row != min_row && c->row != max_row && !c->needsNeighbor)\n {\n flagColumn = true;\n }\n if (c->row == max_row && c->column != min_col && c->column != max_col && !c->needsNeighbor)\n {\n flagRow = true;\n }\n }\n }\n\n if (flagColumn)\n {\n if (max_col - min_col == patternSize.width + 1)\n {\n width = patternSize.width;\n height = patternSize.height;\n }\n else\n {\n width = patternSize.height;\n height = patternSize.width;\n }\n }\n else if (flagRow)\n {\n if (max_row - min_row == patternSize.width + 1)\n {\n height = patternSize.width;\n width = patternSize.height;\n }\n else\n {\n height = patternSize.height;\n width = patternSize.width;\n }\n }\n else\n {\n // If target pattern size is not reached in at least one of the two\n // directions, then we do not know where the remaining corners are\n // located. Account for this.\n width = std::max(patternSize.width, patternSize.height);\n height = std::max(patternSize.width, patternSize.height);\n }\n\n ++min_row;\n ++min_col;\n max_row = min_row + height - 1;\n max_col = min_col + width - 1;\n\n corners.clear();\n\n int linkedBorderCorners = 0;\n\n // Write the corners in increasing order to the output file\n for (int i = min_row; i <= max_row; ++i)\n {\n for (int j = min_col; j <= max_col; ++j)\n {\n // Reset the iterator\n int iter = 1;\n\n for (int k = 0; k < (int)quads.size(); ++k)\n {\n ChessboardQuadPtr& quad = quads.at(k);\n\n for (int l = 0; l < 4; ++l)\n {\n ChessboardCornerPtr& c = quad->corners[l];\n\n if (c->row == i && c->column == j)\n {\n bool boardEdge = false;\n if (i == min_row || i == max_row ||\n j == min_col || j == max_col)\n {\n boardEdge = true;\n }\n\n if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge))\n {\n // The respective row and column have been found\n corners.push_back(quad->corners[l]);\n }\n\n if (iter == 2 && boardEdge)\n {\n ++linkedBorderCorners;\n }\n\n // If the iterator is larger than two, this means that more than\n // two corners have the same row / column entries. Then some\n // linking errors must have occured and we should not use the found\n // pattern\n if (iter > 2)\n {\n return false;\n }\n\n iter++;\n }\n }\n }\n }\n }\n\n if ((int)corners.size() != patternSize.width * patternSize.height ||\n linkedBorderCorners < (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f)\n {\n return false;\n }\n\n // check that no corners lie at image boundary\n float border = 5.0f;\n for (int i = 0; i < (int)corners.size(); ++i)\n {\n ChessboardCornerPtr& c = corners.at(i);\n\n if (c->pt.x < border || c->pt.x > mImage.cols - border ||\n c->pt.y < border || c->pt.y > mImage.rows - border)\n {\n return false;\n }\n }\n\n // check if we need to transpose the board\n if (width != patternSize.width)\n {\n std::swap(width, height);\n\n std::vector outputCorners;\n outputCorners.resize(corners.size());\n\n for (int i = 0; i < height; ++i)\n {\n for (int j = 0; j < width; ++j)\n {\n outputCorners.at(i * width + j) = corners.at(j * height + i);\n }\n }\n\n corners = outputCorners;\n }\n\n // check if we need to revert the order in each row\n cv::Point2f p0 = corners.at(0)->pt;\n cv::Point2f p1 = corners.at(width-1)->pt;\n cv::Point2f p2 = corners.at(width)->pt;\n\n if ((p1 - p0).cross(p2 - p0) < 0.0f)\n {\n for (int i = 0; i < height; ++i)\n {\n for (int j = 0; j < width / 2; ++j)\n {\n std::swap(corners.at(i * width + j), corners.at(i * width + width - j - 1));\n }\n }\n }\n\n p0 = corners.at(0)->pt;\n p2 = corners.at(width)->pt;\n\n // check if we need to rotate the board\n if (p2.y < p0.y)\n {\n std::vector outputCorners;\n outputCorners.resize(corners.size());\n\n for (int i = 0; i < height; ++i)\n {\n for (int j = 0; j < width; ++j)\n {\n outputCorners.at(i * width + j) = corners.at((height - i - 1) * width + width - j - 1);\n }\n }\n\n corners = outputCorners;\n }\n\n return true;\n}\n\nvoid\nChessboard::getQuadrangleHypotheses(const std::vector< std::vector >& contours,\n std::vector< std::pair >& quads,\n int classId) const\n{\n const float minAspectRatio = 0.2f;\n const float maxAspectRatio = 5.0f;\n const float minBoxSize = 10.0f;\n\n for (size_t i = 0; i < contours.size(); ++i)\n {\n cv::RotatedRect box = cv::minAreaRect(contours.at(i));\n float boxSize = std::max(box.size.width, box.size.height);\n if (boxSize < minBoxSize)\n {\n continue;\n }\n\n float aspectRatio = box.size.width / std::max(box.size.height, 1.0f);\n\n if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio)\n {\n continue;\n }\n\n quads.push_back(std::pair(boxSize, classId));\n }\n}\n\nbool less_pred(const std::pair& p1, const std::pair& p2)\n{\n return p1.first < p2.first;\n}\n\nvoid countClasses(const std::vector >& pairs, size_t idx1, size_t idx2, std::vector& counts)\n{\n counts.assign(2, 0);\n for (size_t i = idx1; i != idx2; ++i)\n {\n counts[pairs[i].second]++;\n }\n}\n\nbool\nChessboard::checkChessboard(const cv::Mat& image, cv::Size patternSize) const\n{\n const int erosionCount = 1;\n const float blackLevel = 20.f;\n const float whiteLevel = 130.f;\n const float blackWhiteGap = 70.f;\n\n cv::Mat white = image.clone();\n\n cv::Mat black = image.clone();\n\n cv::erode(white, white, cv::Mat(), cv::Point(-1,-1), erosionCount);\n cv::dilate(black, black, cv::Mat(), cv::Point(-1,-1), erosionCount);\n\n cv::Mat thresh(image.rows, image.cols, CV_8UC1);\n\n bool result = false;\n for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f)\n {\n cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, CV_THRESH_BINARY);\n\n std::vector< std::vector > contours;\n std::vector hierarchy;\n\n // Initialize contour retrieving routine\n cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);\n\n std::vector > quads;\n getQuadrangleHypotheses(contours, quads, 1);\n\n cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV);\n\n cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);\n getQuadrangleHypotheses(contours, quads, 0);\n\n const size_t min_quads_count = patternSize.width * patternSize.height / 2;\n std::sort(quads.begin(), quads.end(), less_pred);\n\n // now check if there are many hypotheses with similar sizes\n // do this by floodfill-style algorithm\n const float sizeRelDev = 0.4f;\n\n for (size_t i = 0; i < quads.size(); ++i)\n {\n size_t j = i + 1;\n for (; j < quads.size(); ++j)\n {\n if (quads[j].first / quads[i].first > 1.0f + sizeRelDev)\n {\n break;\n }\n }\n\n if (j + 1 > min_quads_count + i)\n {\n // check the number of black and white squares\n std::vector counts;\n countClasses(quads, i, j, counts);\n const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f));\n const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * floorf(patternSize.height / 2.0f));\n if (counts[0] < blackCount * 0.75f ||\n counts[1] < whiteCount * 0.75f)\n {\n continue;\n }\n result = true;\n break;\n }\n }\n }\n\n return result;\n}\n\nbool\nChessboard::checkBoardMonotony(std::vector& corners,\n cv::Size patternSize)\n{\n const float threshFactor = 0.2f;\n\n Spline splineXY, splineYX;\n splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC);\n splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC);\n splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC);\n splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC);\n\n // check if each row of corners approximates a cubic spline\n for (int i = 0; i < patternSize.height; ++i)\n {\n splineXY.clear();\n splineYX.clear();\n\n cv::Point2f p[3];\n p[0] = corners.at(i * patternSize.width)->pt;\n p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt;\n p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt;\n\n for (int j = 0; j < 3; ++j)\n {\n splineXY.addPoint(p[j].x, p[j].y);\n splineYX.addPoint(p[j].y, p[j].x);\n }\n\n for (int j = 1; j < patternSize.width - 1; ++j)\n {\n cv::Point2f& p_j = corners.at(i * patternSize.width + j)->pt;\n\n float thresh = std::numeric_limits::max();\n\n // up-neighbor\n if (i > 0)\n {\n cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_j));\n }\n // down-neighbor\n if (i < patternSize.height - 1)\n {\n cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_j));\n }\n // left-neighbor\n {\n cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_j));\n }\n // right-neighbor\n {\n cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_j));\n }\n\n thresh *= threshFactor;\n\n if (fminf(fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > thresh)\n {\n return false;\n }\n }\n }\n\n // check if each column of corners approximates a cubic spline\n for (int j = 0; j < patternSize.width; ++j)\n {\n splineXY.clear();\n splineYX.clear();\n\n cv::Point2f p[3];\n p[0] = corners.at(j)->pt;\n p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt;\n p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt;\n\n for (int i = 0; i < 3; ++i)\n {\n splineXY.addPoint(p[i].x, p[i].y);\n splineYX.addPoint(p[i].y, p[i].x);\n }\n\n for (int i = 1; i < patternSize.height - 1; ++i)\n {\n cv::Point2f& p_i = corners.at(i * patternSize.width + j)->pt;\n\n float thresh = std::numeric_limits::max();\n\n // up-neighbor\n {\n cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_i));\n }\n // down-neighbor\n {\n cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_i));\n }\n // left-neighbor\n if (j > 0)\n {\n cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_i));\n }\n // right-neighbor\n if (j < patternSize.width - 1)\n {\n cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt;\n thresh = fminf(thresh, cv::norm(neighbor - p_i));\n }\n\n thresh *= threshFactor;\n\n if (fminf(fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > thresh)\n {\n return false;\n }\n }\n }\n\n return true;\n}\n\nbool\nChessboard::matchCorners(ChessboardQuadPtr& quad1, int corner1,\n ChessboardQuadPtr& quad2, int corner2) const\n{\n // First Check everything from the viewpoint of the\n // current quad compute midpoints of \"parallel\" quad\n // sides 1\n float x1 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+1)%4]->pt.x)/2;\n float y1 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+1)%4]->pt.y)/2;\n float x2 = (quad1->corners[(corner1+2)%4]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2;\n float y2 = (quad1->corners[(corner1+2)%4]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2;\n // compute midpoints of \"parallel\" quad sides 2\n float x3 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2;\n float y3 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2;\n float x4 = (quad1->corners[(corner1+1)%4]->pt.x + quad1->corners[(corner1+2)%4]->pt.x)/2;\n float y4 = (quad1->corners[(corner1+1)%4]->pt.y + quad1->corners[(corner1+2)%4]->pt.y)/2;\n\n // MARTIN: Heuristic\n // For corner2 of quad2 to be considered,\n // it needs to be on the same side of the two lines as\n // corner1. This is given, if the cross product has\n // the same sign for both computations below:\n float a1 = x1 - x2;\n float b1 = y1 - y2;\n // the current corner\n float c11 = quad1->corners[corner1]->pt.x - x2;\n float d11 = quad1->corners[corner1]->pt.y - y2;\n // the candidate corner\n float c12 = quad2->corners[corner2]->pt.x - x2;\n float d12 = quad2->corners[corner2]->pt.y - y2;\n float sign11 = a1*d11 - c11*b1;\n float sign12 = a1*d12 - c12*b1;\n\n float a2 = x3 - x4;\n float b2 = y3 - y4;\n // the current corner\n float c21 = quad1->corners[corner1]->pt.x - x4;\n float d21 = quad1->corners[corner1]->pt.y - y4;\n // the candidate corner\n float c22 = quad2->corners[corner2]->pt.x - x4;\n float d22 = quad2->corners[corner2]->pt.y - y4;\n float sign21 = a2*d21 - c21*b2;\n float sign22 = a2*d22 - c22*b2;\n\n // Also make shure that two border quads of the same row or\n // column don't link. Check from the current corner's view,\n // whether the corner diagonal from the candidate corner\n // is also on the same side of the two lines as the current\n // corner and the candidate corner.\n float c13 = quad2->corners[(corner2+2)%4]->pt.x - x2;\n float d13 = quad2->corners[(corner2+2)%4]->pt.y - y2;\n float c23 = quad2->corners[(corner2+2)%4]->pt.x - x4;\n float d23 = quad2->corners[(corner2+2)%4]->pt.y - y4;\n float sign13 = a1*d13 - c13*b1;\n float sign23 = a2*d23 - c23*b2;\n\n\n // Second: Then check everything from the viewpoint of\n // the candidate quad. Compute midpoints of \"parallel\"\n // quad sides 1\n float u1 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+1)%4]->pt.x)/2;\n float v1 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+1)%4]->pt.y)/2;\n float u2 = (quad2->corners[(corner2+2)%4]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2;\n float v2 = (quad2->corners[(corner2+2)%4]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2;\n // compute midpoints of \"parallel\" quad sides 2\n float u3 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2;\n float v3 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2;\n float u4 = (quad2->corners[(corner2+1)%4]->pt.x + quad2->corners[(corner2+2)%4]->pt.x)/2;\n float v4 = (quad2->corners[(corner2+1)%4]->pt.y + quad2->corners[(corner2+2)%4]->pt.y)/2;\n\n // MARTIN: Heuristic\n // For corner2 of quad2 to be considered,\n // it needs to be on the same side of the two lines as\n // corner1. This is given, if the cross product has\n // the same sign for both computations below:\n float a3 = u1 - u2;\n float b3 = v1 - v2;\n // the current corner\n float c31 = quad1->corners[corner1]->pt.x - u2;\n float d31 = quad1->corners[corner1]->pt.y - v2;\n // the candidate corner\n float c32 = quad2->corners[corner2]->pt.x - u2;\n float d32 = quad2->corners[corner2]->pt.y - v2;\n float sign31 = a3*d31-c31*b3;\n float sign32 = a3*d32-c32*b3;\n\n float a4 = u3 - u4;\n float b4 = v3 - v4;\n // the current corner\n float c41 = quad1->corners[corner1]->pt.x - u4;\n float d41 = quad1->corners[corner1]->pt.y - v4;\n // the candidate corner\n float c42 = quad2->corners[corner2]->pt.x - u4;\n float d42 = quad2->corners[corner2]->pt.y - v4;\n float sign41 = a4*d41-c41*b4;\n float sign42 = a4*d42-c42*b4;\n\n // Also make sure that two border quads of the same row or\n // column don't link. Check from the candidate corner's view,\n // whether the corner diagonal from the current corner\n // is also on the same side of the two lines as the current\n // corner and the candidate corner.\n float c33 = quad1->corners[(corner1+2)%4]->pt.x - u2;\n float d33 = quad1->corners[(corner1+2)%4]->pt.y - v2;\n float c43 = quad1->corners[(corner1+2)%4]->pt.x - u4;\n float d43 = quad1->corners[(corner1+2)%4]->pt.y - v4;\n float sign33 = a3*d33-c33*b3;\n float sign43 = a4*d43-c43*b4;\n\n\n // This time we also need to make shure, that no quad\n // is linked to a quad of another dilation run which\n // may lie INSIDE it!!!\n // Third: Therefore check everything from the viewpoint\n // of the current quad compute midpoints of \"parallel\"\n // quad sides 1\n float x5 = quad1->corners[corner1]->pt.x;\n float y5 = quad1->corners[corner1]->pt.y;\n float x6 = quad1->corners[(corner1+1)%4]->pt.x;\n float y6 = quad1->corners[(corner1+1)%4]->pt.y;\n // compute midpoints of \"parallel\" quad sides 2\n float x7 = x5;\n float y7 = y5;\n float x8 = quad1->corners[(corner1+3)%4]->pt.x;\n float y8 = quad1->corners[(corner1+3)%4]->pt.y;\n\n // MARTIN: Heuristic\n // For corner2 of quad2 to be considered,\n // it needs to be on the other side of the two lines than\n // corner1. This is given, if the cross product has\n // a different sign for both computations below:\n float a5 = x6 - x5;\n float b5 = y6 - y5;\n // the current corner\n float c51 = quad1->corners[(corner1+2)%4]->pt.x - x5;\n float d51 = quad1->corners[(corner1+2)%4]->pt.y - y5;\n // the candidate corner\n float c52 = quad2->corners[corner2]->pt.x - x5;\n float d52 = quad2->corners[corner2]->pt.y - y5;\n float sign51 = a5*d51 - c51*b5;\n float sign52 = a5*d52 - c52*b5;\n\n float a6 = x8 - x7;\n float b6 = y8 - y7;\n // the current corner\n float c61 = quad1->corners[(corner1+2)%4]->pt.x - x7;\n float d61 = quad1->corners[(corner1+2)%4]->pt.y - y7;\n // the candidate corner\n float c62 = quad2->corners[corner2]->pt.x - x7;\n float d62 = quad2->corners[corner2]->pt.y - y7;\n float sign61 = a6*d61 - c61*b6;\n float sign62 = a6*d62 - c62*b6;\n\n\n // Fourth: Then check everything from the viewpoint of\n // the candidate quad compute midpoints of \"parallel\"\n // quad sides 1\n float u5 = quad2->corners[corner2]->pt.x;\n float v5 = quad2->corners[corner2]->pt.y;\n float u6 = quad2->corners[(corner2+1)%4]->pt.x;\n float v6 = quad2->corners[(corner2+1)%4]->pt.y;\n // compute midpoints of \"parallel\" quad sides 2\n float u7 = u5;\n float v7 = v5;\n float u8 = quad2->corners[(corner2+3)%4]->pt.x;\n float v8 = quad2->corners[(corner2+3)%4]->pt.y;\n\n // MARTIN: Heuristic\n // For corner2 of quad2 to be considered,\n // it needs to be on the other side of the two lines than\n // corner1. This is given, if the cross product has\n // a different sign for both computations below:\n float a7 = u6 - u5;\n float b7 = v6 - v5;\n // the current corner\n float c71 = quad1->corners[corner1]->pt.x - u5;\n float d71 = quad1->corners[corner1]->pt.y - v5;\n // the candidate corner\n float c72 = quad2->corners[(corner2+2)%4]->pt.x - u5;\n float d72 = quad2->corners[(corner2+2)%4]->pt.y - v5;\n float sign71 = a7*d71-c71*b7;\n float sign72 = a7*d72-c72*b7;\n\n float a8 = u8 - u7;\n float b8 = v8 - v7;\n // the current corner\n float c81 = quad1->corners[corner1]->pt.x - u7;\n float d81 = quad1->corners[corner1]->pt.y - v7;\n // the candidate corner\n float c82 = quad2->corners[(corner2+2)%4]->pt.x - u7;\n float d82 = quad2->corners[(corner2+2)%4]->pt.y - v7;\n float sign81 = a8*d81-c81*b8;\n float sign82 = a8*d82-c82*b8;\n\n // Check whether conditions are fulfilled\n if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0)) &&\n ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0)) &&\n ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0)) &&\n ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0)) &&\n ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0)) &&\n ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0)) &&\n ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0)) &&\n ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0)) &&\n ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0)) &&\n ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0)) &&\n ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0)) &&\n ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0)))\n {\n return true;\n }\n else\n {\n return false;\n }\n}\n\n}\n" }, { "path": "src/utils/camodocal/src/gpl/EigenQuaternionParameterization.cc", "content": "#include \"camodocal/gpl/EigenQuaternionParameterization.h\"\n\n#include \n\nnamespace camodocal\n{\n\nbool\nEigenQuaternionParameterization::Plus(const double* x,\n const double* delta,\n double* x_plus_delta) const\n{\n const double norm_delta =\n sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);\n if (norm_delta > 0.0)\n {\n const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);\n double q_delta[4];\n q_delta[0] = sin_delta_by_delta * delta[0];\n q_delta[1] = sin_delta_by_delta * delta[1];\n q_delta[2] = sin_delta_by_delta * delta[2];\n q_delta[3] = cos(norm_delta);\n EigenQuaternionProduct(q_delta, x, x_plus_delta);\n }\n else\n {\n for (int i = 0; i < 4; ++i)\n {\n x_plus_delta[i] = x[i];\n }\n }\n return true;\n}\n\nbool\nEigenQuaternionParameterization::ComputeJacobian(const double* x,\n double* jacobian) const\n{\n jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT\n jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT\n jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT\n jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT\n return true;\n}\n\n}\n" }, { "path": "src/utils/camodocal/src/gpl/gpl.cc", "content": "#include \"camodocal/gpl/gpl.h\"\r\n\r\n#include \r\n#ifdef _WIN32\r\n#include \r\n#else\r\n#include \r\n#endif\r\n\r\n\r\n// source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x\r\n#ifdef __APPLE__\r\n#include \r\n#define ORWL_NANO (+1.0E-9)\r\n#define ORWL_GIGA UINT64_C(1000000000)\r\n\r\nstatic double orwl_timebase = 0.0;\r\nstatic uint64_t orwl_timestart = 0;\r\n\r\nstruct timespec orwl_gettime(void) {\r\n // be more careful in a multithreaded environement\r\n if (!orwl_timestart) {\r\n mach_timebase_info_data_t tb = { 0 };\r\n mach_timebase_info(&tb);\r\n orwl_timebase = tb.numer;\r\n orwl_timebase /= tb.denom;\r\n orwl_timestart = mach_absolute_time();\r\n }\r\n struct timespec t;\r\n double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase;\r\n t.tv_sec = diff * ORWL_NANO;\r\n t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA);\r\n return t;\r\n}\r\n#endif // __APPLE__\r\n\r\n\r\nconst double WGS84_A = 6378137.0;\r\nconst double WGS84_ECCSQ = 0.00669437999013;\r\n\r\n// Windows lacks fminf\r\n#ifndef fminf\r\n#define fminf(x, y) (((x) < (y)) ? (x) : (y))\r\n#endif\r\n\r\nnamespace camodocal\r\n{\r\n\r\ndouble hypot3(double x, double y, double z)\r\n{\r\n\treturn sqrt(square(x) + square(y) + square(z));\r\n}\r\n\r\nfloat hypot3f(float x, float y, float z)\r\n{\r\n\treturn sqrtf(square(x) + square(y) + square(z));\r\n}\r\n\r\ndouble d2r(double deg)\r\n{\r\n\treturn deg / 180.0 * M_PI;\r\n}\r\n\r\nfloat d2r(float deg)\r\n{\r\n\treturn deg / 180.0f * M_PI;\r\n}\r\n\r\ndouble r2d(double rad)\r\n{\r\n\treturn rad / M_PI * 180.0;\r\n}\r\n\r\nfloat r2d(float rad)\r\n{\r\n\treturn rad / M_PI * 180.0f;\r\n}\r\n\r\ndouble sinc(double theta)\r\n{\r\n return sin(theta) / theta;\r\n}\r\n\r\n#ifdef _WIN32\r\n#include \r\n#include \r\n#include \r\nLARGE_INTEGER\r\ngetFILETIMEoffset()\r\n{\r\n SYSTEMTIME s;\r\n FILETIME f;\r\n LARGE_INTEGER t;\r\n\r\n s.wYear = 1970;\r\n s.wMonth = 1;\r\n s.wDay = 1;\r\n s.wHour = 0;\r\n s.wMinute = 0;\r\n s.wSecond = 0;\r\n s.wMilliseconds = 0;\r\n SystemTimeToFileTime(&s, &f);\r\n t.QuadPart = f.dwHighDateTime;\r\n t.QuadPart <<= 32;\r\n t.QuadPart |= f.dwLowDateTime;\r\n return (t);\r\n}\r\n\r\nint\r\nclock_gettime(int X, struct timespec *tp)\r\n{\r\n LARGE_INTEGER t;\r\n FILETIME f;\r\n double microseconds;\r\n static LARGE_INTEGER offset;\r\n static double frequencyToMicroseconds;\r\n static int initialized = 0;\r\n static BOOL usePerformanceCounter = 0;\r\n\r\n if (!initialized) {\r\n LARGE_INTEGER performanceFrequency;\r\n initialized = 1;\r\n usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency);\r\n if (usePerformanceCounter) {\r\n QueryPerformanceCounter(&offset);\r\n frequencyToMicroseconds = (double)performanceFrequency.QuadPart / 1000000.;\r\n } else {\r\n offset = getFILETIMEoffset();\r\n frequencyToMicroseconds = 10.;\r\n }\r\n }\r\n if (usePerformanceCounter) QueryPerformanceCounter(&t);\r\n else {\r\n GetSystemTimeAsFileTime(&f);\r\n t.QuadPart = f.dwHighDateTime;\r\n t.QuadPart <<= 32;\r\n t.QuadPart |= f.dwLowDateTime;\r\n }\r\n\r\n t.QuadPart -= offset.QuadPart;\r\n microseconds = (double)t.QuadPart / frequencyToMicroseconds;\r\n t.QuadPart = microseconds;\r\n tp->tv_sec = t.QuadPart / 1000000;\r\n tp->tv_nsec = (t.QuadPart % 1000000) * 1000;\r\n return (0);\r\n}\r\n#endif\r\n\r\nunsigned long long timeInMicroseconds(void)\r\n{\r\n\t struct timespec tp;\r\n#ifdef __APPLE__\r\n tp = orwl_gettime();\r\n#else\r\n\t clock_gettime(CLOCK_REALTIME, &tp);\r\n#endif\r\n\r\n\t return tp.tv_sec * 1000000 + tp.tv_nsec / 1000;\r\n}\r\n\r\ndouble timeInSeconds(void)\r\n{\r\n struct timespec tp;\r\n#ifdef __APPLE__\r\n tp = orwl_gettime();\r\n#else\r\n\t clock_gettime(CLOCK_REALTIME, &tp);\r\n#endif\r\n\r\n\t return static_cast(tp.tv_sec) +\r\n\t\t\t static_cast(tp.tv_nsec) / 1000000000.0;\r\n}\r\n\r\nfloat colormapAutumn[128][3] =\r\n{\r\n\t\t{1.0f,0.f,0.f},\r\n\t\t{1.0f,0.007874f,0.f},\r\n\t\t{1.0f,0.015748f,0.f},\r\n\t\t{1.0f,0.023622f,0.f},\r\n\t\t{1.0f,0.031496f,0.f},\r\n\t\t{1.0f,0.03937f,0.f},\r\n\t\t{1.0f,0.047244f,0.f},\r\n\t\t{1.0f,0.055118f,0.f},\r\n\t\t{1.0f,0.062992f,0.f},\r\n\t\t{1.0f,0.070866f,0.f},\r\n\t\t{1.0f,0.07874f,0.f},\r\n\t\t{1.0f,0.086614f,0.f},\r\n\t\t{1.0f,0.094488f,0.f},\r\n\t\t{1.0f,0.10236f,0.f},\r\n\t\t{1.0f,0.11024f,0.f},\r\n\t\t{1.0f,0.11811f,0.f},\r\n\t\t{1.0f,0.12598f,0.f},\r\n\t\t{1.0f,0.13386f,0.f},\r\n\t\t{1.0f,0.14173f,0.f},\r\n\t\t{1.0f,0.14961f,0.f},\r\n\t\t{1.0f,0.15748f,0.f},\r\n\t\t{1.0f,0.16535f,0.f},\r\n\t\t{1.0f,0.17323f,0.f},\r\n\t\t{1.0f,0.1811f,0.f},\r\n\t\t{1.0f,0.18898f,0.f},\r\n\t\t{1.0f,0.19685f,0.f},\r\n\t\t{1.0f,0.20472f,0.f},\r\n\t\t{1.0f,0.2126f,0.f},\r\n\t\t{1.0f,0.22047f,0.f},\r\n\t\t{1.0f,0.22835f,0.f},\r\n\t\t{1.0f,0.23622f,0.f},\r\n\t\t{1.0f,0.24409f,0.f},\r\n\t\t{1.0f,0.25197f,0.f},\r\n\t\t{1.0f,0.25984f,0.f},\r\n\t\t{1.0f,0.26772f,0.f},\r\n\t\t{1.0f,0.27559f,0.f},\r\n\t\t{1.0f,0.28346f,0.f},\r\n\t\t{1.0f,0.29134f,0.f},\r\n\t\t{1.0f,0.29921f,0.f},\r\n\t\t{1.0f,0.30709f,0.f},\r\n\t\t{1.0f,0.31496f,0.f},\r\n\t\t{1.0f,0.32283f,0.f},\r\n\t\t{1.0f,0.33071f,0.f},\r\n\t\t{1.0f,0.33858f,0.f},\r\n\t\t{1.0f,0.34646f,0.f},\r\n\t\t{1.0f,0.35433f,0.f},\r\n\t\t{1.0f,0.3622f,0.f},\r\n\t\t{1.0f,0.37008f,0.f},\r\n\t\t{1.0f,0.37795f,0.f},\r\n\t\t{1.0f,0.38583f,0.f},\r\n\t\t{1.0f,0.3937f,0.f},\r\n\t\t{1.0f,0.40157f,0.f},\r\n\t\t{1.0f,0.40945f,0.f},\r\n\t\t{1.0f,0.41732f,0.f},\r\n\t\t{1.0f,0.4252f,0.f},\r\n\t\t{1.0f,0.43307f,0.f},\r\n\t\t{1.0f,0.44094f,0.f},\r\n\t\t{1.0f,0.44882f,0.f},\r\n\t\t{1.0f,0.45669f,0.f},\r\n\t\t{1.0f,0.46457f,0.f},\r\n\t\t{1.0f,0.47244f,0.f},\r\n\t\t{1.0f,0.48031f,0.f},\r\n\t\t{1.0f,0.48819f,0.f},\r\n\t\t{1.0f,0.49606f,0.f},\r\n\t\t{1.0f,0.50394f,0.f},\r\n\t\t{1.0f,0.51181f,0.f},\r\n\t\t{1.0f,0.51969f,0.f},\r\n\t\t{1.0f,0.52756f,0.f},\r\n\t\t{1.0f,0.53543f,0.f},\r\n\t\t{1.0f,0.54331f,0.f},\r\n\t\t{1.0f,0.55118f,0.f},\r\n\t\t{1.0f,0.55906f,0.f},\r\n\t\t{1.0f,0.56693f,0.f},\r\n\t\t{1.0f,0.5748f,0.f},\r\n\t\t{1.0f,0.58268f,0.f},\r\n\t\t{1.0f,0.59055f,0.f},\r\n\t\t{1.0f,0.59843f,0.f},\r\n\t\t{1.0f,0.6063f,0.f},\r\n\t\t{1.0f,0.61417f,0.f},\r\n\t\t{1.0f,0.62205f,0.f},\r\n\t\t{1.0f,0.62992f,0.f},\r\n\t\t{1.0f,0.6378f,0.f},\r\n\t\t{1.0f,0.64567f,0.f},\r\n\t\t{1.0f,0.65354f,0.f},\r\n\t\t{1.0f,0.66142f,0.f},\r\n\t\t{1.0f,0.66929f,0.f},\r\n\t\t{1.0f,0.67717f,0.f},\r\n\t\t{1.0f,0.68504f,0.f},\r\n\t\t{1.0f,0.69291f,0.f},\r\n\t\t{1.0f,0.70079f,0.f},\r\n\t\t{1.0f,0.70866f,0.f},\r\n\t\t{1.0f,0.71654f,0.f},\r\n\t\t{1.0f,0.72441f,0.f},\r\n\t\t{1.0f,0.73228f,0.f},\r\n\t\t{1.0f,0.74016f,0.f},\r\n\t\t{1.0f,0.74803f,0.f},\r\n\t\t{1.0f,0.75591f,0.f},\r\n\t\t{1.0f,0.76378f,0.f},\r\n\t\t{1.0f,0.77165f,0.f},\r\n\t\t{1.0f,0.77953f,0.f},\r\n\t\t{1.0f,0.7874f,0.f},\r\n\t\t{1.0f,0.79528f,0.f},\r\n\t\t{1.0f,0.80315f,0.f},\r\n\t\t{1.0f,0.81102f,0.f},\r\n\t\t{1.0f,0.8189f,0.f},\r\n\t\t{1.0f,0.82677f,0.f},\r\n\t\t{1.0f,0.83465f,0.f},\r\n\t\t{1.0f,0.84252f,0.f},\r\n\t\t{1.0f,0.85039f,0.f},\r\n\t\t{1.0f,0.85827f,0.f},\r\n\t\t{1.0f,0.86614f,0.f},\r\n\t\t{1.0f,0.87402f,0.f},\r\n\t\t{1.0f,0.88189f,0.f},\r\n\t\t{1.0f,0.88976f,0.f},\r\n\t\t{1.0f,0.89764f,0.f},\r\n\t\t{1.0f,0.90551f,0.f},\r\n\t\t{1.0f,0.91339f,0.f},\r\n\t\t{1.0f,0.92126f,0.f},\r\n\t\t{1.0f,0.92913f,0.f},\r\n\t\t{1.0f,0.93701f,0.f},\r\n\t\t{1.0f,0.94488f,0.f},\r\n\t\t{1.0f,0.95276f,0.f},\r\n\t\t{1.0f,0.96063f,0.f},\r\n\t\t{1.0f,0.9685f,0.f},\r\n\t\t{1.0f,0.97638f,0.f},\r\n\t\t{1.0f,0.98425f,0.f},\r\n\t\t{1.0f,0.99213f,0.f},\r\n\t\t{1.0f,1.0f,0.0f}\r\n};\r\n\r\n\r\nfloat colormapJet[128][3] =\r\n{\r\n\t\t{0.0f,0.0f,0.53125f},\r\n\t\t{0.0f,0.0f,0.5625f},\r\n\t\t{0.0f,0.0f,0.59375f},\r\n\t\t{0.0f,0.0f,0.625f},\r\n\t\t{0.0f,0.0f,0.65625f},\r\n\t\t{0.0f,0.0f,0.6875f},\r\n\t\t{0.0f,0.0f,0.71875f},\r\n\t\t{0.0f,0.0f,0.75f},\r\n\t\t{0.0f,0.0f,0.78125f},\r\n\t\t{0.0f,0.0f,0.8125f},\r\n\t\t{0.0f,0.0f,0.84375f},\r\n\t\t{0.0f,0.0f,0.875f},\r\n\t\t{0.0f,0.0f,0.90625f},\r\n\t\t{0.0f,0.0f,0.9375f},\r\n\t\t{0.0f,0.0f,0.96875f},\r\n\t\t{0.0f,0.0f,1.0f},\r\n\t\t{0.0f,0.03125f,1.0f},\r\n\t\t{0.0f,0.0625f,1.0f},\r\n\t\t{0.0f,0.09375f,1.0f},\r\n\t\t{0.0f,0.125f,1.0f},\r\n\t\t{0.0f,0.15625f,1.0f},\r\n\t\t{0.0f,0.1875f,1.0f},\r\n\t\t{0.0f,0.21875f,1.0f},\r\n\t\t{0.0f,0.25f,1.0f},\r\n\t\t{0.0f,0.28125f,1.0f},\r\n\t\t{0.0f,0.3125f,1.0f},\r\n\t\t{0.0f,0.34375f,1.0f},\r\n\t\t{0.0f,0.375f,1.0f},\r\n\t\t{0.0f,0.40625f,1.0f},\r\n\t\t{0.0f,0.4375f,1.0f},\r\n\t\t{0.0f,0.46875f,1.0f},\r\n\t\t{0.0f,0.5f,1.0f},\r\n\t\t{0.0f,0.53125f,1.0f},\r\n\t\t{0.0f,0.5625f,1.0f},\r\n\t\t{0.0f,0.59375f,1.0f},\r\n\t\t{0.0f,0.625f,1.0f},\r\n\t\t{0.0f,0.65625f,1.0f},\r\n\t\t{0.0f,0.6875f,1.0f},\r\n\t\t{0.0f,0.71875f,1.0f},\r\n\t\t{0.0f,0.75f,1.0f},\r\n\t\t{0.0f,0.78125f,1.0f},\r\n\t\t{0.0f,0.8125f,1.0f},\r\n\t\t{0.0f,0.84375f,1.0f},\r\n\t\t{0.0f,0.875f,1.0f},\r\n\t\t{0.0f,0.90625f,1.0f},\r\n\t\t{0.0f,0.9375f,1.0f},\r\n\t\t{0.0f,0.96875f,1.0f},\r\n\t\t{0.0f,1.0f,1.0f},\r\n\t\t{0.03125f,1.0f,0.96875f},\r\n\t\t{0.0625f,1.0f,0.9375f},\r\n\t\t{0.09375f,1.0f,0.90625f},\r\n\t\t{0.125f,1.0f,0.875f},\r\n\t\t{0.15625f,1.0f,0.84375f},\r\n\t\t{0.1875f,1.0f,0.8125f},\r\n\t\t{0.21875f,1.0f,0.78125f},\r\n\t\t{0.25f,1.0f,0.75f},\r\n\t\t{0.28125f,1.0f,0.71875f},\r\n\t\t{0.3125f,1.0f,0.6875f},\r\n\t\t{0.34375f,1.0f,0.65625f},\r\n\t\t{0.375f,1.0f,0.625f},\r\n\t\t{0.40625f,1.0f,0.59375f},\r\n\t\t{0.4375f,1.0f,0.5625f},\r\n\t\t{0.46875f,1.0f,0.53125f},\r\n\t\t{0.5f,1.0f,0.5f},\r\n\t\t{0.53125f,1.0f,0.46875f},\r\n\t\t{0.5625f,1.0f,0.4375f},\r\n\t\t{0.59375f,1.0f,0.40625f},\r\n\t\t{0.625f,1.0f,0.375f},\r\n\t\t{0.65625f,1.0f,0.34375f},\r\n\t\t{0.6875f,1.0f,0.3125f},\r\n\t\t{0.71875f,1.0f,0.28125f},\r\n\t\t{0.75f,1.0f,0.25f},\r\n\t\t{0.78125f,1.0f,0.21875f},\r\n\t\t{0.8125f,1.0f,0.1875f},\r\n\t\t{0.84375f,1.0f,0.15625f},\r\n\t\t{0.875f,1.0f,0.125f},\r\n\t\t{0.90625f,1.0f,0.09375f},\r\n\t\t{0.9375f,1.0f,0.0625f},\r\n\t\t{0.96875f,1.0f,0.03125f},\r\n\t\t{1.0f,1.0f,0.0f},\r\n\t\t{1.0f,0.96875f,0.0f},\r\n\t\t{1.0f,0.9375f,0.0f},\r\n\t\t{1.0f,0.90625f,0.0f},\r\n\t\t{1.0f,0.875f,0.0f},\r\n\t\t{1.0f,0.84375f,0.0f},\r\n\t\t{1.0f,0.8125f,0.0f},\r\n\t\t{1.0f,0.78125f,0.0f},\r\n\t\t{1.0f,0.75f,0.0f},\r\n\t\t{1.0f,0.71875f,0.0f},\r\n\t\t{1.0f,0.6875f,0.0f},\r\n\t\t{1.0f,0.65625f,0.0f},\r\n\t\t{1.0f,0.625f,0.0f},\r\n\t\t{1.0f,0.59375f,0.0f},\r\n\t\t{1.0f,0.5625f,0.0f},\r\n\t\t{1.0f,0.53125f,0.0f},\r\n\t\t{1.0f,0.5f,0.0f},\r\n\t\t{1.0f,0.46875f,0.0f},\r\n\t\t{1.0f,0.4375f,0.0f},\r\n\t\t{1.0f,0.40625f,0.0f},\r\n\t\t{1.0f,0.375f,0.0f},\r\n\t\t{1.0f,0.34375f,0.0f},\r\n\t\t{1.0f,0.3125f,0.0f},\r\n\t\t{1.0f,0.28125f,0.0f},\r\n\t\t{1.0f,0.25f,0.0f},\r\n\t\t{1.0f,0.21875f,0.0f},\r\n\t\t{1.0f,0.1875f,0.0f},\r\n\t\t{1.0f,0.15625f,0.0f},\r\n\t\t{1.0f,0.125f,0.0f},\r\n\t\t{1.0f,0.09375f,0.0f},\r\n\t\t{1.0f,0.0625f,0.0f},\r\n\t\t{1.0f,0.03125f,0.0f},\r\n\t\t{1.0f,0.0f,0.0f},\r\n\t\t{0.96875f,0.0f,0.0f},\r\n\t\t{0.9375f,0.0f,0.0f},\r\n\t\t{0.90625f,0.0f,0.0f},\r\n\t\t{0.875f,0.0f,0.0f},\r\n\t\t{0.84375f,0.0f,0.0f},\r\n\t\t{0.8125f,0.0f,0.0f},\r\n\t\t{0.78125f,0.0f,0.0f},\r\n\t\t{0.75f,0.0f,0.0f},\r\n\t\t{0.71875f,0.0f,0.0f},\r\n\t\t{0.6875f,0.0f,0.0f},\r\n\t\t{0.65625f,0.0f,0.0f},\r\n\t\t{0.625f,0.0f,0.0f},\r\n\t\t{0.59375f,0.0f,0.0f},\r\n\t\t{0.5625f,0.0f,0.0f},\r\n\t\t{0.53125f,0.0f,0.0f},\r\n\t\t{0.5f,0.0f,0.0f}\r\n};\r\n\r\nvoid colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth,\r\n\t\t\t\t\t float minRange, float maxRange)\r\n{\r\n\timgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3);\r\n\r\n\tfor (int i = 0; i < imgColoredDepth.rows; ++i)\r\n\t{\r\n\t\tconst float* depth = imgDepth.ptr(i);\r\n\t\tunsigned char* pixel = imgColoredDepth.ptr(i);\r\n\t\tfor (int j = 0; j < imgColoredDepth.cols; ++j)\r\n\t\t{\r\n\t\t\tif (depth[j] != 0)\r\n\t\t\t{\r\n\t\t\t\tint idx = fminf(depth[j] - minRange, maxRange - minRange) / (maxRange - minRange) * 127.0f;\r\n\t\t\t\tidx = 127 - idx;\r\n\r\n\t\t\t\tpixel[0] = colormapJet[idx][2] * 255.0f;\r\n\t\t\t\tpixel[1] = colormapJet[idx][1] * 255.0f;\r\n\t\t\t\tpixel[2] = colormapJet[idx][0] * 255.0f;\r\n\t\t\t}\r\n\r\n\t\t\tpixel += 3;\r\n\t\t}\r\n\t}\r\n}\r\n\r\nbool colormap(const std::string& name, unsigned char idx,\r\n\t\t\t float& r, float& g, float& b)\r\n{\r\n\tif (name.compare(\"jet\") == 0)\r\n\t{\r\n\t\tfloat* color = colormapJet[idx];\r\n\r\n\t\tr = color[0];\r\n\t\tg = color[1];\r\n\t\tb = color[2];\r\n\r\n\t\treturn true;\r\n\t}\r\n\telse if (name.compare(\"autumn\") == 0)\r\n\t{\r\n\t\tfloat* color = colormapAutumn[idx];\r\n\r\n\t\tr = color[0];\r\n\t\tg = color[1];\r\n\t\tb = color[2];\r\n\r\n\t\treturn true;\r\n\t}\r\n\r\n\treturn false;\r\n}\r\n\r\nstd::vector bresLine(int x0, int y0, int x1, int y1)\r\n{\r\n\t// Bresenham's line algorithm\r\n\t// Find cells intersected by line between (x0,y0) and (x1,y1)\r\n\r\n\tstd::vector cells;\r\n\r\n\tint dx = std::abs(x1 - x0);\r\n\tint dy = std::abs(y1 - y0);\r\n\r\n\tint sx = (x0 < x1) ? 1 : -1;\r\n\tint sy = (y0 < y1) ? 1 : -1;\r\n\r\n\tint err = dx - dy;\r\n\r\n\twhile (1)\r\n\t{\r\n\t\tcells.push_back(cv::Point2i(x0, y0));\r\n\r\n\t\tif (x0 == x1 && y0 == y1)\r\n\t\t{\r\n\t\t\tbreak;\r\n\t\t}\r\n\r\n\t\tint e2 = 2 * err;\r\n\t\tif (e2 > -dy)\r\n\t\t{\r\n\t\t\terr -= dy;\r\n\t\t\tx0 += sx;\r\n\t\t}\r\n\t\tif (e2 < dx)\r\n\t\t{\r\n\t\t\terr += dx;\r\n\t\t\ty0 += sy;\r\n\t\t}\r\n\t}\r\n\r\n\treturn cells;\r\n}\r\n\r\nstd::vector bresCircle(int x0, int y0, int r)\r\n{\r\n\t// Bresenham's circle algorithm\r\n\t// Find cells intersected by circle with center (x0,y0) and radius r\r\n\r\n\tstd::vector< std::vector > mask(2 * r + 1);\r\n\t\r\n\tfor (int i = 0; i < 2 * r + 1; ++i)\r\n\t{\r\n\t\tmask[i].resize(2 * r + 1);\r\n\t\tfor (int j = 0; j < 2 * r + 1; ++j)\r\n\t\t{\r\n\t\t\tmask[i][j] = false;\r\n\t\t}\r\n\t}\r\n\r\n\tint f = 1 - r;\r\n\tint ddF_x = 1;\r\n\tint ddF_y = -2 * r;\r\n\tint x = 0;\r\n\tint y = r;\r\n\r\n\tstd::vector line;\r\n\r\n\tline = bresLine(x0, y0 - r, x0, y0 + r);\r\n\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t{\r\n\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t}\r\n\r\n\tline = bresLine(x0 - r, y0, x0 + r, y0);\r\n\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t{\r\n\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t}\r\n\r\n\twhile (x < y)\r\n\t{\r\n\t\tif (f >= 0)\r\n\t\t{\r\n\t\t\ty--;\r\n\t\t\tddF_y += 2;\r\n\t\t\tf += ddF_y;\r\n\t\t}\r\n\r\n\t\tx++;\r\n\t\tddF_x += 2;\r\n\t\tf += ddF_x;\r\n\r\n\t\tline = bresLine(x0 - x, y0 + y, x0 + x, y0 + y);\r\n\t\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - x, y0 - y, x0 + x, y0 - y);\r\n\t\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - y, y0 + x, x0 + y, y0 + x);\r\n\t\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - y, y0 - x, x0 + y, y0 - x);\r\n\t\tfor (std::vector::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\t}\r\n\r\n\tstd::vector cells;\r\n\tfor (int i = 0; i < 2 * r + 1; ++i)\r\n\t{\r\n\t\tfor (int j = 0; j < 2 * r + 1; ++j)\r\n\t\t{\r\n\t\t\tif (mask[i][j])\r\n\t\t\t{\r\n\t\t\t\tcells.push_back(cv::Point2i(i - r + x0, j - r + y0));\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\treturn cells;\r\n}\r\n\r\nvoid\r\nfitCircle(const std::vector& points,\r\n double& centerX, double& centerY, double& radius)\r\n{\r\n // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data,\r\n // IEEE Transactions on Instrumentation and Measurement, 2000\r\n // We use the modified least squares method.\r\n double sum_x = 0.0;\r\n double sum_y = 0.0;\r\n double sum_xx = 0.0;\r\n double sum_xy = 0.0;\r\n double sum_yy = 0.0;\r\n double sum_xxx = 0.0;\r\n double sum_xxy = 0.0;\r\n double sum_xyy = 0.0;\r\n double sum_yyy = 0.0;\r\n\r\n int n = points.size();\r\n for (int i = 0; i < n; ++i)\r\n {\r\n double x = points.at(i).x;\r\n double y = points.at(i).y;\r\n\r\n sum_x += x;\r\n sum_y += y;\r\n sum_xx += x * x;\r\n sum_xy += x * y;\r\n sum_yy += y * y;\r\n sum_xxx += x * x * x;\r\n sum_xxy += x * x * y;\r\n sum_xyy += x * y * y;\r\n sum_yyy += y * y * y;\r\n }\r\n\r\n double A = n * sum_xx - square(sum_x);\r\n double B = n * sum_xy - sum_x * sum_y;\r\n double C = n * sum_yy - square(sum_y);\r\n double D = 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx);\r\n double E = 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy);\r\n\r\n centerX = (D * C - B * E) / (A * C - square(B));\r\n centerY = (A * E - B * D) / (A * C - square(B));\r\n\r\n double sum_r = 0.0;\r\n for (int i = 0; i < n; ++i)\r\n {\r\n double x = points.at(i).x;\r\n double y = points.at(i).y;\r\n\r\n sum_r += hypot(x - centerX, y - centerY);\r\n }\r\n\r\n radius = sum_r / n;\r\n}\r\n\r\nstd::vector\r\nintersectCircles(double x1, double y1, double r1,\r\n double x2, double y2, double r2)\r\n{\r\n std::vector ipts;\r\n\r\n double d = hypot(x1 - x2, y1 - y2);\r\n if (d > r1 + r2)\r\n {\r\n // circles are separate\r\n return ipts;\r\n }\r\n if (d < fabs(r1 - r2))\r\n {\r\n // one circle is contained within the other\r\n return ipts;\r\n }\r\n\r\n double a = (square(r1) - square(r2) + square(d)) / (2.0 * d);\r\n double h = sqrt(square(r1) - square(a));\r\n\r\n double x3 = x1 + a * (x2 - x1) / d;\r\n double y3 = y1 + a * (y2 - y1) / d;\r\n\r\n if (h < 1e-10)\r\n {\r\n // two circles touch at one point\r\n ipts.push_back(cv::Point2d(x3, y3));\r\n return ipts;\r\n }\r\n\r\n ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d,\r\n y3 - h * (x2 - x1) / d));\r\n ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d,\r\n y3 + h * (x2 - x1) / d));\r\n return ipts;\r\n}\r\n\r\nchar\r\nUTMLetterDesignator(double latitude)\r\n{\r\n // This routine determines the correct UTM letter designator for the given latitude\r\n // returns 'Z' if latitude is outside the UTM limits of 84N to 80S\r\n // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n char letterDesignator;\r\n\r\n if ((84.0 >= latitude) && (latitude >= 72.0)) letterDesignator = 'X';\r\n else if ((72.0 > latitude) && (latitude >= 64.0)) letterDesignator = 'W';\r\n else if ((64.0 > latitude) && (latitude >= 56.0)) letterDesignator = 'V';\r\n else if ((56.0 > latitude) && (latitude >= 48.0)) letterDesignator = 'U';\r\n else if ((48.0 > latitude) && (latitude >= 40.0)) letterDesignator = 'T';\r\n else if ((40.0 > latitude) && (latitude >= 32.0)) letterDesignator = 'S';\r\n else if ((32.0 > latitude) && (latitude >= 24.0)) letterDesignator = 'R';\r\n else if ((24.0 > latitude) && (latitude >= 16.0)) letterDesignator = 'Q';\r\n else if ((16.0 > latitude) && (latitude >= 8.0)) letterDesignator = 'P';\r\n else if (( 8.0 > latitude) && (latitude >= 0.0)) letterDesignator = 'N';\r\n else if (( 0.0 > latitude) && (latitude >= -8.0)) letterDesignator = 'M';\r\n else if ((-8.0 > latitude) && (latitude >= -16.0)) letterDesignator = 'L';\r\n else if ((-16.0 > latitude) && (latitude >= -24.0)) letterDesignator = 'K';\r\n else if ((-24.0 > latitude) && (latitude >= -32.0)) letterDesignator = 'J';\r\n else if ((-32.0 > latitude) && (latitude >= -40.0)) letterDesignator = 'H';\r\n else if ((-40.0 > latitude) && (latitude >= -48.0)) letterDesignator = 'G';\r\n else if ((-48.0 > latitude) && (latitude >= -56.0)) letterDesignator = 'F';\r\n else if ((-56.0 > latitude) && (latitude >= -64.0)) letterDesignator = 'E';\r\n else if ((-64.0 > latitude) && (latitude >= -72.0)) letterDesignator = 'D';\r\n else if ((-72.0 > latitude) && (latitude >= -80.0)) letterDesignator = 'C';\r\n else letterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits\r\n\r\n return letterDesignator;\r\n}\r\n\r\nvoid\r\nLLtoUTM(double latitude, double longitude,\r\n double& utmNorthing, double& utmEasting, std::string& utmZone)\r\n{\r\n // converts lat/long to UTM coords. Equations from USGS Bulletin 1532\r\n // East Longitudes are positive, West longitudes are negative.\r\n // North latitudes are positive, South latitudes are negative\r\n // Lat and Long are in decimal degrees\r\n // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n\r\n double k0 = 0.9996;\r\n\r\n double LongOrigin;\r\n double eccPrimeSquared;\r\n double N, T, C, A, M;\r\n\r\n double LatRad = latitude * M_PI / 180.0;\r\n double LongRad = longitude * M_PI / 180.0;\r\n double LongOriginRad;\r\n\r\n int ZoneNumber = static_cast((longitude + 180.0) / 6.0) + 1;\r\n\r\n if (latitude >= 56.0 && latitude < 64.0 &&\r\n longitude >= 3.0 && longitude < 12.0) {\r\n ZoneNumber = 32;\r\n }\r\n\r\n // Special zones for Svalbard\r\n if (latitude >= 72.0 && latitude < 84.0) {\r\n if ( longitude >= 0.0 && longitude < 9.0) ZoneNumber = 31;\r\n else if (longitude >= 9.0 && longitude < 21.0) ZoneNumber = 33;\r\n else if (longitude >= 21.0 && longitude < 33.0) ZoneNumber = 35;\r\n else if (longitude >= 33.0 && longitude < 42.0) ZoneNumber = 37;\r\n }\r\n LongOrigin = static_cast((ZoneNumber - 1) * 6 - 180 + 3); //+3 puts origin in middle of zone\r\n LongOriginRad = LongOrigin * M_PI / 180.0;\r\n\r\n // compute the UTM Zone from the latitude and longitude\r\n std::ostringstream oss;\r\n oss << ZoneNumber << UTMLetterDesignator(latitude);\r\n utmZone = oss.str();\r\n\r\n eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);\r\n\r\n N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad));\r\n T = tan(LatRad) * tan(LatRad);\r\n C = eccPrimeSquared * cos(LatRad) * cos(LatRad);\r\n A = cos(LatRad) * (LongRad - LongOriginRad);\r\n\r\n M = WGS84_A * ((1.0 - WGS84_ECCSQ / 4.0\r\n - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0\r\n - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0)\r\n * LatRad\r\n - (3.0 * WGS84_ECCSQ / 8.0\r\n + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0\r\n + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0)\r\n * sin(2.0 * LatRad)\r\n + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0\r\n + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0)\r\n * sin(4.0 * LatRad)\r\n - (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0)\r\n * sin(6.0 * LatRad));\r\n\r\n utmEasting = k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0\r\n + (5.0 - 18.0 * T + T * T + 72.0 * C\r\n - 58.0 * eccPrimeSquared)\r\n * A * A * A * A * A / 120.0)\r\n + 500000.0;\r\n\r\n utmNorthing = k0 * (M + N * tan(LatRad) *\r\n (A * A / 2.0 +\r\n (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0\r\n + (61.0 - 58.0 * T + T * T + 600.0 * C\r\n - 330.0 * eccPrimeSquared)\r\n * A * A * A * A * A * A / 720.0));\r\n if (latitude < 0.0) {\r\n utmNorthing += 10000000.0; //10000000 meter offset for southern hemisphere\r\n }\r\n}\r\n\r\nvoid\r\nUTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone,\r\n double& latitude, double& longitude)\r\n{\r\n // converts UTM coords to lat/long. Equations from USGS Bulletin 1532\r\n // East Longitudes are positive, West longitudes are negative.\r\n // North latitudes are positive, South latitudes are negative\r\n // Lat and Long are in decimal degrees.\r\n // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n\r\n double k0 = 0.9996;\r\n double eccPrimeSquared;\r\n double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ));\r\n double N1, T1, C1, R1, D, M;\r\n double LongOrigin;\r\n double mu, phi1, phi1Rad;\r\n double x, y;\r\n int ZoneNumber;\r\n char ZoneLetter;\r\n bool NorthernHemisphere;\r\n\r\n x = utmEasting - 500000.0; //remove 500,000 meter offset for longitude\r\n y = utmNorthing;\r\n\r\n std::istringstream iss(utmZone);\r\n iss >> ZoneNumber >> ZoneLetter;\r\n if ((static_cast(ZoneLetter) - static_cast('N')) >= 0) {\r\n NorthernHemisphere = true;//point is in northern hemisphere\r\n } else {\r\n NorthernHemisphere = false;//point is in southern hemisphere\r\n y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere\r\n }\r\n\r\n LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + 3.0; //+3 puts origin in middle of zone\r\n\r\n eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);\r\n\r\n M = y / k0;\r\n mu = M / (WGS84_A * (1.0 - WGS84_ECCSQ / 4.0\r\n - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0\r\n - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0));\r\n\r\n phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu)\r\n + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0)\r\n * sin(4.0 * mu)\r\n + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu);\r\n phi1 = phi1Rad / M_PI * 180.0;\r\n\r\n N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad));\r\n T1 = tan(phi1Rad) * tan(phi1Rad);\r\n C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);\r\n R1 = WGS84_A * (1.0 - WGS84_ECCSQ) /\r\n pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5);\r\n D = x / (N1 * k0);\r\n\r\n latitude = phi1Rad - (N1 * tan(phi1Rad) / R1)\r\n * (D * D / 2.0 - (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1\r\n - 9.0 * eccPrimeSquared) * D * D * D * D / 24.0\r\n + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1\r\n - 252.0 * eccPrimeSquared - 3.0 * C1 * C1)\r\n * D * D * D * D * D * D / 720.0);\r\n latitude *= 180.0 / M_PI;\r\n\r\n longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0\r\n + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1\r\n + 8.0 * eccPrimeSquared + 24.0 * T1 * T1)\r\n * D * D * D * D * D / 120.0) / cos(phi1Rad);\r\n longitude = LongOrigin + longitude / M_PI * 180.0;\r\n}\r\n\r\nlong int\r\ntimestampDiff(uint64_t t1, uint64_t t2)\r\n{\r\n if (t2 > t1)\r\n {\r\n uint64_t d = t2 - t1;\r\n\r\n if (d > std::numeric_limits::max())\r\n {\r\n return std::numeric_limits::max();\r\n }\r\n else\r\n {\r\n return d;\r\n }\r\n }\r\n else\r\n {\r\n uint64_t d = t1 - t2;\r\n\r\n if (d > std::numeric_limits::max())\r\n {\r\n return std::numeric_limits::min();\r\n }\r\n else\r\n {\r\n return - static_cast(d);\r\n }\r\n }\r\n}\r\n\r\n}\r\n" }, { "path": "src/utils/camodocal/src/intrinsic_calib.cc", "content": "#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"camodocal/chessboard/Chessboard.h\"\n#include \"camodocal/calib/CameraCalibration.h\"\n#include \"camodocal/gpl/gpl.h\"\n\nint main(int argc, char** argv)\n{\n cv::Size boardSize;\n float squareSize;\n std::string inputDir;\n std::string cameraModel;\n std::string cameraName;\n std::string prefix;\n std::string fileExtension;\n bool useOpenCV;\n bool viewResults;\n bool verbose;\n\n //========= Handling Program options =========\n boost::program_options::options_description desc(\"Allowed options\");\n desc.add_options()\n (\"help\", \"produce help message\")\n (\"width,w\", boost::program_options::value(&boardSize.width)->default_value(8), \"Number of inner corners on the chessboard pattern in x direction\")\n (\"height,h\", boost::program_options::value(&boardSize.height)->default_value(12), \"Number of inner corners on the chessboard pattern in y direction\")\n (\"size,s\", boost::program_options::value(&squareSize)->default_value(7.f), \"Size of one square in mm\")\n (\"input,i\", boost::program_options::value(&inputDir)->default_value(\"calibrationdata\"), \"Input directory containing chessboard images\")\n (\"prefix,p\", boost::program_options::value(&prefix)->default_value(\"left-\"), \"Prefix of images\")\n (\"file-extension,e\", boost::program_options::value(&fileExtension)->default_value(\".png\"), \"File extension of images\")\n (\"camera-model\", boost::program_options::value(&cameraModel)->default_value(\"mei\"), \"Camera model: kannala-brandt | mei | pinhole\")\n (\"camera-name\", boost::program_options::value(&cameraName)->default_value(\"camera\"), \"Name of camera\")\n (\"opencv\", boost::program_options::bool_switch(&useOpenCV)->default_value(true), \"Use OpenCV to detect corners\")\n (\"view-results\", boost::program_options::bool_switch(&viewResults)->default_value(false), \"View results\")\n (\"verbose,v\", boost::program_options::bool_switch(&verbose)->default_value(true), \"Verbose output\")\n ;\n\n boost::program_options::positional_options_description pdesc;\n pdesc.add(\"input\", 1);\n\n boost::program_options::variables_map vm;\n boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);\n boost::program_options::notify(vm);\n\n if (vm.count(\"help\"))\n {\n std::cout << desc << std::endl;\n return 1;\n }\n\n if (!boost::filesystem::exists(inputDir) && !boost::filesystem::is_directory(inputDir))\n {\n std::cerr << \"# ERROR: Cannot find input directory \" << inputDir << \".\" << std::endl;\n return 1;\n }\n\n camodocal::Camera::ModelType modelType;\n if (boost::iequals(cameraModel, \"kannala-brandt\"))\n {\n modelType = camodocal::Camera::KANNALA_BRANDT;\n }\n else if (boost::iequals(cameraModel, \"mei\"))\n {\n modelType = camodocal::Camera::MEI;\n }\n else if (boost::iequals(cameraModel, \"pinhole\"))\n {\n modelType = camodocal::Camera::PINHOLE;\n }\n else if (boost::iequals(cameraModel, \"scaramuzza\"))\n {\n modelType = camodocal::Camera::SCARAMUZZA;\n }\n else\n {\n std::cerr << \"# ERROR: Unknown camera model: \" << cameraModel << std::endl;\n return 1;\n }\n\n switch (modelType)\n {\n case camodocal::Camera::KANNALA_BRANDT:\n std::cout << \"# INFO: Camera model: Kannala-Brandt\" << std::endl;\n break;\n case camodocal::Camera::MEI:\n std::cout << \"# INFO: Camera model: Mei\" << std::endl;\n break;\n case camodocal::Camera::PINHOLE:\n std::cout << \"# INFO: Camera model: Pinhole\" << std::endl;\n break;\n case camodocal::Camera::SCARAMUZZA:\n std::cout << \"# INFO: Camera model: Scaramuzza-Omnidirect\" << std::endl;\n break;\n }\n\n // look for images in input directory\n std::vector imageFilenames;\n boost::filesystem::directory_iterator itr;\n for (boost::filesystem::directory_iterator itr(inputDir); itr != boost::filesystem::directory_iterator(); ++itr)\n {\n if (!boost::filesystem::is_regular_file(itr->status()))\n {\n continue;\n }\n\n std::string filename = itr->path().filename().string();\n\n // check if prefix matches\n if (!prefix.empty())\n {\n if (filename.compare(0, prefix.length(), prefix) != 0)\n {\n continue;\n }\n }\n\n // check if file extension matches\n if (filename.compare(filename.length() - fileExtension.length(), fileExtension.length(), fileExtension) != 0)\n {\n continue;\n }\n\n imageFilenames.push_back(itr->path().string());\n\n if (verbose)\n {\n std::cerr << \"# INFO: Adding \" << imageFilenames.back() << std::endl;\n }\n }\n\n if (imageFilenames.empty())\n {\n std::cerr << \"# ERROR: No chessboard images found.\" << std::endl;\n return 1;\n }\n\n if (verbose)\n {\n std::cerr << \"# INFO: # images: \" << imageFilenames.size() << std::endl;\n }\n\n std::sort(imageFilenames.begin(), imageFilenames.end());\n\n cv::Mat image = cv::imread(imageFilenames.front(), -1);\n const cv::Size frameSize = image.size();\n\n camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, boardSize, squareSize);\n calibration.setVerbose(verbose);\n\n std::vector chessboardFound(imageFilenames.size(), false);\n for (size_t i = 0; i < imageFilenames.size(); ++i)\n {\n // image = cv::imread(imageFilenames.at(i), -1);\n image = cv::imread(imageFilenames.at(i), 0);\n\n camodocal::Chessboard chessboard(boardSize, image);\n\n chessboard.findCorners(useOpenCV);\n if (chessboard.cornersFound())\n {\n if (verbose)\n {\n std::cerr << \"# INFO: Detected chessboard in image \" << i + 1 << \", \" << imageFilenames.at(i) << std::endl;\n }\n\n calibration.addChessboardData(chessboard.getCorners());\n\n cv::Mat sketch;\n chessboard.getSketch().copyTo(sketch);\n\n cv::imshow(\"Image\", sketch);\n cv::waitKey(50);\n }\n else if (verbose)\n {\n std::cerr << \"# INFO: Did not detect chessboard in image \" << i + 1 << std::endl;\n }\n chessboardFound.at(i) = chessboard.cornersFound();\n }\n cv::destroyWindow(\"Image\");\n\n if (calibration.sampleCount() < 10)\n {\n std::cerr << \"# ERROR: Insufficient number of detected chessboards.\" << std::endl;\n return 1;\n }\n\n if (verbose)\n {\n std::cerr << \"# INFO: Calibrating...\" << std::endl;\n }\n\n double startTime = camodocal::timeInSeconds();\n\n calibration.calibrate();\n calibration.writeParams(cameraName + \"_camera_calib.yaml\");\n calibration.writeChessboardData(cameraName + \"_chessboard_data.dat\");\n\n if (verbose)\n {\n std::cout << \"# INFO: Calibration took a total time of \"\n << std::fixed << std::setprecision(3) << camodocal::timeInSeconds() - startTime\n << \" sec.\\n\";\n }\n\n if (verbose)\n {\n std::cerr << \"# INFO: Wrote calibration file to \" << cameraName + \"_camera_calib.yaml\" << std::endl;\n }\n\n if (viewResults)\n {\n std::vector cbImages;\n std::vector cbImageFilenames;\n\n for (size_t i = 0; i < imageFilenames.size(); ++i)\n {\n if (!chessboardFound.at(i))\n {\n continue;\n }\n\n cbImages.push_back(cv::imread(imageFilenames.at(i), -1));\n cbImageFilenames.push_back(imageFilenames.at(i));\n }\n\n // visualize observed and reprojected points\n calibration.drawResults(cbImages);\n\n for (size_t i = 0; i < cbImages.size(); ++i)\n {\n cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10,20),\n cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),\n 1, CV_AA);\n cv::imshow(\"Image\", cbImages.at(i));\n cv::waitKey(0);\n }\n }\n\n return 0;\n}\n" }, { "path": "src/utils/camodocal/src/sparse_graph/Transform.cc", "content": "#include \n\nnamespace camodocal\n{\n\nTransform::Transform()\n{\n m_q.setIdentity();\n m_t.setZero();\n}\n\nTransform::Transform(const Eigen::Matrix4d& H)\n{\n m_q = Eigen::Quaterniond(H.block<3,3>(0,0));\n m_t = H.block<3,1>(0,3);\n}\n\nEigen::Quaterniond&\nTransform::rotation(void)\n{\n return m_q;\n}\n\nconst Eigen::Quaterniond&\nTransform::rotation(void) const\n{\n return m_q;\n}\n\ndouble*\nTransform::rotationData(void)\n{\n return m_q.coeffs().data();\n}\n\nconst double* const\nTransform::rotationData(void) const\n{\n return m_q.coeffs().data();\n}\n\nEigen::Vector3d&\nTransform::translation(void)\n{\n return m_t;\n}\n\nconst Eigen::Vector3d&\nTransform::translation(void) const\n{\n return m_t;\n}\n\ndouble*\nTransform::translationData(void)\n{\n return m_t.data();\n}\n\nconst double* const\nTransform::translationData(void) const\n{\n return m_t.data();\n}\n\nEigen::Matrix4d\nTransform::toMatrix(void) const\n{\n Eigen::Matrix4d H;\n H.setIdentity();\n H.block<3,3>(0,0) = m_q.toRotationMatrix();\n H.block<3,1>(0,3) = m_t;\n\n return H;\n}\n\n}\n" }, { "path": "src/utils/nlohmann/json.hpp", "content": "/*\n __ _____ _____ _____\n __| | __| | | | JSON for Modern C++\n| | |__ | | | | | | version 3.4.0\n|_____|_____|_____|_|___| https://github.com/nlohmann/json\n\nLicensed under the MIT License .\nSPDX-License-Identifier: MIT\nCopyright (c) 2013-2018 Niels Lohmann .\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n*/\n\n#ifndef NLOHMANN_JSON_HPP\n#define NLOHMANN_JSON_HPP\n\n#define NLOHMANN_JSON_VERSION_MAJOR 3\n#define NLOHMANN_JSON_VERSION_MINOR 4\n#define NLOHMANN_JSON_VERSION_PATCH 0\n\n#include // all_of, find, for_each\n#include // assert\n#include // and, not, or\n#include // nullptr_t, ptrdiff_t, size_t\n#include // hash, less\n#include // initializer_list\n#include // istream, ostream\n#include // iterator_traits, random_access_iterator_tag\n#include // accumulate\n#include // string, stoi, to_string\n#include // declval, forward, move, pair, swap\n\n// #include \n#ifndef NLOHMANN_JSON_FWD_HPP\n#define NLOHMANN_JSON_FWD_HPP\n\n#include // int64_t, uint64_t\n#include // map\n#include // allocator\n#include // string\n#include // vector\n\n/*!\n@brief namespace for Niels Lohmann\n@see https://github.com/nlohmann\n@since version 1.0.0\n*/\nnamespace nlohmann\n{\n/*!\n@brief default JSONSerializer template argument\n\nThis serializer ignores the template arguments and uses ADL\n([argument-dependent lookup](https://en.cppreference.com/w/cpp/language/adl))\nfor serialization.\n*/\ntemplate\nstruct adl_serializer;\n\ntemplate class ObjectType =\n std::map,\n template class ArrayType = std::vector,\n class StringType = std::string, class BooleanType = bool,\n class NumberIntegerType = std::int64_t,\n class NumberUnsignedType = std::uint64_t,\n class NumberFloatType = double,\n template class AllocatorType = std::allocator,\n template class JSONSerializer =\n adl_serializer>\nclass basic_json;\n\n/*!\n@brief JSON Pointer\n\nA JSON pointer defines a string syntax for identifying a specific value\nwithin a JSON document. It can be used with functions `at` and\n`operator[]`. Furthermore, JSON pointers are the base for JSON patches.\n\n@sa [RFC 6901](https://tools.ietf.org/html/rfc6901)\n\n@since version 2.0.0\n*/\ntemplate\nclass json_pointer;\n\n/*!\n@brief default JSON class\n\nThis type is the default specialization of the @ref basic_json class which\nuses the standard template types.\n\n@since version 1.0.0\n*/\nusing json = basic_json<>;\n} // namespace nlohmann\n\n#endif\n\n// #include \n\n\n// This file contains all internal macro definitions\n// You MUST include macro_unscope.hpp at the end of json.hpp to undef all of them\n\n// exclude unsupported compilers\n#if !defined(JSON_SKIP_UNSUPPORTED_COMPILER_CHECK)\n #if defined(__clang__)\n #if (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__) < 30400\n #error \"unsupported Clang version - see https://github.com/nlohmann/json#supported-compilers\"\n #endif\n #elif defined(__GNUC__) && !(defined(__ICC) || defined(__INTEL_COMPILER))\n #if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) < 40800\n #error \"unsupported GCC version - see https://github.com/nlohmann/json#supported-compilers\"\n #endif\n #endif\n#endif\n\n// disable float-equal warnings on GCC/clang\n#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)\n #pragma GCC diagnostic push\n #pragma GCC diagnostic ignored \"-Wfloat-equal\"\n#endif\n\n// disable documentation warnings on clang\n#if defined(__clang__)\n #pragma GCC diagnostic push\n #pragma GCC diagnostic ignored \"-Wdocumentation\"\n#endif\n\n// allow for portable deprecation warnings\n#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)\n #define JSON_DEPRECATED __attribute__((deprecated))\n#elif defined(_MSC_VER)\n #define JSON_DEPRECATED __declspec(deprecated)\n#else\n #define JSON_DEPRECATED\n#endif\n\n// allow to disable exceptions\n#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || defined(_CPPUNWIND)) && !defined(JSON_NOEXCEPTION)\n #define JSON_THROW(exception) throw exception\n #define JSON_TRY try\n #define JSON_CATCH(exception) catch(exception)\n #define JSON_INTERNAL_CATCH(exception) catch(exception)\n#else\n #define JSON_THROW(exception) std::abort()\n #define JSON_TRY if(true)\n #define JSON_CATCH(exception) if(false)\n #define JSON_INTERNAL_CATCH(exception) if(false)\n#endif\n\n// override exception macros\n#if defined(JSON_THROW_USER)\n #undef JSON_THROW\n #define JSON_THROW JSON_THROW_USER\n#endif\n#if defined(JSON_TRY_USER)\n #undef JSON_TRY\n #define JSON_TRY JSON_TRY_USER\n#endif\n#if defined(JSON_CATCH_USER)\n #undef JSON_CATCH\n #define JSON_CATCH JSON_CATCH_USER\n #undef JSON_INTERNAL_CATCH\n #define JSON_INTERNAL_CATCH JSON_CATCH_USER\n#endif\n#if defined(JSON_INTERNAL_CATCH_USER)\n #undef JSON_INTERNAL_CATCH\n #define JSON_INTERNAL_CATCH JSON_INTERNAL_CATCH_USER\n#endif\n\n// manual branch prediction\n#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)\n #define JSON_LIKELY(x) __builtin_expect(!!(x), 1)\n #define JSON_UNLIKELY(x) __builtin_expect(!!(x), 0)\n#else\n #define JSON_LIKELY(x) x\n #define JSON_UNLIKELY(x) x\n#endif\n\n// C++ language standard detection\n#if (defined(__cplusplus) && __cplusplus >= 201703L) || (defined(_HAS_CXX17) && _HAS_CXX17 == 1) // fix for issue #464\n #define JSON_HAS_CPP_17\n #define JSON_HAS_CPP_14\n#elif (defined(__cplusplus) && __cplusplus >= 201402L) || (defined(_HAS_CXX14) && _HAS_CXX14 == 1)\n #define JSON_HAS_CPP_14\n#endif\n\n/*!\n@brief macro to briefly define a mapping between an enum and JSON\n@def NLOHMANN_JSON_SERIALIZE_ENUM\n@since version 3.4.0\n*/\n#define NLOHMANN_JSON_SERIALIZE_ENUM(ENUM_TYPE, ...) \\\n template \\\n inline void to_json(BasicJsonType& j, const ENUM_TYPE& e) \\\n { \\\n static_assert(std::is_enum::value, #ENUM_TYPE \" must be an enum!\"); \\\n static const std::pair m[] = __VA_ARGS__; \\\n auto it = std::find_if(std::begin(m), std::end(m), \\\n [e](const std::pair& ej_pair) -> bool \\\n { \\\n return ej_pair.first == e; \\\n }); \\\n j = ((it != std::end(m)) ? it : std::begin(m))->second; \\\n } \\\n template \\\n inline void from_json(const BasicJsonType& j, ENUM_TYPE& e) \\\n { \\\n static_assert(std::is_enum::value, #ENUM_TYPE \" must be an enum!\"); \\\n static const std::pair m[] = __VA_ARGS__; \\\n auto it = std::find_if(std::begin(m), std::end(m), \\\n [j](const std::pair& ej_pair) -> bool \\\n { \\\n return ej_pair.second == j; \\\n }); \\\n e = ((it != std::end(m)) ? it : std::begin(m))->first; \\\n }\n\n// Ugly macros to avoid uglier copy-paste when specializing basic_json. They\n// may be removed in the future once the class is split.\n\n#define NLOHMANN_BASIC_JSON_TPL_DECLARATION \\\n template class ObjectType, \\\n template class ArrayType, \\\n class StringType, class BooleanType, class NumberIntegerType, \\\n class NumberUnsignedType, class NumberFloatType, \\\n template class AllocatorType, \\\n template class JSONSerializer>\n\n#define NLOHMANN_BASIC_JSON_TPL \\\n basic_json\n\n// #include \n\n\n#include // not\n#include // size_t\n#include // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type\n\nnamespace nlohmann\n{\nnamespace detail\n{\n// alias templates to reduce boilerplate\ntemplate\nusing enable_if_t = typename std::enable_if::type;\n\ntemplate\nusing uncvref_t = typename std::remove_cv::type>::type;\n\n// implementation of C++14 index_sequence and affiliates\n// source: https://stackoverflow.com/a/32223343\ntemplate\nstruct index_sequence\n{\n using type = index_sequence;\n using value_type = std::size_t;\n static constexpr std::size_t size() noexcept\n {\n return sizeof...(Ints);\n }\n};\n\ntemplate\nstruct merge_and_renumber;\n\ntemplate\nstruct merge_and_renumber, index_sequence>\n : index_sequence < I1..., (sizeof...(I1) + I2)... > {};\n\ntemplate\nstruct make_index_sequence\n : merge_and_renumber < typename make_index_sequence < N / 2 >::type,\n typename make_index_sequence < N - N / 2 >::type > {};\n\ntemplate<> struct make_index_sequence<0> : index_sequence<> {};\ntemplate<> struct make_index_sequence<1> : index_sequence<0> {};\n\ntemplate\nusing index_sequence_for = make_index_sequence;\n\n// dispatch utility (taken from ranges-v3)\ntemplate struct priority_tag : priority_tag < N - 1 > {};\ntemplate<> struct priority_tag<0> {};\n\n// taken from ranges-v3\ntemplate\nstruct static_const\n{\n static constexpr T value{};\n};\n\ntemplate\nconstexpr T static_const::value;\n} // namespace detail\n} // namespace nlohmann\n\n// #include \n\n\n#include // not\n#include // numeric_limits\n#include // false_type, is_constructible, is_integral, is_same, true_type\n#include // declval\n\n// #include \n\n// #include \n\n// #include \n\n\n#include \n\n// #include \n\n\nnamespace nlohmann\n{\nnamespace detail\n{\ntemplate struct make_void\n{\n using type = void;\n};\ntemplate using void_t = typename make_void::type;\n} // namespace detail\n} // namespace nlohmann\n\n\n// http://en.cppreference.com/w/cpp/experimental/is_detected\nnamespace nlohmann\n{\nnamespace detail\n{\nstruct nonesuch\n{\n nonesuch() = delete;\n ~nonesuch() = delete;\n nonesuch(nonesuch const&) = delete;\n void operator=(nonesuch const&) = delete;\n};\n\ntemplate class Op,\n class... Args>\nstruct detector\n{\n using value_t = std::false_type;\n using type = Default;\n};\n\ntemplate class Op, class... Args>\nstruct detector>, Op, Args...>\n{\n using value_t = std::true_type;\n using type = Op;\n};\n\ntemplate