Repository: mpkuse/cerebro
Branch: master
Commit: 1dea0b09e12e
Files: 154
Total size: 1.9 MB
Directory structure:
gitextract_xwomkigw/
├── .gitignore
├── CMakeLists.txt
├── README.md
├── config/
│ ├── echo__terminator_config_vins_fusion_cerebro
│ ├── euroc/
│ │ └── euroc_config_no_extrinsic.yaml
│ ├── fly
│ ├── mynteye/
│ │ ├── camera_left.yaml
│ │ ├── camera_right.yaml
│ │ ├── extrinsics.yaml
│ │ └── mynteye_config.yaml
│ ├── mynteye__terminator_config_vins_fusion_cerebro
│ ├── mynteye_kannala_brandt/
│ │ ├── camera_left.yaml
│ │ ├── camera_right.yaml
│ │ ├── extrinsics.yaml
│ │ └── mynteye_config.yaml
│ ├── mynteye_pinhole/
│ │ ├── camera_left.yaml
│ │ ├── camera_right.yaml
│ │ ├── extrinsics.yaml
│ │ └── mynteye_config.yaml
│ ├── realsense__terminator_config_vins_fusion_cerebro
│ ├── vinsfusion/
│ │ ├── djirosimu_realsense_d435i/
│ │ │ ├── README.md
│ │ │ ├── extrinsics.yaml
│ │ │ ├── left.yaml
│ │ │ ├── realsense_stereo_imu_config.yaml
│ │ │ └── right.yaml
│ │ ├── euroc/
│ │ │ ├── cam0_pinhole.yaml
│ │ │ ├── cam1_pinhole.yaml
│ │ │ ├── euroc_stereo_imu_config.yaml
│ │ │ └── extrinsics.yaml
│ │ ├── mynteye/
│ │ │ ├── camera_left.yaml
│ │ │ ├── camera_left_pinhole.yaml
│ │ │ ├── camera_right.yaml
│ │ │ ├── camera_right_pinhole.yaml
│ │ │ ├── extrinsics.yaml
│ │ │ └── mynteye_stereo_imu_config.yaml
│ │ └── realsense_d435i/
│ │ ├── extrinsics.yaml
│ │ ├── left.yaml
│ │ ├── realsense_stereo_imu_config.yaml
│ │ └── right.yaml
│ └── vinsmono_debug_config.yaml
├── launch/
│ ├── debugging_vinsmono.launch
│ ├── euroc_vinsfusion.launch
│ ├── keras_server.launch
│ ├── mynteye_vinsfusion.launch
│ ├── mynteye_vinsfusion_loadstate.launch
│ ├── mynteye_vinsfusion_ondrone.launch
│ ├── mynteye_vinsfusion_savestate.launch
│ ├── realsense_camera.launch
│ ├── realsense_vinsfusion.launch
│ ├── realsense_vinsfusion_ondrone_repeat.launch
│ └── realsense_vinsfusion_ondrone_teach.launch
├── msg/
│ └── LoopEdge.msg
├── package.xml
├── rviz/
│ ├── dev-config.rviz
│ ├── good-viz.rviz
│ └── vins-fusion.rviz
├── scripts/
│ ├── TerminalColors.py
│ ├── keras.models/
│ │ ├── .gitignore
│ │ ├── Apr2019/
│ │ │ └── gray_conv6_K16__centeredinput/
│ │ │ ├── core_model.1000.keras
│ │ │ └── model.json
│ │ ├── June2019/
│ │ │ ├── centeredinput-m1to1-240x320x1__mobilenetv2-block_9_add__K16__allpairloss/
│ │ │ │ └── modelarch_and_weights.2000.h5
│ │ │ ├── centeredinput-m1to1-240x320x3__mobilenet-conv_pw_6_relu__K16__allpairloss/
│ │ │ │ └── modelarch_and_weights.700.h5
│ │ │ └── centeredinput-m1to1-240x320x3__mobilenetv2-block_9_add__K16__allpairloss/
│ │ │ └── modelarch_and_weights.800.h5
│ │ ├── README.md
│ │ ├── core_model.keras
│ │ ├── mobilenet_conv7_allpairloss.keras
│ │ ├── model.json
│ │ └── modelarch_and_weights.h5
│ ├── keras_helpers.py
│ ├── predict_utils.py
│ ├── unittest/
│ │ ├── demo_superpoint.py
│ │ └── rtry_superpoint.py
│ ├── whole_image_desc_compute_client.py
│ └── whole_image_desc_compute_server.py
├── src/
│ ├── Cerebro.cpp
│ ├── Cerebro.h
│ ├── DataManager.cpp
│ ├── DataManager.h
│ ├── DataNode.cpp
│ ├── DataNode.h
│ ├── DlsPnpWithRansac.cpp
│ ├── DlsPnpWithRansac.h
│ ├── HypothesisManager.cpp
│ ├── HypothesisManager.h
│ ├── ImageDataManager.cpp
│ ├── ImageDataManager.h
│ ├── PNPCeresCostFunctions.h
│ ├── PinholeCamera.cpp
│ ├── PinholeCamera.h
│ ├── ProcessedLoopCandidate.cpp
│ ├── ProcessedLoopCandidate.h
│ ├── Visualization.cpp
│ ├── Visualization.h
│ ├── cerebro_node.cpp
│ ├── unittest/
│ │ ├── unittest_camera_geom_class_usage.cpp
│ │ ├── unittest_camodocal_proj.cpp
│ │ ├── unittest_plot2mat.cpp
│ │ ├── unittest_pose_tester.cpp
│ │ ├── unittest_rosservice_client.cpp
│ │ ├── unittest_termcolor.cpp
│ │ ├── unittest_theia.cpp
│ │ └── unittest_theia_ransac.cpp
│ └── utils/
│ ├── CameraGeometry.cpp
│ ├── CameraGeometry.h
│ ├── ElapsedTime.h
│ ├── GMSMatcher/
│ │ ├── gms_matcher.cpp
│ │ └── gms_matcher.h
│ ├── MiscUtils.cpp
│ ├── MiscUtils.h
│ ├── Plot2Mat.cpp
│ ├── Plot2Mat.h
│ ├── PointFeatureMatching.cpp
│ ├── PointFeatureMatching.h
│ ├── PoseManipUtils.cpp
│ ├── PoseManipUtils.h
│ ├── RawFileIO.cpp
│ ├── RawFileIO.h
│ ├── RosMarkerUtils.cpp
│ ├── RosMarkerUtils.h
│ ├── SafeQueue.h
│ ├── TermColor.h
│ ├── camera_geometry_usage.cpp
│ ├── camodocal/
│ │ ├── README.md
│ │ ├── include/
│ │ │ └── camodocal/
│ │ │ ├── calib/
│ │ │ │ └── CameraCalibration.h
│ │ │ ├── camera_models/
│ │ │ │ ├── Camera.h
│ │ │ │ ├── CameraFactory.h
│ │ │ │ ├── CataCamera.h
│ │ │ │ ├── CostFunctionFactory.h
│ │ │ │ ├── EquidistantCamera.h
│ │ │ │ ├── PinholeCamera.h
│ │ │ │ └── ScaramuzzaCamera.h
│ │ │ ├── chessboard/
│ │ │ │ ├── Chessboard.h
│ │ │ │ ├── ChessboardCorner.h
│ │ │ │ ├── ChessboardQuad.h
│ │ │ │ └── Spline.h
│ │ │ ├── gpl/
│ │ │ │ ├── EigenQuaternionParameterization.h
│ │ │ │ ├── EigenUtils.h
│ │ │ │ └── gpl.h
│ │ │ └── sparse_graph/
│ │ │ └── Transform.h
│ │ └── src/
│ │ ├── calib/
│ │ │ └── CameraCalibration.cc
│ │ ├── camera_models/
│ │ │ ├── Camera.cc
│ │ │ ├── CameraFactory.cc
│ │ │ ├── CataCamera.cc
│ │ │ ├── CostFunctionFactory.cc
│ │ │ ├── EquidistantCamera.cc
│ │ │ ├── PinholeCamera.cc
│ │ │ └── ScaramuzzaCamera.cc
│ │ ├── chessboard/
│ │ │ └── Chessboard.cc
│ │ ├── gpl/
│ │ │ ├── EigenQuaternionParameterization.cc
│ │ │ └── gpl.cc
│ │ ├── intrinsic_calib.cc
│ │ └── sparse_graph/
│ │ └── Transform.cc
│ └── nlohmann/
│ └── json.hpp
└── srv/
└── WholeImageDescriptorCompute.srv
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
*.data
*.pyc
*.npy
*.npz
*.pth
*.bak
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(cerebro)
# Catkin components
find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
roscpp
rospy
std_msgs
message_generation
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
set( CMAKE_CXX_STANDARD 11 )
find_package(Threads)
find_package(Eigen3 REQUIRED)
find_package(Ceres REQUIRED)
find_package(OpenCV 3 REQUIRED)
#set( CMAKE_CXX_FLAGS "-fpermissive -std=c++11 -O3 -Wall -Wextra -pedantic" )
# Removing the dependence on theia for the production code.
# If you want to compile unit tests, some of those need theia-sfm as dependence.
find_package(Theia REQUIRED)
include_directories(${THEIA_INCLUDE_DIRS})
find_package(Boost REQUIRED COMPONENTS filesystem program_options system)
include_directories(${Boost_INCLUDE_DIRS})
#----------START gperftools
# install : sudo apt-get install google-perftools libgoogle-perftools-dev
# get cmake file: https://raw.githubusercontent.com/vast-io/vast/master/cmake/FindGperftools.cmake
# save to '/usr/local/lib/cmake'
#list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake")
#find_package(Gperftools REQUIRED)
#message( "----- GPERF")
#message( "----- "${GPERFTOOLS_LIBRARIES} )
set (CMAKE_CXX_FLAGS " -Wl,-no-as-needed")
#set(CMAKE_BUILD_TYPE Debug)
set(CMAKE_BUILD_TYPE Release)
#set(CMAKE_BUILD_TYPE RelWithDebInfo)
#----------END GPERFTOOLS
option(COMPILE_UNIT_TEST "Compile unit tests" OFF) #OFF by default. catkin_make -DCOMPILE_UNIT_TEST=ON will compile it. OFF will skip it
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
LoopEdge.msg
)
## Generate services in the 'srv' folder
add_service_files(
FILES
WholeImageDescriptorCompute.srv
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
sensor_msgs
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES cerebro
# CATKIN_DEPENDS cv_bridge image_transport roscpp rospy std_msgs
# DEPENDS system_lib
CATKIN_DEPENDS message_runtime
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
include_directories(
src/utils/camodocal/include
src/utils/
)
FILE(GLOB CamodocalCameraModelSources
src/utils/camodocal/src/chessboard/Chessboard.cc
src/utils/camodocal/src/calib/CameraCalibration.cc
src/utils/camodocal/src/camera_models/Camera.cc
src/utils/camodocal/src/camera_models/CameraFactory.cc
src/utils/camodocal/src/camera_models/CostFunctionFactory.cc
src/utils/camodocal/src/camera_models/PinholeCamera.cc
src/utils/camodocal/src/camera_models/CataCamera.cc
src/utils/camodocal/src/camera_models/EquidistantCamera.cc
src/utils/camodocal/src/camera_models/ScaramuzzaCamera.cc
src/utils/camodocal/src/sparse_graph/Transform.cc
src/utils/camodocal/src/gpl/gpl.cc
src/utils/camodocal/src/gpl/EigenQuaternionParameterization.cc
)
FILE( GLOB KuseUtilsSources
src/utils/PoseManipUtils.cpp
src/utils/RosMarkerUtils.cpp
src/utils/RawFileIO.cpp
src/utils/MiscUtils.cpp
src/utils/Plot2Mat.cpp
src/utils/CameraGeometry.cpp
src/utils/PointFeatureMatching.cpp
src/utils/GMSMatcher/gms_matcher.cpp
)
## Declare a C++ executable
add_executable(
cerebro_node
src/cerebro_node.cpp
src/PinholeCamera.cpp
src/DataManager.cpp
src/DataNode.cpp
src/Cerebro.cpp
src/Visualization.cpp
src/ProcessedLoopCandidate.cpp
src/DlsPnpWithRansac.cpp
src/HypothesisManager.cpp
src/ImageDataManager.cpp
${KuseUtilsSources}
${CamodocalCameraModelSources}
)
# this is needed to indicate this this executable depends on the messages of the pkg.
add_dependencies(cerebro_node cerebro_generate_messages_cpp)
target_link_libraries(cerebro_node
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
${CERES_LIBRARIES}
${Boost_LIBRARIES}
${THEIA_LIBRARIES}
#${GPERFTOOLS_LIBRARIES}
#/usr/local/lib/libfaiss.so
)
if(COMPILE_UNIT_TEST)
#add some compilation flags
## Unit Tests
add_executable (
unittest_termcolor
src/unittest/unittest_termcolor.cpp
)
add_executable (
unittest_rosservice_client
src/unittest/unittest_rosservice_client.cpp
)
add_executable (
unittest_camodocal_proj
src/unittest/unittest_camodocal_proj.cpp
${KuseUtilsSources}
${CamodocalCameraModelSources}
)
add_executable (
unittest_camera_geom_class_usage
src/unittest/unittest_camera_geom_class_usage.cpp
${KuseUtilsSources}
${CamodocalCameraModelSources}
)
add_executable (
unittest_theia
src/unittest/unittest_theia.cpp
${KuseUtilsSources}
${CamodocalCameraModelSources}
)
add_executable (
unittest_pose_tester
src/unittest/unittest_pose_tester.cpp
${KuseUtilsSources}
${CamodocalCameraModelSources}
src/DlsPnpWithRansac.cpp
)
add_executable (
unittest_plot2mat
src/unittest/unittest_plot2mat.cpp
src/utils/Plot2Mat.cpp
)
target_link_libraries(unittest_rosservice_client
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
${CERES_LIBRARIES}
)
target_link_libraries(unittest_camodocal_proj
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
${CERES_LIBRARIES}
)
target_link_libraries(unittest_camera_geom_class_usage
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
${CERES_LIBRARIES}
)
target_link_libraries(unittest_theia
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
${CERES_LIBRARIES}
${THEIA_LIBRARIES}
)
target_link_libraries(unittest_pose_tester
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
${CERES_LIBRARIES}
${THEIA_LIBRARIES}
)
target_link_libraries(unittest_plot2mat
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)
else()
#add some other compilation flags
endif(COMPILE_UNIT_TEST)
unset(COMPILE_UNIT_TEST CACHE) # <---- this is the important!!
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS cerebro cerebro_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_cerebro.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
================================================
FILE: README.md
================================================
# VINS-Fusion with Cerebro
This is the cerebro module for VINS-Fusion. The aim of this project
is better loop detection and recover from kidnap. The cerebro node connects to
the vins_estimator nodes of VINS-Fusion
(with ros interface). The DataManager class handles
all the incoming data. Visualization handles all the visualization.
Cerebro class handles the loop closure intelligence part. It publishes a LoopMsg
which contains timestamps of the identified loopcandidate along with the
computed relative pose between the pair. The pose computation needs a stereo pair
for reliable pose computation.
This is a multi-threaded object oriented implementation and I observe a CPU load factor
of about 2.0. A separate node handles pose graph solver (it is in [github-repo](https://github.com/mpkuse/solve_keyframe_pose_graph) ).
This repo also support for saving the trajectories to file and later
loading it from file for relocalization. See [launch/realsense_vinsfusion_ondrone_teach.launch](launch/realsense_vinsfusion_ondrone_teach.launch) on how to do it.
If you use this work in your research, please cite:
Manohar 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
## Highlight Video
[](http://www.youtube.com/watch?v=lDzDHZkInos "Video Title")
## MyntEye Demo (Using VINS-Fusion as Odometry Estimator)
We showcase the kidnap recovery and relocalization.
[](http://www.youtube.com/watch?v=3YQF4_v7AEg "Video Title")
[](http://www.youtube.com/watch?v=sTd_rZdW4DQ "Video Title")
## In Plane Rotation Test (Uses Realsense Camera)
[](http://www.youtube.com/watch?v=8bsRCNF2rnA "Video Title")
## Relocalization from Previously Constructed Trajectories.
We show the accurary of the system by plotting a previously constructed surfel map (in gray color)
and 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.
[](http://www.youtube.com/watch?v=OViEEB3rINo "Video Title")
## MyntEye Demo (Using VINS-Mono as Odometry Estimator)
[](http://www.youtube.com/watch?v=KDRo9LpL6Hs "Video Title")
[](http://www.youtube.com/watch?v=XvoCrLFq99I "Video Title")
## EuRoC MAV Dataset live merge MH-01, ... MH-05.
[](http://www.youtube.com/watch?v=mnnoAlAIsN8 "Video Title")
## EuRoC MAV Dataset live merge V1_01, V1_02, V1_03, V2_01, V2_02
[](http://www.youtube.com/watch?v=rIaANkd74cQ "Video Title")
For more demonstration, have a look at my [youtube playlist](https://www.youtube.com/playlist?list=PLWyydx20vdPzs5VVhZu0TGsReT7U17Fxp)
## Visual-Inertial Datasets
- [ETHZ EuroC Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets)
- [TUM Visual Inertial Dataset](https://vision.in.tum.de/data/datasets/visual-inertial-dataset)
- UPenn - [Penncosyvio](https://github.com/daniilidis-group/penncosyvio)
- [ADVIO](https://github.com/AaltoVision/ADVIO) ARKIT, Tango logging.
- 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)
## How to run - Docker
I highly recommend the already deployed packages with docker.
Run the roscore on your host pc and all the packages run inside
of docker container. rviz runs on the host pc.
I assume you have a PC with a graphics card and cuda9 working smoothly
and nvidia-docker installed.
```
$(host) export ROS_HOSTNAME=`hostname`
$(host) roscore
# assume that host has the ip address 172.17.0.1 in docker-network aka docker0
$(host) docker run --runtime=nvidia -it --rm \
--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-tf-faiss bash
$(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.
$(docker) roslaunch cerebro mynteye_vinsfusion.launch
OR
$(docker) roslaunch cerebro euroc_vinsfusion.launch
$(host) rosbag play 1.bag
```
If 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/)
on using docker for computer vision researchers.
You 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).
List of all my docker-images can be found [here](https://hub.docker.com/r/mpkuse/kusevisionkit).
Other docker images that work:
- mpkuse/kusevisionkit:ros-kinetic-vins-tf-faiss (preferred)
- mpkuse/kusevisionkit:vins-kidnap
## How to compile (from scratch)
You will need a) VINS-Fusion (with modification for reset by mpkuse), b) cerebro
and c) solve_keyframe_pose_graph. Besure to setup a `catkin_ws` and make sure
your ROS works correctly.
### Dependencies
- ROS Kinetic
- Eigen3
- Ceres
- OpenCV3 (should also work with 2.4 (not tested), 3.3 (tested Ok) and 3.4 (tested ok))
- [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. *
- [OpenImageIO](https://github.com/OpenImageIO/oiio) (Release-1.7.6RC1)
- [RocksDB](https://github.com/facebook/rocksdb) (v5.9.2)
- OpenExr
- Tensorflow (tested on 1.11.0)
- Keras (atleast 2.2.4), I noticed issues in jsonmodel with 2.2.2, 2.2.2 still works though!
### Get VINS-Fusion Working
I recommend you use my fork of VINS-Fusion, in which I have fixed some bugs
and added mechanism for reseting the VINS.
```
cd catkin_ws/src
#git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
git clone https://github.com/mpkuse/VINS-Fusion
cd ../
catkin_make
source catkin_ws/devel/setup.bash
```
Make sure your vins-fusion can compile and run correctly. See vins-fusion github repo
for the latest information on prerequisites and compilation instructions.
For compatibility I recommend using my fork of vins-mono/vins-fusion. Some minor
modifications have been made by me for working with kidnap cases.
### Cerebro
```
cd catkin_ws/src/
git clone https://github.com/mpkuse/cerebro
cd ../
catkin_make
```
This has 2 exectables. **a)** ros server that takes as input an image and
returns a image descriptor. **b)** cerebro_node, this
finds the loop candidates and computes the relative poses. I have also
included my trained model (about 4 MB) in this package (located scripts/keras.model). The pose computation
uses the stereo pair in this node. This node publishes the loopcandidate's relative pose
which is expected to be consumed the pose-graph solver.
If you wish to train your own model, you may use [my learning code here](https://github.com/mpkuse/cartwheel_train).
**Threads in cerebro_node:**
- *Main Thread* : ros-callbacks
- *data_association_th* : Sync the incoming image data and incoming data vins_estimator.
- *desc_th* : Consumes the images to produce whole-image-descriptors.
- *dot_product_th* : Dot product of current image descriptor with all the previous ones.
- *loopcandidate_consumer_th* : Computes the relative pose at the loopcandidates. Publishes the loopEdge.
- *kidnap_th* : Identifies kidnap. If kidnap publishes the reset signals for vins_estimator.
- *viz_th* : Publishes the image-pair, and more things for debugging and analysis.
- *dm_cleanup_th* : Deallocate/Store to file images to reduce RAM consumption.
**Nvidia TX2**: Often times for live run, you might want to run the
ros-server on a Nvidia-TX2. I recommend compiling tensorflow from scratch.
The thing is prebuilt binaries may not be compatible with the version
of CUDA and CUDNN on your device. Also some binaries may not be
compatible to arm (could likely be built for x86). Before you can
compile tensorflow you need java, bazel. See this [gist](https://gist.github.com/vellamike/7c26158c93e89ef155c1cc953bbba956). Also tools and repos from [jetsonhacks](https://github.com/jetsonhacks)
might come in handy.
### Pose Graph Solver
Use my pose graph solver, [github-repo](https://github.com/mpkuse/solve_keyframe_pose_graph).
The differences between this implementation and the
original from VINS-Fusion is that mine can handle kidnap cases,
handles multiple world co-ordinate frames and it
uses a switch-constraint formulation of the pose-graph problem.
```
cd catkin_ws/src/
git clone https://github.com/mpkuse/solve_keyframe_pose_graph
cd ../
catkin_make
```
**Threads:**
- *Main thread* : ros-callbacks for odometry poses (from vins_estimator) and LoopMsg (from cerebro).
- *SLAM* : Monitors the node-poses and loop-edges, on new loop-edges constructs and solves the pose-graph optimization problem.
- *th4* : Publish latest odometry.
- *th5* : Display image to visualize disjoint set datastructure.
- *th6* : Publish corrected poses, Different color for nodes in different co-ordinate systems.
### vins_mono_debug_pkg (optional, needed only if you wish to debug vins-mono/vins-fusion)
With cerebro node it is possible to live run the vins and make it log all the
details to file for further analysis/debugging. This might be useful
for researchers and other Ph.D. student to help VINS-Fusion improve further.
see [github/mpkuse/vins_mono](https://github.com/mpkuse/vins_mono_debug_pkg).
It basically contains unit tests and some standalone tools which might come in handy.
If you are looking to help improve VINS-fusion or cerebro also look at 'Development Guidelines'.
## How to run the Full System
```
roslaunch cerebro mynteye_vinsfusion.launch
```
You can get some of my bag files collected with the mynteye camera HERE. More
example launch files in folder `launch`, all the config files which contains
calibration info are in folder `config`.
## Development Guidelines
If you are developing I still recommend using docker. with -v flags in docker you could mount your
pc's folders on the docker. I recommend keeping all the packages in folder `docker_ws_slam/catkin_ws/src`
on your host pc. And all the rosbags in folder `/media/mpkuse/Bulk_Data`. And then mount these
two folders on the docker-container. Edit the following command as needed.
```
docker 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
```
Use the following if you wish to use xhost(gui) from inside docker
```
$(host) xhost +local:root
$(host)
docker 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
```
Other docker images that work:
- mpkuse/kusevisionkit:ros-kinetic-vins-tf-faiss (preferred)
- mpkuse/kusevisionkit:vins-kidnap
Each of my classes can export the data they hold as json objects and image files. Look at the
end of `main()` in `cerebro_node.cpp` and modify as needed to extract more debug data. Similarly
the pose graph solver can also be debugged.
If writing a lot of debug data (usually taking more than 10sec) roslaunch force-kills the process.
To get around this issue, you need to increase the timeout:
```
For kinetic this can be found in:
/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch
In the file nodeprocess.py there will be 2 variables on line 57 and 58.
_TIMEOUT_SIGINT = 15.0 #seconds
_TIMEOUT_SIGTERM = 2.0 #seconds
```
For streamlining printing messages
I have preprocessor macros at the start of function implementation (of classes DataManager and Cerebro),
read the comments there and edit as per need. Try to implement your algorithms in
object oriented way and using the producer-consumer paradigm. Look at my thread-mains for example.
Finally, sensible PR with bug fixes, enhancements are welcome!
## Authors
Manohar Kuse
================================================
FILE: config/echo__terminator_config_vins_fusion_cerebro
================================================
[global_config]
[keybindings]
[layouts]
[[default]]
[[[child0]]]
fullscreen = False
last_active_term = 1cc3df64-f997-4766-9317-0ce75d5dc2cd
last_active_window = True
maximised = True
order = 0
parent = ""
position = 65:24
size = 1855, 1056
title = dji@MANIFOLD-2-C: ~
type = Window
[[[child1]]]
order = 0
parent = child0
position = 525
ratio = 0.5
type = VPaned
[[[child2]]]
order = 0
parent = child1
position = 513
ratio = 0.278167115903
type = HPaned
[[[child4]]]
order = 1
parent = child2
position = 648
ratio = 0.487275449102
type = HPaned
[[[child7]]]
order = 1
parent = child1
position = 513
ratio = 0.278167115903
type = HPaned
[[[child9]]]
order = 1
parent = child7
position = 644
ratio = 0.484281437126
type = HPaned
[[[terminal10]]]
command = echo "roslaunch cerebro realsense_vinsfusion_ondrone.launch"; echo "roslaunch cerebro mynteye_vinsfusion_ondrone.launch"; bash
order = 0
parent = child9
profile = default
type = Terminal
uuid = 8fc7a21c-3feb-468d-aae5-c363bc5c9f80
[[[terminal11]]]
command = echo "roslaunch cerebro realsense_camera.launch"; echo "roslaunch mynt_eye_ros_wrapper mynteye.launch"; echo "rosbag play "; bash
order = 1
parent = child9
profile = default
type = Terminal
uuid = a63545ab-f3c1-4426-a8ef-173f57ca8f28
[[[terminal3]]]
command = echo "roscore"; bash
order = 0
parent = child2
profile = default
type = Terminal
uuid = 27ae967a-b56c-4d92-a56e-67a2abe6b22d
[[[terminal5]]]
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'''
order = 0
parent = child4
profile = default
type = Terminal
uuid = 6184900a-1468-4ffa-96a0-b1c00563ead7
[[[terminal6]]]
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'''
order = 1
parent = child4
profile = default
type = Terminal
uuid = 09e32ec9-5e11-4bb0-980d-4c1abeb77791
[[[terminal8]]]
command = echo "rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz"; bash
order = 0
parent = child7
profile = default
type = Terminal
uuid = 1cc3df64-f997-4766-9317-0ce75d5dc2cd
[[vins-cerebro]]
[[[child0]]]
fullscreen = False
last_active_term = b015529f-251d-4563-81f8-86f2d24badbe
last_active_window = True
maximised = False
order = 0
parent = ""
position = 289:106
size = 642, 378
title = dji@MANIFOLD-2-C: ~
type = Window
[[[terminal1]]]
order = 0
parent = child0
profile = vins-cerebro
type = Terminal
uuid = b015529f-251d-4563-81f8-86f2d24badbe
[plugins]
[profiles]
[[default]]
background_image = None
scrollback_lines = 5000
[[vins-cerebro]]
background_image = None
================================================
FILE: config/euroc/euroc_config_no_extrinsic.yaml
================================================
%YAML:1.0
#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
output_path: "/home/tony-ws1/output/"
#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: -2.917e-01
k2: 8.228e-02
p1: 5.333e-05
p2: -1.578e-04
projection_parameters:
fx: 4.616e+02
fy: 4.603e+02
cx: 3.630e+02
cy: 2.481e+02
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 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.
#If you choose 0 or 1, you should write down the following matrix.
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05
acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#loop closure parameters
loop_closure: 1 # start loop closure
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0 # useful in real-time and large project
pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#rolling shutter parameters
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
#visualization parameters
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4 # size of camera marker in RVIZ
================================================
FILE: config/fly
================================================
[global_config]
[keybindings]
[layouts]
[[default]]
[[[child0]]]
fullscreen = False
last_active_term = 1cc3df64-f997-4766-9317-0ce75d5dc2cd
last_active_window = True
maximised = True
order = 0
parent = ""
position = 65:24
size = 1855, 1056
title = dji@MANIFOLD-2-C: ~
type = Window
[[[child1]]]
order = 0
parent = child0
position = 525
ratio = 0.5
type = VPaned
[[[child2]]]
order = 0
parent = child1
position = 513
ratio = 0.278167115903
type = HPaned
[[[child4]]]
order = 1
parent = child2
position = 648
ratio = 0.487275449102
type = HPaned
[[[child7]]]
order = 1
parent = child1
position = 513
ratio = 0.278167115903
type = HPaned
[[[child9]]]
order = 1
parent = child7
position = 644
ratio = 0.484281437126
type = HPaned
[[[terminal10]]]
command = echo "roslaunch cerebro realsense_vinsfusion.launch"; sleep 15s; roslaunch --wait cerebro realsense_vinsfusion.launch; bash
order = 0
parent = child9
profile = default
type = Terminal
uuid = 8fc7a21c-3feb-468d-aae5-c363bc5c9f80
[[[terminal11]]]
command = echo "roslaunch cerebro realsense_camera.launch"; sleep 5s; roslaunch --wait cerebro realsense_camera.launch; bash
order = 1
parent = child9
profile = default
type = Terminal
uuid = a63545ab-f3c1-4426-a8ef-173f57ca8f28
[[[terminal3]]]
command = echo "roscore"; roscore; bash
order = 0
parent = child2
profile = default
type = Terminal
uuid = 27ae967a-b56c-4d92-a56e-67a2abe6b22d
[[[terminal5]]]
#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'''
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 '''
order = 0
parent = child4
profile = default
type = Terminal
uuid = 6184900a-1468-4ffa-96a0-b1c00563ead7
[[[terminal6]]]
#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'''
command = " sleep 3s; echo 'rosrun trajectory_node trajectory_node' ; export ROS_MASTER_URI=http://MANIFOLD-2-C:11311/; rosrun trajectory_node trajectory_node ; bash "
order = 1
parent = child4
profile = default
type = Terminal
uuid = 09e32ec9-5e11-4bb0-980d-4c1abeb77791
[[[terminal8]]]
command = echo "roslaunch --wait machine_defined ctrl_md.launch"; roslaunch --wait machine_defined ctrl_md.launch ; bash
order = 0
parent = child7
profile = default
type = Terminal
uuid = 1cc3df64-f997-4766-9317-0ce75d5dc2cd
[[vins-cerebro]]
[[[child0]]]
fullscreen = False
last_active_term = b015529f-251d-4563-81f8-86f2d24badbe
last_active_window = True
maximised = False
order = 0
parent = ""
position = 289:106
size = 642, 378
title = dji@MANIFOLD-2-C: ~
type = Window
[[[terminal1]]]
order = 0
parent = child0
profile = vins-cerebro
type = Terminal
uuid = b015529f-251d-4563-81f8-86f2d24badbe
[plugins]
[profiles]
[[default]]
background_image = None
scrollback_lines = 5000
[[vins-cerebro]]
background_image = None
================================================
FILE: config/mynteye/camera_left.yaml
================================================
%YAML:1.0
---
model_type: MEI
camera_name: camera_left
image_width: 752
image_height: 480
mirror_parameters:
xi: 9.1080215802208964e-01
distortion_parameters:
k1: -4.1813210333143819e-01
k2: 1.7403431210939191e-01
p1: 9.9413203823357906e-04
p2: 7.3177696678596699e-04
projection_parameters:
gamma1: 7.0261362770226128e+02
gamma2: 7.0163099053315807e+02
u0: 3.7079141993914720e+02
v0: 2.3833755987148535e+02
================================================
FILE: config/mynteye/camera_right.yaml
================================================
%YAML:1.0
---
model_type: MEI
camera_name: camera_right
image_width: 752
image_height: 480
mirror_parameters:
xi: 1.0430682060403200e+00
distortion_parameters:
k1: -4.1052639675713765e-01
k2: 1.5594870262700564e-01
p1: 1.0690635619397511e-03
p2: -1.2541037785533519e-04
projection_parameters:
gamma1: 7.5079430544256968e+02
gamma2: 7.5002280082558298e+02
u0: 3.6187005259990548e+02
v0: 2.4784480598161096e+02
================================================
FILE: config/mynteye/extrinsics.yaml
================================================
%YAML:1.0
---
transform:
q_x: -1.8252509868889259e-04
q_y: -1.6291774489779708e-03
q_z: -1.2462127842978489e-03
q_w: 9.9999787970731446e-01
t_x: -1.2075905420832895e+02
t_y: 5.4110610639412482e-01
t_z: 2.4484815673909591e-01
================================================
FILE: config/mynteye/mynteye_config.yaml
================================================
%YAML:1.0
#common parameters
imu_topic: "/mynteye/imu/data_raw"
image_topic: "/mynteye/left/image_raw"
output_path: "/Bulk_Data/vins-mono-output/"
# Additional camera (stereo) for cerebro - added my mpkuse
image_topic_1: "/mynteye/right/image_raw"
camera_yaml_1: "camera_right.yaml" # this yaml file will be searched in directory where this yaml file is.
extrinsic_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.
#camera calibration
model_type: MEI
camera_name: camera_left
image_width: 752
image_height: 480
mirror_parameters:
xi: 9.1080215802208964e-01
distortion_parameters:
k1: -4.1813210333143819e-01
k2: 1.7403431210939191e-01
p1: 9.9413203823357906e-04
p2: 7.3177696678596699e-04
projection_parameters:
gamma1: 7.0261362770226128e+02
gamma2: 7.0163099053315807e+02
u0: 3.7079141993914720e+02
v0: 2.3833755987148535e+02
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 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.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 4.2739206953025244e-03, -9.9997227378122833e-01,
-6.0979726705474944e-03, 9.9998919890819160e-01,
4.2626966744552242e-03, 1.8524265207998580e-03,
-1.8263813521932205e-03, -6.1058239298286089e-03,
9.9997969141642784e-01 ]
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [ 6.1040452082857912e-03, -4.8408978130397302e-02,
2.2594515206886469e-02 ]
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 25 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.80766 # gravity magnitude
#loop closure parameters
loop_closure: 0 # start loop closure
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0 # useful in real-time and large project
pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#rolling shutter parameters
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
#visualization parameters
save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4 # size of camera marker in RVIZ
================================================
FILE: config/mynteye__terminator_config_vins_fusion_cerebro
================================================
[global_config]
[keybindings]
[layouts]
[[default]]
[[[child0]]]
fullscreen = False
last_active_term = 1cc3df64-f997-4766-9317-0ce75d5dc2cd
last_active_window = True
maximised = True
order = 0
parent = ""
position = 65:24
size = 1855, 1056
title = dji@MANIFOLD-2-C: ~
type = Window
[[[child1]]]
order = 0
parent = child0
position = 525
ratio = 0.5
type = VPaned
[[[child2]]]
order = 0
parent = child1
position = 513
ratio = 0.278167115903
type = HPaned
[[[child4]]]
order = 1
parent = child2
position = 648
ratio = 0.487275449102
type = HPaned
[[[child7]]]
order = 1
parent = child1
position = 513
ratio = 0.278167115903
type = HPaned
[[[child9]]]
order = 1
parent = child7
position = 644
ratio = 0.484281437126
type = HPaned
[[[terminal10]]]
command = '''echo "roslaunch cerebro realsense_vinsfusion_ondrone.launch";
echo "roslaunch cerebro mynteye_vinsfusion_ondrone.launch";
export KUSE_CMD="roslaunch cerebro mynteye_vinsfusion_ondrone.launch";
roslaunch --wait cerebro mynteye_vinsfusion_ondrone.launch;
bash'''
order = 0
parent = child9
profile = default
type = Terminal
uuid = 8fc7a21c-3feb-468d-aae5-c363bc5c9f80
[[[terminal11]]]
command = '''#echo "roslaunch cerebro realsense_camera.launch";
echo "roslaunch mynt_eye_ros_wrapper mynteye.launch";
echo "rosbag play ";
export KUSE_CMD="roslaunch mynt_eye_ros_wrapper mynteye.launch or rosbag play"
bash'''
order = 1
parent = child9
profile = default
type = Terminal
uuid = a63545ab-f3c1-4426-a8ef-173f57ca8f28
[[[terminal3]]]
command = echo "roscore"; exec "roscore"; bash
order = 0
parent = child2
profile = default
type = Terminal
uuid = 27ae967a-b56c-4d92-a56e-67a2abe6b22d
[[[terminal5]]]
command = '''echo "ssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks'";
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 --wait 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 --wait tx2_whole_image_desc_server py3_tf_server_mynteye.launch'";
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'";
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'
bash'''
order = 0
parent = child4
profile = default
type = Terminal
uuid = 6184900a-1468-4ffa-96a0-b1c00563ead7
[[[terminal6]]]
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'";
export 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'";
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'''
order = 1
parent = child4
profile = default
type = Terminal
uuid = 09e32ec9-5e11-4bb0-980d-4c1abeb77791
[[[terminal8]]]
command = '''echo "rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz";
rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz
bash'''
order = 0
parent = child7
profile = default
type = Terminal
uuid = 1cc3df64-f997-4766-9317-0ce75d5dc2cd
[[vins-cerebro]]
[[[child0]]]
fullscreen = False
last_active_term = b015529f-251d-4563-81f8-86f2d24badbe
last_active_window = True
maximised = False
order = 0
parent = ""
position = 289:106
size = 642, 378
title = dji@MANIFOLD-2-C: ~
type = Window
[[[terminal1]]]
order = 0
parent = child0
profile = vins-cerebro
type = Terminal
uuid = b015529f-251d-4563-81f8-86f2d24badbe
[plugins]
[profiles]
[[default]]
background_image = None
scrollback_lines = 5000
[[vins-cerebro]]
background_image = None
================================================
FILE: config/mynteye_kannala_brandt/camera_left.yaml
================================================
%YAML:1.0
---
model_type: KANNALA_BRANDT
camera_name: camera_left
image_width: 752
image_height: 480
projection_parameters:
k2: -1.9372136316513616e-02
k3: -9.8140934466631590e-03
k4: 5.2090950264238800e-03
k5: -2.5384805224148016e-03
mu: 3.6779833417894611e+02
mv: 3.6735717027889308e+02
u0: 3.7150938938370604e+02
v0: 2.3994369620747401e+02
================================================
FILE: config/mynteye_kannala_brandt/camera_right.yaml
================================================
%YAML:1.0
---
model_type: KANNALA_BRANDT
camera_name: camera_right
image_width: 752
image_height: 480
projection_parameters:
k2: -1.6877708507279571e-02
k3: -2.2125250367976849e-02
k4: 2.5629804328110812e-02
k5: -1.3352680211095494e-02
mu: 3.6746513955323167e+02
mv: 3.6703089274525593e+02
u0: 3.6200946050656484e+02
v0: 2.4919721516867821e+02
================================================
FILE: config/mynteye_kannala_brandt/extrinsics.yaml
================================================
%YAML:1.0
---
transform:
q_x: -7.0955103716253032e-04
q_y: -1.5775578725333590e-03
q_z: -1.2732644461763854e-03
q_w: 9.9999769332040711e-01
t_x: -1.2006984141573309e+02
t_y: 3.3956264524978619e-01
t_z: -1.6784055634087214e-01
================================================
FILE: config/mynteye_kannala_brandt/mynteye_config.yaml
================================================
%YAML:1.0
#common parameters
imu_topic: "/mynteye/imu/data_raw"
image_topic: "/mynteye/left/image_raw"
output_path: "/Bulk_Data/vins-mono-output/"
# Additional camera (stereo) for cerebro - added my mpkuse
image_topic_1: "/mynteye/right/image_raw"
camera_yaml_1: "camera_right.yaml" # this yaml file will be searched in directory where this yaml file is.
extrinsic_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.
# publish image pair on loopcandidate detection
#camera calibration
model_type: KANNALA_BRANDT
camera_name: camera_left
image_width: 752
image_height: 480
projection_parameters:
k2: -1.9372136316513616e-02
k3: -9.8140934466631590e-03
k4: 5.2090950264238800e-03
k5: -2.5384805224148016e-03
mu: 3.6779833417894611e+02
mv: 3.6735717027889308e+02
u0: 3.7150938938370604e+02
v0: 2.3994369620747401e+02
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 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.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 4.2739206953025244e-03, -9.9997227378122833e-01,
-6.0979726705474944e-03, 9.9998919890819160e-01,
4.2626966744552242e-03, 1.8524265207998580e-03,
-1.8263813521932205e-03, -6.1058239298286089e-03,
9.9997969141642784e-01 ]
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [ 6.1040452082857912e-03, -4.8408978130397302e-02,
2.2594515206886469e-02 ]
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 25 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.80766 # gravity magnitude
#loop closure parameters
loop_closure: 1 # start loop closure
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 1 # useful in real-time and large project
pose_graph_save_path: "/Bulk_Data/vins-mono-output/pose_graph/" # save and load path
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#rolling shutter parameters
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
#visualization parameters
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4 # size of camera marker in RVIZ
================================================
FILE: config/mynteye_pinhole/camera_left.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera_left
image_width: 752
image_height: 480
distortion_parameters:
k1: -3.0304011877067349e-01
k2: 7.8611376314970768e-02
p1: 1.3200427858719645e-03
p2: -1.2942361409192899e-03
projection_parameters:
fx: 3.7560167589977470e+02
fy: 3.7552589588133230e+02
cx: 3.7887177487131697e+02
cy: 2.3402525474822104e+02
================================================
FILE: config/mynteye_pinhole/camera_right.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera_right
image_width: 752
image_height: 480
distortion_parameters:
k1: -3.0790744874410708e-01
k2: 8.3037204977051693e-02
p1: 1.1347651021695062e-03
p2: -7.8671202943393290e-04
projection_parameters:
fx: 3.7573448135250288e+02
fy: 3.7536464618331070e+02
cx: 3.6634215933104940e+02
cy: 2.4325470902936044e+02
================================================
FILE: config/mynteye_pinhole/extrinsics.yaml
================================================
%YAML:1.0
---
transform:
q_x: -7.8278125678242136e-04
q_y: 2.4666921736014214e-03
q_z: -1.4477856356187120e-03
q_w: 9.9999560329032322e-01
t_x: -1.1985630585227842e+02
t_y: 1.2186198275182303e-01
t_z: -1.4431533628123798e+00
================================================
FILE: config/mynteye_pinhole/mynteye_config.yaml
================================================
%YAML:1.0
#common parameters
imu_topic: "/mynteye/imu/data_raw"
image_topic: "/mynteye/left/image_raw"
output_path: "/Bulk_Data/vins-mono-output/"
# Additional camera (stereo) for cerebro - added my mpkuse
image_topic_1: "/mynteye/right/image_raw"
camera_yaml_1: "camera_right.yaml" # this yaml file will be searched in directory where this yaml file is.
extrinsic_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.
#camera calibration
model_type: PINHOLE
camera_name: camera_left
image_width: 752
image_height: 480
distortion_parameters:
k1: -3.0304011877067349e-01
k2: 7.8611376314970768e-02
p1: 1.3200427858719645e-03
p2: -1.2942361409192899e-03
projection_parameters:
fx: 3.7560167589977470e+02
fy: 3.7552589588133230e+02
cx: 3.7887177487131697e+02
cy: 2.3402525474822104e+02
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 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.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 4.2739206953025244e-03, -9.9997227378122833e-01,
-6.0979726705474944e-03, 9.9998919890819160e-01,
4.2626966744552242e-03, 1.8524265207998580e-03,
-1.8263813521932205e-03, -6.1058239298286089e-03,
9.9997969141642784e-01 ]
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [ 6.1040452082857912e-03, -4.8408978130397302e-02,
2.2594515206886469e-02 ]
#feature traker paprameters
max_cnt: 130 # max feature number in feature tracking
min_dist: 25 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.80766 # gravity magnitude
#loop closure parameters
loop_closure: 0 # start loop closure
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0 # useful in real-time and large project
pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#rolling shutter parameters
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
#visualization parameters
save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4 # size of camera marker in RVIZ
================================================
FILE: config/realsense__terminator_config_vins_fusion_cerebro
================================================
[global_config]
[keybindings]
[layouts]
[[default]]
[[[child0]]]
fullscreen = False
last_active_term = 1cc3df64-f997-4766-9317-0ce75d5dc2cd
last_active_window = True
maximised = True
order = 0
parent = ""
position = 65:24
size = 1855, 1056
title = dji@MANIFOLD-2-C: ~
type = Window
[[[child1]]]
order = 0
parent = child0
position = 525
ratio = 0.5
type = VPaned
[[[child2]]]
order = 0
parent = child1
position = 513
ratio = 0.278167115903
type = HPaned
[[[child4]]]
order = 1
parent = child2
position = 648
ratio = 0.487275449102
type = HPaned
[[[child7]]]
order = 1
parent = child1
position = 513
ratio = 0.278167115903
type = HPaned
[[[child9]]]
order = 1
parent = child7
position = 644
ratio = 0.484281437126
type = HPaned
[[[terminal10]]]
command = '''echo "roslaunch cerebro realsense_vinsfusion_ondrone.launch";
roslaunch --wait cerebro realsense_vinsfusion_ondrone.launch
#echo "roslaunch cerebro mynteye_vinsfusion_ondrone.launch";
#roslaunch --wait cerebro mynteye_vinsfusion_ondrone.launch;
bash'''
order = 0
parent = child9
profile = default
type = Terminal
uuid = 8fc7a21c-3feb-468d-aae5-c363bc5c9f80
[[[terminal11]]]
command = '''echo "roslaunch cerebro realsense_camera.launch";
#echo "roslaunch mynt_eye_ros_wrapper mynteye.launch";
echo "rosbag play "; bash'''
order = 1
parent = child9
profile = default
type = Terminal
uuid = a63545ab-f3c1-4426-a8ef-173f57ca8f28
[[[terminal3]]]
command = echo "roscore"; exec "roscore"; bash
order = 0
parent = child2
profile = default
type = Terminal
uuid = 27ae967a-b56c-4d92-a56e-67a2abe6b22d
[[[terminal5]]]
command = '''echo "ssh root@dji-tx2 'nvpmodel -m 0 && jetson_clocks'";
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 --wait tx2_whole_image_desc_server py3_tf_server_realsense.launch'";
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'
#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'";
#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'
bash'''
order = 0
parent = child4
profile = default
type = Terminal
uuid = 6184900a-1468-4ffa-96a0-b1c00563ead7
[[[terminal6]]]
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'";
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'''
order = 1
parent = child4
profile = default
type = Terminal
uuid = 09e32ec9-5e11-4bb0-980d-4c1abeb77791
[[[terminal8]]]
command = '''echo "rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz";
rviz -d catkin_ws/src/cerebro/rviz/vins-fusion.rviz
bash'''
order = 0
parent = child7
profile = default
type = Terminal
uuid = 1cc3df64-f997-4766-9317-0ce75d5dc2cd
[[vins-cerebro]]
[[[child0]]]
fullscreen = False
last_active_term = b015529f-251d-4563-81f8-86f2d24badbe
last_active_window = True
maximised = False
order = 0
parent = ""
position = 289:106
size = 642, 378
title = dji@MANIFOLD-2-C: ~
type = Window
[[[terminal1]]]
order = 0
parent = child0
profile = vins-cerebro
type = Terminal
uuid = b015529f-251d-4563-81f8-86f2d24badbe
[plugins]
[profiles]
[[default]]
background_image = None
scrollback_lines = 5000
[[vins-cerebro]]
background_image = None
================================================
FILE: config/vinsfusion/djirosimu_realsense_d435i/README.md
================================================
The configuration when using realsense stereo camera and djiros's imu (ie. imu data from N3)
================================================
FILE: config/vinsfusion/djirosimu_realsense_d435i/extrinsics.yaml
================================================
%YAML:1.0
---
transform:
q_x: 0.
q_y: 0.
q_z: 0.
q_w: 1.
t_x: -49.8
t_y: 0.
t_z: 0.
#4.98 cm for the realsense d435i device. This device has hardware undistort so can safely use the pinhole model
================================================
FILE: config/vinsfusion/djirosimu_realsense_d435i/left.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
projection_parameters:
fx: 385.7544860839844
fy: 385.7544860839844
cx: 323.1204833984375
cy: 236.7432098388672
================================================
FILE: config/vinsfusion/djirosimu_realsense_d435i/realsense_stereo_imu_config.yaml
================================================
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
# imu_topic: "/camera/imu"
imu_topic: "/djiros/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/dji/vinsfusion-output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
# 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.
# This is an optional argument even if using `num_of_cam:=2`. If I cannot
# find this key I will use `body_T_cam0` and `body_T_cam1` to compute cam1_T_cam0.
# But if this key exists, we will use these values as stereo baseline.
# **In this file, I assume translation re specified ****in mm**** (and not in meters).**
extrinsic_1_T_0: "extrinsics.yaml"
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -1.0406548080980293e-02, -6.8339359975994052e-04,
9.9994561688635142e-01, 5.2600599061337287e-02,
-9.9994461366801901e-01, -1.5656625000493030e-03,
-1.0407607662295648e-02, 2.5510536758700533e-02,
1.5726898469127781e-03, -9.9999854083599726e-01,
-6.6706260700977182e-04, 9.5887981235633201e-03, 0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -1.3185778504660295e-02, 1.2513252883544768e-04,
9.9991305601390978e-01, 4.2975722207919849e-02,
-9.9984703667423980e-01, 1.1490149582040754e-02,
-1.3186345829069746e-02, -5.4397185804457168e-02,
-1.1490800623434938e-02, -9.9993397822277874e-01,
-2.6393179226591457e-05, 1.8167602224138771e-02, 0., 0., 0., 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 0 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 0.00005 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
================================================
FILE: config/vinsfusion/djirosimu_realsense_d435i/right.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
projection_parameters:
fx: 385.7544860839844
fy: 385.7544860839844
cx: 323.1204833984375
cy: 236.7432098388672
================================================
FILE: config/vinsfusion/euroc/cam0_pinhole.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: -2.9545645106987750e-01
k2: 8.6623215640186171e-02
p1: 2.0132892276082517e-06
p2: 1.3924531371276508e-05
projection_parameters:
fx: 4.6115862106007575e+02
fy: 4.5975286598073296e+02
cx: 3.6265929181685937e+02
cy: 2.4852105668448124e+02
================================================
FILE: config/vinsfusion/euroc/cam1_pinhole.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: -2.9294124381930947e-01
k2: 8.4798002331543665e-02
p1: -2.9984646536002372e-04
p2: 3.0028216325237329e-04
projection_parameters:
fx: 4.6009781682258682e+02
fy: 4.5890983492218902e+02
cx: 3.7314916359808268e+02
cy: 2.5440734973672119e+02
================================================
FILE: config/vinsfusion/euroc/euroc_stereo_imu_config.yaml
================================================
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
output_path: "~/output/"
#cam0_calib: "cam0_mei.yaml"
#cam1_calib: "cam1_mei.yaml"
cam0_calib: "cam0_pinhole.yaml"
cam1_calib: "cam1_pinhole.yaml"
image_width: 752
image_height: 480
# 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.
# This is an optional argument even if using `num_of_cam:=2`. If I cannot
# find this key I will use `body_T_cam0` and `body_T_cam1` to compute cam1_T_cam0.
# But if this key exists, we will use these values as stereo baseline.
# **In this file, I assume translation re specified ****in mm**** (and not in meters).**
extrinsic_1_T_0: "extrinsics.yaml"
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 0 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation.
gyr_n: 0.01 # gyroscope measurement noise standard deviation.
acc_w: 0.001 # accelerometer bias random work noise standard deviation.
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation.
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
================================================
FILE: config/vinsfusion/euroc/extrinsics.yaml
================================================
%YAML:1.0
---
# this is computed for euric using body_T_cam0 and body_T_cam1.
transform:
q_x: -0.00704531
q_y: 0.000179855
q_z: -0.00115733
q_w: 0.999974
t_x: -110.074
t_y: 0.399122
t_z: -0.853703
================================================
FILE: config/vinsfusion/mynteye/camera_left.yaml
================================================
%YAML:1.0
---
model_type: KANNALA_BRANDT
camera_name: camera_left
image_width: 752
image_height: 480
projection_parameters:
k2: -1.9372136316513616e-02
k3: -9.8140934466631590e-03
k4: 5.2090950264238800e-03
k5: -2.5384805224148016e-03
mu: 3.6779833417894611e+02
mv: 3.6735717027889308e+02
u0: 3.7150938938370604e+02
v0: 2.3994369620747401e+02
================================================
FILE: config/vinsfusion/mynteye/camera_left_pinhole.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera_left
image_width: 752
image_height: 480
distortion_parameters:
k1: -3.0304011877067349e-01
k2: 7.8611376314970768e-02
p1: 1.3200427858719645e-03
p2: -1.2942361409192899e-03
projection_parameters:
fx: 3.7560167589977470e+02
fy: 3.7552589588133230e+02
cx: 3.7887177487131697e+02
cy: 2.3402525474822104e+02
================================================
FILE: config/vinsfusion/mynteye/camera_right.yaml
================================================
%YAML:1.0
---
model_type: KANNALA_BRANDT
camera_name: camera_right
image_width: 752
image_height: 480
projection_parameters:
k2: -1.6877708507279571e-02
k3: -2.2125250367976849e-02
k4: 2.5629804328110812e-02
k5: -1.3352680211095494e-02
mu: 3.6746513955323167e+02
mv: 3.6703089274525593e+02
u0: 3.6200946050656484e+02
v0: 2.4919721516867821e+02
================================================
FILE: config/vinsfusion/mynteye/camera_right_pinhole.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera_right
image_width: 752
image_height: 480
distortion_parameters:
k1: -3.0790744874410708e-01
k2: 8.3037204977051693e-02
p1: 1.1347651021695062e-03
p2: -7.8671202943393290e-04
projection_parameters:
fx: 3.7573448135250288e+02
fy: 3.7536464618331070e+02
cx: 3.6634215933104940e+02
cy: 2.4325470902936044e+02
================================================
FILE: config/vinsfusion/mynteye/extrinsics.yaml
================================================
%YAML:1.0
---
transform:
q_x: -7.0955103716253032e-04
q_y: -1.5775578725333590e-03
q_z: -1.2732644461763854e-03
q_w: 9.9999769332040711e-01
t_x: -1.2006984141573309e+02
t_y: 3.3956264524978619e-01
t_z: -1.6784055634087214e-01
================================================
FILE: config/vinsfusion/mynteye/mynteye_stereo_imu_config.yaml
================================================
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
imu_topic: "/mynteye/imu/data_raw"
image0_topic: "/mynteye/left/image_raw"
image1_topic: "/mynteye/right/image_raw"
output_path: "/Bulk_Data/vins-mono-output/"
#cam0_calib: "camera_left_pinhole.yaml"
#cam1_calib: "camera_right_pinhole.yaml"
cam0_calib: "camera_left.yaml"
cam1_calib: "camera_right.yaml"
image_width: 752
image_height: 480
# 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.
# This is an optional argument even if using `num_of_cam:=2`. If I cannot
# find this key I will use `body_T_cam0` and `body_T_cam1` to compute cam1_T_cam0.
# But if this key exists, we will use these values as stereo baseline.
# **In this file, I assume translation re specified ****in mm**** (and not in meters).**
extrinsic_1_T_0: "extrinsics.yaml"
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [4.2739206953025244e-03, -9.9997227378122833e-01,-6.0979726705474944e-03,6.1040452082857912e-03,
9.9998919890819160e-01, 4.2626966744552242e-03, 1.8524265207998580e-03,-4.8408978130397302e-02,
-1.8263813521932205e-03, -6.1058239298286089e-03, 9.9997969141642784e-01,2.2594515206886469e-02,
0,0,0,1]
# ===> imu_T_cam0 * inv( cam1_T_cam0 )
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 0.00174443, -0.99998733, -0.0046694 , 0.00665227,
0.99998578, 0.00172106, 0.00500326, 0.07165957,
-0.00499517, -0.00467806, 0.9999766 , 0.02216417,
0. , 0. , 0. , 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 100 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.1 # ransac threshold (pixel)
show_track: 0 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.05 # max solver itration time (ms), to guarantee real time
max_num_iterations: 12 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.80766 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
================================================
FILE: config/vinsfusion/realsense_d435i/extrinsics.yaml
================================================
%YAML:1.0
---
transform:
q_x: 0.
q_y: 0.
q_z: 0.
q_w: 1.
t_x: -49.8
t_y: 0.
t_z: 0.
#4.98 cm for the realsense d435i device. This device has hardware undistort so can safely use the pinhole model
================================================
FILE: config/vinsfusion/realsense_d435i/left.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
projection_parameters:
fx: 385.7544860839844
fy: 385.7544860839844
cx: 323.1204833984375
cy: 236.7432098388672
================================================
FILE: config/vinsfusion/realsense_d435i/realsense_stereo_imu_config.yaml
================================================
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
#imu_topic: "/camera/imu"
imu_topic: "/djiros/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/dji/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
# 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.
# This is an optional argument even if using `num_of_cam:=2`. If I cannot
# find this key I will use `body_T_cam0` and `body_T_cam1` to compute cam1_T_cam0.
# But if this key exists, we will use these values as stereo baseline.
# **In this file, I assume translation re specified ****in mm**** (and not in meters).**
extrinsic_1_T_0: "extrinsics.yaml"
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -1.0406548080980293e-02, -6.8339359975994052e-04,
9.9994561688635142e-01, 5.2600599061337287e-02,
-9.9994461366801901e-01, -1.5656625000493030e-03,
-1.0407607662295648e-02, 2.5510536758700533e-02,
1.5726898469127781e-03, -9.9999854083599726e-01,
-6.6706260700977182e-04, 9.5887981235633201e-03, 0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -1.3185778504660295e-02, 1.2513252883544768e-04,
9.9991305601390978e-01, 4.2975722207919849e-02,
-9.9984703667423980e-01, 1.1490149582040754e-02,
-1.3186345829069746e-02, -5.4397185804457168e-02,
-1.1490800623434938e-02, -9.9993397822277874e-01,
-2.6393179226591457e-05, 1.8167602224138771e-02, 0., 0., 0., 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 100 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 0 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
================================================
FILE: config/vinsfusion/realsense_d435i/right.yaml
================================================
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
projection_parameters:
fx: 385.7544860839844
fy: 385.7544860839844
cx: 323.1204833984375
cy: 236.7432098388672
================================================
FILE: config/vinsmono_debug_config.yaml
================================================
%YAML:1.0
#######
## This is config file for running vins-mono. We use this for imu_T_camera calibration.
## Set the estimate_extrinsic to 2. If vins mono works on, just copy (written in `output_path`)
#######
# imu_topic: "/camera/imu"
imu_topic: "/djiros/imu"
image_topic: "/camera/infra1/image_rect_raw"
#image_topic: "/camera/infra2/image_rect_raw"
# output_path: "/home/tony-ws1/output/"
output_path: "/home/dji/output/"
#camera calibration - left , right same for realsense
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
projection_parameters:
fx: 385.7544860839844
fy: 385.7544860839844
cx: 323.1204833984375
cy: 236.7432098388672
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 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.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ -1.0406548080980293e-02, -6.8339359975994052e-04,
9.9994561688635142e-01, -9.9994461366801901e-01,
-1.5656625000493030e-03, -1.0407607662295648e-02,
1.5726898469127781e-03, -9.9999854083599726e-01,
-6.6706260700977182e-04 ]
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [ 5.2600599061337287e-02, 2.5510536758700533e-02,
9.5887981235633201e-03 ]
#feature traker paprameters
max_cnt: 200 # max feature number in feature tracking
min_dist: 25 # min distance between two features
freq: 15 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.05 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
#loop closure parameters
loop_closure: 0 # start loop closure
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0 # useful in real-time and large project
pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.038 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#rolling shutter parameters
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
#visualization parameters
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4 # size of camera marker in RVIZ
================================================
FILE: launch/debugging_vinsmono.launch
================================================
================================================
FILE: launch/euroc_vinsfusion.launch
================================================
================================================
FILE: launch/keras_server.launch
================================================
================================================
FILE: launch/mynteye_vinsfusion.launch
================================================
================================================
FILE: launch/mynteye_vinsfusion_loadstate.launch
================================================
================================================
FILE: launch/mynteye_vinsfusion_ondrone.launch
================================================
================================================
FILE: launch/mynteye_vinsfusion_savestate.launch
================================================
================================================
FILE: launch/realsense_camera.launch
================================================
/camera/stereo_module/emitter_enabled: false
================================================
FILE: launch/realsense_vinsfusion.launch
================================================
================================================
FILE: launch/realsense_vinsfusion_ondrone_repeat.launch
================================================
================================================
FILE: launch/realsense_vinsfusion_ondrone_teach.launch
================================================
================================================
FILE: msg/LoopEdge.msg
================================================
time timestamp0
time timestamp1
geometry_msgs/Pose pose_1T0 # pose of 0 as observed from 1.
float32 weight
string description
================================================
FILE: package.xml
================================================
cerebro
0.0.1
The cerebro package. There is a multi-threaded node which stores all the incoming info from vins.
There is a service to compute image level descriptor. Geometry is also a separate service.
mpkuse
TODO
catkin
cv_bridge
image_transport
roscpp
rospy
std_msgs
message_generation
cv_bridge
image_transport
roscpp
rospy
std_msgs
message_runtime
================================================
FILE: rviz/dev-config.rviz
================================================
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Ref1
- /Ref1/Grid1
- /Feat Tracker1/Input Image1
- /Feat Tracker1/Input Image1/Status1
- /VIO1
- /VIO1/Odom In Cam-frame-of-ref1/Cam Odometry1
- /Cerebro1
- /Cerebro1/Marker1
- /Cerebro1/Marker1/Namespaces1
- /AR_demo1
- /AR_demo1/AR_image_odm1/Status1
- /Solve Pose Graph Opt1
- /Solve Pose Graph Opt1/Marker1
- /Solve Pose Graph Opt1/Marker1/Status1
- /Solve Pose Graph Opt1/Marker1/Namespaces1
- /Solve Pose Graph Opt1/Debug Node Marker1/Namespaces1
Splitter Ratio: 0.62647754
Tree Height: 697
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Tracked Features
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.5
Tree Height: 141
Visualization Manager:
Class: ""
Displays:
- Class: rviz/Group
Displays:
- Class: rviz/Axes
Enabled: true
Length: 1
Name: Axes
Radius: 0.200000003
Reference Frame:
Value: true
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 20
Reference Frame:
Value: true
Enabled: true
Name: Ref
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: false
Image Topic: /cam0/image_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: Input Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /feature_tracker/feature_img
Max Value: 1
Median window: 5
Min Value: 0
Name: Tracked Features
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Name: Feat Tracker
- Class: rviz/Group
Displays:
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: VIO Path
Offset:
X: 0
Y: 0
Z: 0
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /vins_estimator/path
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /vins_estimator/camera_pose_visual
Name: Visual
Namespaces:
{}
Queue Size: 100
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.46719348
Min Value: -0.613372922
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud
Color: 0; 255; 0
Color Transformer: FlatColor
Decay Time: 100
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: KF PointCloud
Position Transformer: XYZ
Queue Size: 100
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Points
Topic: /vins_estimator/keyframe_point
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 100
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Points
Topic: /vins_estimator/point_cloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 170; 85; 255
Enabled: false
Keep: 100
Length: 1
Name: imu_T_cam
Position Tolerance: 0.100000001
Topic: /vins_estimator/extrinsic
Value: false
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 255; 25; 0
Enabled: true
Keep: 100
Length: 1
Name: Cam Odometry
Position Tolerance: 0.100000001
Topic: /vins_estimator/camera_pose
Value: true
Enabled: false
Name: Odom In Cam-frame-of-ref
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 0; 255; 0
Enabled: true
Keep: 100
Length: 1
Name: KF Odometry
Position Tolerance: 0.100000001
Topic: /vins_estimator/keyframe_pose
Value: true
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 0; 170; 255
Enabled: true
Keep: 100
Length: 1
Name: IMU Odometry
Position Tolerance: 0.100000001
Topic: /vins_estimator/odometry
Value: true
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 255; 25; 0
Enabled: true
Keep: 100
Length: 1
Name: IMU Prop 200hz
Position Tolerance: 0.100000001
Topic: /vins_estimator/imu_propagate
Value: true
Enabled: false
Name: Odom In IMU-Frame-of-ref
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 255; 25; 0
Enabled: false
Keep: 100
Length: 1
Name: vinsmono-reloc-odom
Position Tolerance: 0.100000001
Topic: /vins_estimator/relo_relative_pose
Value: false
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: vinsmono-relocalized_path
Offset:
X: 0
Y: 0
Z: 0
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /vins_estimator/relocalization_path
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /pose_graph/match_image
Max Value: 1
Median window: 5
Min Value: 0
Name: vins-monomatch-images
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /pose_graph/pose_graph
Name: vinsmono loop viz
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: false
Name: vins-mono relocalized
Enabled: false
Name: VIO
- Class: rviz/Group
Displays:
- Class: rviz/Marker
Enabled: false
Marker Topic: /cerebro_node/viz/framedata
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /cerebro_node/viz/imagepaire
Max Value: 1
Median window: 5
Min Value: 0
Name: imagepaire
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Name: Cerebro
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: true
Image Topic: /ar_demo_node3_corrected/AR_image_corrected
Max Value: 1
Median window: 5
Min Value: 0
Name: AR_image_corrected
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /ar_demo_node3_odom/AR_image_odom
Max Value: 1
Median window: 5
Min Value: 0
Name: AR_image_odm
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /points_and_lines/AR_object
Name: AR_object
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz/InteractiveMarkers
Enable Transparency: true
Enabled: true
Name: InteractiveMarkers
Show Axes: false
Show Descriptions: true
Show Visual Aids: false
Update Topic: /interactive_marker_server/update
Value: true
Enabled: true
Name: AR_demo
- Class: rviz/Group
Displays:
- Class: rviz/Marker
Enabled: true
Marker Topic: /keyframe_pose_graph_slam_node/viz/visualization_marker
Name: Marker
Namespaces:
latest_odometry: true
world##_cam_visual: true
world#-1: true
world#-2: true
world#-3: true
world#-4: true
world#0: true
world#1: true
world#2: true
world#3: true
world#4: true
Queue Size: 100
Value: true
- Class: rviz/Marker
Enabled: false
Marker Topic: /debug_pose_graph_solver/visualization_marker
Name: Debug Node Marker
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: true
Name: Solve Pose Graph Opt
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 46.7297897
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 3.76500964
Y: 2.58213377
Z: -2.90765786
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.0647974
Target Frame:
Value: Orbit (rviz)
Yaw: 4.69420815
Saved:
- Class: rviz/Orbit
Distance: 49.6446877
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 22.8474731
Y: 20.4583664
Z: -6.84013748
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Name: Orbit
Near Clip Distance: 0.00999999978
Pitch: 1.16479671
Target Frame:
Value: Orbit (rviz)
Yaw: 5.02046442
Window Geometry:
AR_image_corrected:
collapsed: false
AR_image_odm:
collapsed: false
Displays:
collapsed: false
Height: 1003
Hide Left Dock: false
Hide Right Dock: false
Input Image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Tracked Features:
collapsed: false
Width: 1918
X: 0
Y: 24
imagepaire:
collapsed: false
vins-monomatch-images:
collapsed: false
================================================
FILE: rviz/good-viz.rviz
================================================
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Ref1
- /Ref1/Axes1
- /Feat Tracker1/Input Image1
- /Feat Tracker1/Input Image1/Status1
- /VIO1
- /VIO1/Odom In Cam-frame-of-ref1/Cam Odometry1
- /Cerebro1
- /Cerebro1/Marker1
- /Cerebro1/Marker1/Namespaces1
- /AR_demo1
- /Solve Pose Graph Opt1
- /Solve Pose Graph Opt1/Marker1
- /Solve Pose Graph Opt1/Marker1/Namespaces1
- /Solve Pose Graph Opt1/Debug Node Marker1/Namespaces1
- /Ref21
- /Ref21/Grid1/Offset1
Splitter Ratio: 0.62647754
Tree Height: 805
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Tracked Features
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.5
Tree Height: 359
Visualization Manager:
Class: ""
Displays:
- Class: rviz/Group
Displays:
- Class: rviz/Axes
Enabled: true
Length: 1
Name: Axes
Radius: 0.200000003
Reference Frame:
Value: true
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 20
Reference Frame:
Value: true
Enabled: true
Name: Ref
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: false
Image Topic: /cam0/image_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: Input Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /feature_tracker/feature_img
Max Value: 1
Median window: 5
Min Value: 0
Name: Tracked Features
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Name: Feat Tracker
- Class: rviz/Group
Displays:
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: VIO Path
Offset:
X: 0
Y: 0
Z: 0
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /vins_estimator/path
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /vins_estimator/camera_pose_visual
Name: Visual
Namespaces:
{}
Queue Size: 100
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.46719348
Min Value: -0.613372922
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud
Color: 0; 255; 0
Color Transformer: FlatColor
Decay Time: 100
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: KF PointCloud
Position Transformer: XYZ
Queue Size: 100
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Points
Topic: /vins_estimator/keyframe_point
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 100
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Points
Topic: /vins_estimator/point_cloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 170; 85; 255
Enabled: false
Keep: 100
Length: 1
Name: imu_T_cam
Position Tolerance: 0.100000001
Topic: /vins_estimator/extrinsic
Value: false
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 255; 25; 0
Enabled: true
Keep: 100
Length: 1
Name: Cam Odometry
Position Tolerance: 0.100000001
Topic: /vins_estimator/camera_pose
Value: true
Enabled: false
Name: Odom In Cam-frame-of-ref
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 0; 255; 0
Enabled: true
Keep: 100
Length: 1
Name: KF Odometry
Position Tolerance: 0.100000001
Topic: /vins_estimator/keyframe_pose
Value: true
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 0; 170; 255
Enabled: true
Keep: 100
Length: 1
Name: IMU Odometry
Position Tolerance: 0.100000001
Topic: /vins_estimator/odometry
Value: true
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 255; 25; 0
Enabled: true
Keep: 100
Length: 1
Name: IMU Prop 200hz
Position Tolerance: 0.100000001
Topic: /vins_estimator/imu_propagate
Value: true
Enabled: false
Name: Odom In IMU-Frame-of-ref
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Color: 255; 25; 0
Enabled: false
Keep: 100
Length: 1
Name: vinsmono-reloc-odom
Position Tolerance: 0.100000001
Topic: /vins_estimator/relo_relative_pose
Value: false
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: vinsmono-relocalized_path
Offset:
X: 0
Y: 0
Z: 0
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /vins_estimator/relocalization_path
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /pose_graph/match_image
Max Value: 1
Median window: 5
Min Value: 0
Name: vins-monomatch-images
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /pose_graph/pose_graph
Name: vinsmono loop viz
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: false
Name: vins-mono relocalized
Enabled: false
Name: VIO
- Class: rviz/Group
Displays:
- Class: rviz/Marker
Enabled: true
Marker Topic: /cerebro_node/viz/framedata
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /cerebro_node/viz/imagepaire
Max Value: 1
Median window: 5
Min Value: 0
Name: imagepaire
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Name: Cerebro
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: true
Image Topic: /ar_demo_node3_corrected/AR_image_corrected
Max Value: 1
Median window: 5
Min Value: 0
Name: AR_image_corrected
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /ar_demo_node3_odom/AR_image_odom
Max Value: 1
Median window: 5
Min Value: 0
Name: AR_image_odm
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /points_and_lines/AR_object
Name: AR_object
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz/InteractiveMarkers
Enable Transparency: true
Enabled: true
Name: InteractiveMarkers
Show Axes: false
Show Descriptions: true
Show Visual Aids: false
Update Topic: /interactive_marker_server/update
Value: true
Enabled: false
Name: AR_demo
- Class: rviz/Group
Displays:
- Class: rviz/Marker
Enabled: true
Marker Topic: /keyframe_pose_graph_slam_node/viz/visualization_marker
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /keyframe_pose_graph_slam_node/viz/disjoint_set_status_image
Max Value: 1
Median window: 5
Min Value: 0
Name: disjoint_set_status_image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: false
Marker Topic: /debug_pose_graph_solver/visualization_marker
Name: Debug Node Marker
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: true
Name: Solve Pose Graph Opt
- Class: rviz/Group
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 40; 164; 44
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 30
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 20
Reference Frame:
Value: true
Enabled: true
Name: Ref2
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 49.2109833
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 3.41268897
Y: -0.305300832
Z: -1.72543097
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.884797573
Target Frame:
Value: Orbit (rviz)
Yaw: 3.05431199
Saved:
- Class: rviz/Orbit
Distance: 49.6446877
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 22.8474731
Y: 20.4583664
Z: -6.84013748
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Name: Orbit
Near Clip Distance: 0.00999999978
Pitch: 1.16479671
Target Frame:
Value: Orbit (rviz)
Yaw: 5.02046442
Window Geometry:
AR_image_corrected:
collapsed: false
AR_image_odm:
collapsed: false
Displays:
collapsed: false
Height: 1003
Hide Left Dock: false
Hide Right Dock: false
Input Image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Tracked Features:
collapsed: false
Width: 1918
X: 0
Y: 24
disjoint_set_status_image:
collapsed: false
imagepaire:
collapsed: false
vins-monomatch-images:
collapsed: false
================================================
FILE: rviz/vins-fusion.rviz
================================================
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /raw_input_image1
- /Ref1
- /Ref1/Axes1
- /Ref1/Grid1
- /Feat Tracker1/Input Image1
- /AR_demo1
- /AR_demo1/AR_image_corrected1
- /AR_demo1/AR_image_odm1
- /AR_demo1/ModelViz1
- /AR_demo1/ModelViz1/Status1
- /AR_demo1/ModelViz1/Namespaces1
- /Cerebro1
- /Cerebro1/Marker1
- /Cerebro1/Marker1/Namespaces1
- /Solve Pose Graph Opt1/Marker1
- /Solve Pose Graph Opt1/Marker1/Namespaces1
- /Solve Pose Graph Opt1/Debug Node Marker1/Namespaces1
- /VINS-fusion1/MarkerArray1/Namespaces1
- /VINS-fusion1/VIO Path1
- /VINS-fusion1/VIO Path1/Offset1
- /VINS-fusion1/After Loop Fusion1
- /VINS-fusion1/Loop Fusion Edges1
- /Marker1
- /Surfel Mapping1/PointCloud21
Splitter Ratio: 0.62647754
Tree Height: 853
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: raw_input_image
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.5
Tree Height: 359
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Class: rviz/Image
Enabled: true
Image Topic: /camera/infra1/image_rect_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: raw_input_image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Group
Displays:
- Class: rviz/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.200000003
Reference Frame:
Value: false
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 20
Reference Frame:
Value: true
Enabled: true
Name: Ref
- Class: rviz/Group
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 40; 164; 44
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 30
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 20
Reference Frame:
Value: true
Enabled: true
Name: Ref2
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: true
Image Topic: /cam0/image_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: Input Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /feature_tracker/feature_img
Max Value: 1
Median window: 5
Min Value: 0
Name: Tracked Features
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: false
Name: Feat Tracker
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: true
Image Topic: /ar_demo_node3_odom/AR_image_corrected
Max Value: 1
Median window: 5
Min Value: 0
Name: AR_image_corrected
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: false
Image Topic: /ar_demo_node3_odom/AR_image_odom
Max Value: 1
Median window: 5
Min Value: 0
Name: AR_image_odm
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /points_and_lines/AR_object
Name: AR_object
Namespaces:
{}
Queue Size: 100
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: x
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 9.04478359
Min Color: 0; 0; 0
Min Intensity: 3.50075197
Name: debug ptcld
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.0199999996
Style: Flat Squares
Topic: /unit_test_estimate_ground_plane/pt_cld
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /ar_demo_node3_odom/models
Name: ModelViz
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/InteractiveMarkers
Enable Transparency: true
Enabled: true
Name: InteractiveMarkers
Show Axes: false
Show Descriptions: true
Show Visual Aids: false
Update Topic: /interactive_marker_server/update
Value: true
Enabled: true
Name: AR_demo
- Class: rviz/Group
Displays:
- Class: rviz/Marker
Enabled: false
Marker Topic: /cerebro_node/viz/framedata
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /cerebro_node/viz/imagepaire
Max Value: 1
Median window: 5
Min Value: 0
Name: cerebro - imagepaire
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Name: Cerebro
- Class: rviz/Group
Displays:
- Class: rviz/Marker
Enabled: true
Marker Topic: /keyframe_pose_graph_slam_node/viz/visualization_marker
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /keyframe_pose_graph_slam_node/viz/disjoint_set_status_image
Max Value: 1
Median window: 5
Min Value: 0
Name: disjoint_set_status_image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: false
Marker Topic: /debug_pose_graph_solver/visualization_marker
Name: Debug Node Marker
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: true
Name: Solve Pose Graph Opt
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: true
Image Topic: /loop_fusion/match_image
Max Value: 1
Median window: 5
Min Value: 0
Name: DBOW Match Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /vins_estimator/camera_pose_visual
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: VIO Path
Offset:
X: 0
Y: 0
Z: 0
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /vins_estimator/path
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: After Loop Fusion
Offset:
X: 0
Y: 0
Z: 0
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /loop_fusion/pose_graph_path
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /loop_fusion/pose_graph
Name: Loop Fusion Edges
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: false
Name: VINS-fusion
- Class: rviz/Marker
Enabled: false
Marker Topic: /keyframe_pose_graph_slam_node/hz200/visualization_marker
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: z
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0.863711178
Min Color: 0; 0; 0
Min Intensity: -2.19427705
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.0500000007
Style: Flat Squares
Topic: /surfel_fusion/pointcloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 255
Min Color: 0; 0; 0
Min Intensity: 17
Name: current
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Flat Squares
Topic: /surfel_fusion/raw_pointcloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: z
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: true
Max Color: 255; 255; 255
Max Intensity: 1.4563067
Min Color: 0; 0; 0
Min Intensity: -1.5742451
Name: filtered
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Points
Topic: /map_server/filtered_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Name: Surfel Mapping
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 88.2317963
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 14.8281975
Y: -0.740946651
Z: 0.155234724
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.29479516
Target Frame:
Value: Orbit (rviz)
Yaw: 4.72152758
Saved:
- Class: rviz/Orbit
Distance: 49.6446877
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 22.8474731
Y: 20.4583664
Z: -6.84013748
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Name: Orbit
Near Clip Distance: 0.00999999978
Pitch: 1.16479671
Target Frame:
Value: Orbit (rviz)
Yaw: 5.02046442
Window Geometry:
AR_image_corrected:
collapsed: false
AR_image_odm:
collapsed: false
DBOW Match Image:
collapsed: false
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
Input Image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Tracked Features:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24
cerebro - imagepaire:
collapsed: false
disjoint_set_status_image:
collapsed: false
raw_input_image:
collapsed: false
================================================
FILE: scripts/TerminalColors.py
================================================
class bcolors:
HEADER = '\033[95m'
OKBLUE = '\033[94m'
OKGREEN = '\033[92m'
WARNING = '\033[93m'
FAIL = '\033[91m'
ENDC = '\033[0m'
BOLD = '\033[1m'
UNDERLINE = '\033[4m'
# USage
#```
# from TerminalColors import bcolors
# tcol = bcolors()
# print tcol.OKGREEN, "hello in green color", tcol.ENDC
#```
================================================
FILE: scripts/keras.models/.gitignore
================================================
================================================
FILE: scripts/keras.models/Apr2019/gray_conv6_K16__centeredinput/model.json
================================================
"{\"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\"}"
================================================
FILE: scripts/keras.models/README.md
================================================
# Pretrained Models for Place Recognition.
This folder contains tested models for place recognition. The models have been tested for real-time
performance.
the 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.
================================================
FILE: scripts/keras.models/model.json
================================================
"{\"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\"}"
================================================
FILE: scripts/keras_helpers.py
================================================
from keras import backend as K
from keras.engine.topology import Layer
import keras
import code
import numpy as np
import cv2
import code
#---------------------------------------------------------------------------------
# My Layers
# NetVLADLayer
#---------------------------------------------------------------------------------
# # Writing your own custom layers
# class MyLayer(Layer):
#
# def __init__(self, output_dim, **kwargs):
# self.output_dim = output_dim
# super(MyLayer, self).__init__(**kwargs)
#
# def build(self, input_shape):
# # Create a trainable weight variable for this layer.
# self.kernel = self.add_weight(name='kernel',
# shape=(input_shape[1], self.output_dim),
# initializer='uniform',
# trainable=True)
# super(MyLayer, self).build(input_shape) # Be sure to call this at the end
#
# def call(self, x):
# return [K.dot(x, self.kernel), K.dot(x, self.kernel)]
#
# def compute_output_shape(self, input_shape):
# return [(input_shape[0], self.output_dim), (input_shape[0], self.output_dim)]
#
#
# class NetVLADLayer( Layer ):
#
# def __init__( self, num_clusters, **kwargs ):
# self.num_clusters = num_clusters
# super(NetVLADLayer, self).__init__(**kwargs)
#
# def build( self, input_shape ):
# self.K = self.num_clusters
# self.D = input_shape[-1]
#
# self.kernel = self.add_weight( name='kernel',
# shape=(1,1,self.D,self.K),
# initializer='uniform',
# trainable=True )
#
# self.bias = self.add_weight( name='bias',
# shape=(1,1,self.K),
# initializer='uniform',
# trainable=True )
#
# self.C = self.add_weight( name='cluster_centers',
# shape=[1,1,1,self.D,self.K],
# initializer='uniform',
# trainable=True)
#
# def call( self, x ):
# # soft-assignment.
# s = K.conv2d( x, self.kernel, padding='same' ) + self.bias
# a = K.softmax( s )
# self.amap = K.argmax( a, -1 )
# print 'amap.shape', self.amap.shape
#
# # Dims used hereafter: batch, H, W, desc_coeff, cluster
# a = K.expand_dims( a, -2 )
# # print 'a.shape=',a.shape
#
# # Core
# v = K.expand_dims(x, -1) + self.C
# # print 'v.shape', v.shape
# v = a * v
# # print 'v.shape', v.shape
# v = K.sum(v, axis=[1, 2])
# # print 'v.shape', v.shape
# v = K.permute_dimensions(v, pattern=[0, 2, 1])
# # print 'v.shape', v.shape
# #v.shape = None x K x D
#
# # Normalize v (Intra Normalization)
# v = K.l2_normalize( v, axis=-1 )
# v = K.batch_flatten( v )
# v = K.l2_normalize( v, axis=-1 )
#
# return [v, self.amap]
#
# def compute_output_shape( self, input_shape ):
# # return (input_shape[0], self.v.shape[-1].value )
# # return [(input_shape[0], self.K*self.D ), (input_shape[0], self.amap.shape[1].value, self.amap.shape[2].value) ]
# return [(input_shape[0], self.K*self.D ), (input_shape[0], input_shape[1], input_shape[2]) ]
#--------------------------------------------------------------------------------
# Base CNNs
#--------------------------------------------------------------------------------
# def make_vgg( input_img ):
# r_l2=keras.regularizers.l2(0.01)
# r_l1=keras.regularizers.l1(0.01)
#
# x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( input_img )
# x_64 = keras.layers.normalization.BatchNormalization()( x_64 )
# x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )
# x_64 = keras.layers.normalization.BatchNormalization()( x_64 )
# x_64 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_64 )
#
#
# x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )
# x_128 = keras.layers.normalization.BatchNormalization()( x_128 )
# x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )
# x_128 = keras.layers.normalization.BatchNormalization()( x_128 )
# x_128 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_128 )
#
#
# # x_256 = keras.layers.Conv2D( 256, (3,3), padding='same', activation='relu' )( x_128 )
# # x_256 = keras.layers.normalization.BatchNormalization()( x_256 )
# # x_256 = keras.layers.Conv2D( 256, (3,3), padding='same', activation='relu' )( x_256 )
# # x_256 = keras.layers.normalization.BatchNormalization()( x_256 )
# # x_256 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_256 )
#
# #
# # x_512 = keras.layers.Conv2D( 512, (3,3), padding='same', activation='relu' )( x_256 )
# # # BN
# # x_512 = keras.layers.Conv2D( 512, (3,3), padding='same', activation='relu' )( x_512 )
# # # BN
# # x_512 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_512 )
#
#
# x = keras.layers.Conv2DTranspose( 32, (5,5), strides=4, padding='same' )( x_128 )
# # x = x_128
#
# return x
#
#
# def make_upsampling_vgg( input_img ):
# r_l2=keras.regularizers.l2(0.01)
# r_l1=keras.regularizers.l1(0.01)
#
# x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( input_img )
# x_64 = keras.layers.normalization.BatchNormalization()( x_64 )
# x_64 = keras.layers.Conv2D( 64, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )
# x_64 = keras.layers.normalization.BatchNormalization()( x_64 )
#
# x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )
# x_128 = keras.layers.normalization.BatchNormalization()( x_128 )
# x_128 = keras.layers.Conv2D( 128, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )
# x_128 = keras.layers.normalization.BatchNormalization()( x_128 )
#
# x_256 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )
# x_256 = keras.layers.normalization.BatchNormalization()( x_256 )
# x_256 = keras.layers.Conv2D( 128, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_256 )
# x_256 = keras.layers.normalization.BatchNormalization()( x_256 )
#
# z = keras.layers.Conv2DTranspose( 32, (11,11), strides=8, padding='same' )( x_256 )
# x = keras.layers.Conv2DTranspose( 32, (9,9), strides=4, padding='same' )( x_128 )
# y = keras.layers.Conv2DTranspose( 32, (7,7), strides=2, padding='same' )( x_64 )
#
# out = keras.layers.Add()( [x,y,z] )
# return out
#
# def make_from_vgg19( input_img, trainable=True, weights=None ):
# base_model = keras.applications.vgg19.VGG19(weights=weights, include_top=False, input_tensor=input_img)
#
# for l in base_model.layers:
# l.trainable = trainable
#
# base_model_out = base_model.get_layer('block2_pool').output
#
# z = keras.layers.Conv2DTranspose( 32, (9,9), strides=4, padding='same' )( base_model_out )
# return z
#
# def make_from_vgg19_multiconvup( input_img, trainable=True, weights=None ):
# base_model = keras.applications.vgg19.VGG19(weights=weights, include_top=False, input_tensor=input_img)
#
# for l in base_model.layers:
# l.trainable = trainable
# #TODO : add kernel regularizers and activity_regularizer to conv layers
#
# base_model_out = base_model.get_layer('block2_pool').output
#
# up_conv_out = keras.layers.Conv2DTranspose( 32, (9,9), strides=2, padding='same', activation='relu' )( base_model_out )
# up_conv_out = keras.layers.normalization.BatchNormalization()( up_conv_out )
#
# up_conv_out = keras.layers.Conv2DTranspose( 32, (9,9), strides=2, padding='same', activation='relu' )( up_conv_out )
# up_conv_out = keras.layers.normalization.BatchNormalization()( up_conv_out )
#
#
# return up_conv_out
#
#
# def make_from_mobilenet( input_img=None, weights=None ):
# # input_img = keras.layers.BatchNormalization()(input_img)
#
# base_model = keras.applications.mobilenet.MobileNet( weights=weights, include_top=False, input_tensor=input_img )
# # base_model = keras.applications.mobilenet.MobileNet( weights=None, include_top=False, input_tensor=input_img )
# # keras.utils.plot_model( base_model, to_file='base_model.png', show_shapes=True )
#
# # Pull out a layer from original network
# base_model_out = base_model.get_layer( 'conv_pw_7_relu').output # can also try conv_pw_7_relu etc.
#
# # Up-sample
# # Basic idea is to try upsampling without using the transposed-conv layers. Instead use either of
# # a) upsampling2d
# # b) depth to space
# # followed by CBR (conv-BN-relu)
# # TODO
# ups_out = base_model_out
# # ups_out = keras.layers.UpSampling2D( size=(4,4) )( base_model_out )
#
# # model = keras.models.Model( inputs=input_img, outputs=ups_out )
# # model.summary()
# # keras.utils.plot_model( model, to_file='base_model.png', show_shapes=True )
# # code.interact( local=locals() )
#
# return ups_out
#
#
#--------------------------------------------------------------------------------
# Base CNNs
#--------------------------------------------------------------------------------
def make_vgg( input_img ):
r_l2=keras.regularizers.l2(0.01)
r_l1=keras.regularizers.l1(0.01)
x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( input_img )
x_64 = keras.layers.normalization.BatchNormalization()( x_64 )
x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )
x_64 = keras.layers.normalization.BatchNormalization()( x_64 )
x_64 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_64 )
x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )
x_128 = keras.layers.normalization.BatchNormalization()( x_128 )
x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )
x_128 = keras.layers.normalization.BatchNormalization()( x_128 )
x_128 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_128 )
# x_256 = keras.layers.Conv2D( 256, (3,3), padding='same', activation='relu' )( x_128 )
# x_256 = keras.layers.normalization.BatchNormalization()( x_256 )
# x_256 = keras.layers.Conv2D( 256, (3,3), padding='same', activation='relu' )( x_256 )
# x_256 = keras.layers.normalization.BatchNormalization()( x_256 )
# x_256 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_256 )
#
# x_512 = keras.layers.Conv2D( 512, (3,3), padding='same', activation='relu' )( x_256 )
# # BN
# x_512 = keras.layers.Conv2D( 512, (3,3), padding='same', activation='relu' )( x_512 )
# # BN
# x_512 = keras.layers.MaxPooling2D( pool_size=(2,2), padding='same' )( x_512 )
x = keras.layers.Conv2DTranspose( 32, (5,5), strides=4, padding='same' )( x_128 )
# x = x_128
return x
def make_upsampling_vgg( input_img ):
r_l2=keras.regularizers.l2(0.01)
r_l1=keras.regularizers.l1(0.01)
x_64 = keras.layers.Conv2D( 64, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( input_img )
x_64 = keras.layers.normalization.BatchNormalization()( x_64 )
x_64 = keras.layers.Conv2D( 64, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )
x_64 = keras.layers.normalization.BatchNormalization()( x_64 )
x_128 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_64 )
x_128 = keras.layers.normalization.BatchNormalization()( x_128 )
x_128 = keras.layers.Conv2D( 128, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )
x_128 = keras.layers.normalization.BatchNormalization()( x_128 )
x_256 = keras.layers.Conv2D( 128, (3,3), padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_128 )
x_256 = keras.layers.normalization.BatchNormalization()( x_256 )
x_256 = keras.layers.Conv2D( 128, (3,3), strides=2, padding='same', activation='relu', kernel_regularizer=r_l2, activity_regularizer=r_l1 )( x_256 )
x_256 = keras.layers.normalization.BatchNormalization()( x_256 )
z = keras.layers.Conv2DTranspose( 32, (11,11), strides=8, padding='same' )( x_256 )
x = keras.layers.Conv2DTranspose( 32, (9,9), strides=4, padding='same' )( x_128 )
y = keras.layers.Conv2DTranspose( 32, (7,7), strides=2, padding='same' )( x_64 )
out = keras.layers.Add()( [x,y,z] )
return out
def make_from_vgg19_multiconvup( input_img, trainable=False ):
base_model = keras.applications.vgg19.VGG19(weights='imagenet', include_top=False, input_tensor=input_img)
for l in base_model.layers:
l.trainable = trainable
#TODO : add kernel regularizers and activity_regularizer to conv layers
base_model_out = base_model.get_layer('block2_pool').output
up_conv_out = keras.layers.Conv2DTranspose( 32, (9,9), strides=2, padding='same', activation='relu' )( base_model_out )
up_conv_out = keras.layers.normalization.BatchNormalization()( up_conv_out )
up_conv_out = keras.layers.Conv2DTranspose( 32, (9,9), strides=2, padding='same', activation='relu' )( up_conv_out )
up_conv_out = keras.layers.normalization.BatchNormalization()( up_conv_out )
return up_conv_out
def make_from_mobilenet( input_img, weights=None, layer_name='conv_pw_7_relu' ):
# input_img = keras.layers.BatchNormalization()(input_img)
base_model = keras.applications.mobilenet.MobileNet( weights=weights, include_top=False, input_tensor=input_img )
# Pull out a layer from original network
base_model_out = base_model.get_layer( layer_name ).output # can also try conv_pw_7_relu etc.
return base_model_out
def make_from_vgg16( input_img, weights='imagenet', trainable=False, layer_name='block2_pool' ):
base_model = keras.applications.vgg16.VGG16(weights=weights, include_top=False, input_tensor=input_img)
for l in base_model.layers:
l.trainable = trainable
base_model_out = base_model.get_layer(layer_name).output
return base_model_out
================================================
FILE: scripts/predict_utils.py
================================================
import keras
import json
import pprint
import numpy as np
import cv2
import code
from keras import backend as K
from keras.engine.topology import Layer
# This was copied from the training code's CustomNets.py
class NetVLADLayer( Layer ):
def __init__( self, num_clusters, **kwargs ):
self.num_clusters = num_clusters
super(NetVLADLayer, self).__init__(**kwargs)
def build( self, input_shape ):
self.K = self.num_clusters
self.D = input_shape[-1]
self.kernel = self.add_weight( name='kernel',
shape=(1,1,self.D,self.K),
initializer='uniform',
trainable=True )
self.bias = self.add_weight( name='bias',
shape=(1,1,self.K),
initializer='uniform',
trainable=True )
self.C = self.add_weight( name='cluster_centers',
shape=[1,1,1,self.D,self.K],
initializer='uniform',
trainable=True)
def call( self, x ):
# soft-assignment.
s = K.conv2d( x, self.kernel, padding='same' ) + self.bias
a = K.softmax( s )
self.amap = K.argmax( a, -1 )
# print 'amap.shape', self.amap.shape
# Dims used hereafter: batch, H, W, desc_coeff, cluster
a = K.expand_dims( a, -2 )
# print 'a.shape=',a.shape
# Core
v = K.expand_dims(x, -1) + self.C
# print 'v.shape', v.shape
v = a * v
# print 'v.shape', v.shape
v = K.sum(v, axis=[1, 2])
# print 'v.shape', v.shape
v = K.permute_dimensions(v, pattern=[0, 2, 1])
# print 'v.shape', v.shape
#v.shape = None x K x D
# Normalize v (Intra Normalization)
v = K.l2_normalize( v, axis=-1 )
v = K.batch_flatten( v )
v = K.l2_normalize( v, axis=-1 )
# return [v, self.amap]
return v
def compute_output_shape( self, input_shape ):
# return [(input_shape[0], self.K*self.D ), (input_shape[0], input_shape[1], input_shape[2]) ]
return (input_shape[0], self.K*self.D )
def get_config( self ):
pass
# base_config = super(NetVLADLayer, self).get_config()
# return dict(list(base_config.items()))
# As suggested by: https://github.com/keras-team/keras/issues/4871#issuecomment-269731817
config = {'num_clusters': self.num_clusters}
base_config = super(NetVLADLayer, self).get_config()
return dict(list(base_config.items()) + list(config.items()))
class GhostVLADLayer( Layer ):
def __init__( self, num_clusters, num_ghost_clusters, **kwargs ):
self.num_clusters = num_clusters
self.num_ghost_clusters = num_ghost_clusters
super(GhostVLADLayer, self).__init__(**kwargs)
def build( self, input_shape ):
# self.K = self.num_clusters
self.K = self.num_clusters + self.num_ghost_clusters
self.D = input_shape[-1]
self.kernel = self.add_weight( name='kernel',
shape=(1,1,self.D,self.K),
initializer='uniform',
trainable=True )
self.bias = self.add_weight( name='bias',
shape=(1,1,self.K),
initializer='uniform',
trainable=True )
self.C = self.add_weight( name='cluster_centers',
shape=[1,1,1,self.D,self.K],
initializer='uniform',
trainable=True)
def call( self, x ):
# soft-assignment.
s = K.conv2d( x, self.kernel, padding='same' ) + self.bias
a = K.softmax( s )
self.amap = K.argmax( a, -1 )
# print 'amap.shape', self.amap.shape
# Dims used hereafter: batch, H, W, desc_coeff, cluster
a = K.expand_dims( a, -2 )
# print 'a.shape=',a.shape
# Core
v = K.expand_dims(x, -1) + self.C
# print 'v.shape', v.shape
v = a * v
# print 'v.shape', v.shape
v = K.sum(v, axis=[1, 2])
# print 'v.shape', v.shape
v = K.permute_dimensions(v, pattern=[0, 2, 1])
# print 'v.shape', v.shape
#v.shape = None x K x D
# Normalize v (Intra Normalization)
v = v[:,0:self.num_clusters,:]
# print 'after ghosting v.shape', v.shape
v = K.l2_normalize( v, axis=-1 )
v = K.batch_flatten( v )
v = K.l2_normalize( v, axis=-1 )
# return [v, self.amap]
return v
def compute_output_shape( self, input_shape ):
# return [(input_shape[0], self.num_clusters*self.D ), (input_shape[0], input_shape[1], input_shape[2]) ]
return (input_shape[0], self.num_clusters*self.D )
def get_config( self ):
pass
# As suggested by: https://github.com/keras-team/keras/issues/4871#issuecomment-269731817
config = {'num_clusters': self.num_clusters, 'num_ghost_clusters': self.num_ghost_clusters}
base_config = super(GhostVLADLayer, self).get_config()
return dict(list(base_config.items()) + list(config.items()))
#--------------------------- UTILS --------------------------------------------#
def open_json_file( fname ):
print 'Load JSON file: ', fname
jsonX = json.loads(open(fname).read())
return jsonX
def change_model_inputshape(model, new_input_shape=(None, 40, 40, 3), verbose=False):
"""
Given a model and new input shape it changes all the allocations.
Note: It uses custom_objects={'NetVLAD': NetVLADLayer}. If you have any other
custom-layer change the code here accordingly.
"""
print '[change_model_inputshape], new_input_shape=', new_input_shape
# replace input shape of first layer
model._layers[0].batch_input_shape = new_input_shape
# feel free to modify additional parameters of other layers, for example...
# model._layers[2].pool_size = (8, 8)
# model._layers[2].strides = (8, 8)
# rebuild model architecture by exporting and importing via json
new_model = keras.models.model_from_json(model.to_json(), custom_objects={'NetVLADLayer': NetVLADLayer, 'GhostVLADLayer':GhostVLADLayer} )
# new_model.summary()
# copy weights from old model to new one
print 'copy weights from old model to new one....this usually takes upto 10 sec'
for layer in new_model.layers:
try:
if verbose:
print( 'transfer weights for layer.name='+layer.name )
layer.set_weights(model.get_layer(name=layer.name).get_weights())
except:
print '[Almost certainly FATAL] '
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'
print("Could not transfer weights for layer {}".format(layer.name))
quit()
# test new model on a random input image
# X = np.random.rand(new_input_shape[0], new_input_shape[1], new_input_shape[2], new_input_shape[3] )
# y_pred = new_model.predict(X)
# print('try predict with a random input_img with shape='+str(X.shape)+ str(y_pred) )
return new_model
#--------------------------- END UTILS ----------------------------------------#
================================================
FILE: scripts/unittest/demo_superpoint.py
================================================
#!/usr/bin/env python
#
# %BANNER_BEGIN%
# ---------------------------------------------------------------------
# %COPYRIGHT_BEGIN%
#
# Magic Leap, Inc. ("COMPANY") CONFIDENTIAL
#
# Unpublished Copyright (c) 2018
# Magic Leap, Inc., All Rights Reserved.
#
# NOTICE: All information contained herein is, and remains the property
# of COMPANY. The intellectual and technical concepts contained herein
# are proprietary to COMPANY and may be covered by U.S. and Foreign
# Patents, patents in process, and are protected by trade secret or
# copyright law. Dissemination of this information or reproduction of
# this material is strictly forbidden unless prior written permission is
# obtained from COMPANY. Access to the source code contained herein is
# hereby forbidden to anyone except current COMPANY employees, managers
# or contractors who have executed Confidentiality and Non-disclosure
# agreements explicitly covering such access.
#
# The copyright notice above does not evidence any actual or intended
# publication or disclosure of this source code, which includes
# information that is confidential and/or proprietary, and is a trade
# secret, of COMPANY. ANY REPRODUCTION, MODIFICATION, DISTRIBUTION,
# PUBLIC PERFORMANCE, OR PUBLIC DISPLAY OF OR THROUGH USE OF THIS
# SOURCE CODE WITHOUT THE EXPRESS WRITTEN CONSENT OF COMPANY IS
# STRICTLY PROHIBITED, AND IN VIOLATION OF APPLICABLE LAWS AND
# INTERNATIONAL TREATIES. THE RECEIPT OR POSSESSION OF THIS SOURCE
# CODE AND/OR RELATED INFORMATION DOES NOT CONVEY OR IMPLY ANY RIGHTS
# TO REPRODUCE, DISCLOSE OR DISTRIBUTE ITS CONTENTS, OR TO MANUFACTURE,
# USE, OR SELL ANYTHING THAT IT MAY DESCRIBE, IN WHOLE OR IN PART.
#
# %COPYRIGHT_END%
# ----------------------------------------------------------------------
# %AUTHORS_BEGIN%
#
# Originating Authors: Daniel DeTone (ddetone)
# Tomasz Malisiewicz (tmalisiewicz)
#
# %AUTHORS_END%
# --------------------------------------------------------------------*/
# %BANNER_END%
import argparse
import glob
import numpy as np
import os
import time
import code
import cv2
import torch
# Stub to warn about opencv version.
if int(cv2.__version__[0]) < 3: # pragma: no cover
print('Warning: OpenCV 3 is not installed')
# Jet colormap for visualization.
myjet = np.array([[0. , 0. , 0.5 ],
[0. , 0. , 0.99910873],
[0. , 0.37843137, 1. ],
[0. , 0.83333333, 1. ],
[0.30044276, 1. , 0.66729918],
[0.66729918, 1. , 0.30044276],
[1. , 0.90123457, 0. ],
[1. , 0.48002905, 0. ],
[0.99910873, 0.07334786, 0. ],
[0.5 , 0. , 0. ]])
class SuperPointNet(torch.nn.Module):
""" Pytorch definition of SuperPoint Network. """
def __init__(self):
super(SuperPointNet, self).__init__()
self.relu = torch.nn.ReLU(inplace=True)
self.pool = torch.nn.MaxPool2d(kernel_size=2, stride=2)
c1, c2, c3, c4, c5, d1 = 64, 64, 128, 128, 256, 256
# Shared Encoder.
self.conv1a = torch.nn.Conv2d(1, c1, kernel_size=3, stride=1, padding=1)
self.conv1b = torch.nn.Conv2d(c1, c1, kernel_size=3, stride=1, padding=1)
self.conv2a = torch.nn.Conv2d(c1, c2, kernel_size=3, stride=1, padding=1)
self.conv2b = torch.nn.Conv2d(c2, c2, kernel_size=3, stride=1, padding=1)
self.conv3a = torch.nn.Conv2d(c2, c3, kernel_size=3, stride=1, padding=1)
self.conv3b = torch.nn.Conv2d(c3, c3, kernel_size=3, stride=1, padding=1)
self.conv4a = torch.nn.Conv2d(c3, c4, kernel_size=3, stride=1, padding=1)
self.conv4b = torch.nn.Conv2d(c4, c4, kernel_size=3, stride=1, padding=1)
# Detector Head.
self.convPa = torch.nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1)
self.convPb = torch.nn.Conv2d(c5, 65, kernel_size=1, stride=1, padding=0)
# Descriptor Head.
self.convDa = torch.nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1)
self.convDb = torch.nn.Conv2d(c5, d1, kernel_size=1, stride=1, padding=0)
def forward(self, x):
""" Forward pass that jointly computes unprocessed point and descriptor
tensors.
Input
x: Image pytorch tensor shaped N x 1 x H x W.
Output
semi: Output point pytorch tensor shaped N x 65 x H/8 x W/8.
desc: Output descriptor pytorch tensor shaped N x 256 x H/8 x W/8.
"""
# Shared Encoder.
x = self.relu(self.conv1a(x))
x = self.relu(self.conv1b(x))
x = self.pool(x)
x = self.relu(self.conv2a(x))
x = self.relu(self.conv2b(x))
x = self.pool(x)
x = self.relu(self.conv3a(x))
x = self.relu(self.conv3b(x))
x = self.pool(x)
x = self.relu(self.conv4a(x))
x = self.relu(self.conv4b(x))
# Detector Head.
cPa = self.relu(self.convPa(x))
semi = self.convPb(cPa)
# Descriptor Head.
cDa = self.relu(self.convDa(x))
desc = self.convDb(cDa)
dn = torch.norm(desc, p=2, dim=1) # Compute the norm.
desc = desc.div(torch.unsqueeze(dn, 1)) # Divide by norm to normalize.
return semi, desc
class SuperPointFrontend(object):
""" Wrapper around pytorch net to help with pre and post image processing. """
def __init__(self, weights_path, nms_dist, conf_thresh, nn_thresh,
cuda=False):
self.name = 'SuperPoint'
self.cuda = cuda
self.nms_dist = nms_dist
self.conf_thresh = conf_thresh
self.nn_thresh = nn_thresh # L2 descriptor distance for good match.
self.cell = 8 # Size of each output cell. Keep this fixed.
self.border_remove = 4 # Remove points this close to the border.
# Load the network in inference mode.
self.net = SuperPointNet()
if cuda:
# Train on GPU, deploy on GPU.
self.net.load_state_dict(torch.load(weights_path))
self.net = self.net.cuda()
else:
# Train on GPU, deploy on CPU.
self.net.load_state_dict(torch.load(weights_path,
map_location=lambda storage, loc: storage))
self.net.eval()
def nms_fast(self, in_corners, H, W, dist_thresh):
"""
Run a faster approximate Non-Max-Suppression on numpy corners shaped:
3xN [x_i,y_i,conf_i]^T
Algo summary: Create a grid sized HxW. Assign each corner location a 1, rest
are zeros. Iterate through all the 1's and convert them either to -1 or 0.
Suppress points by setting nearby values to 0.
Grid Value Legend:
-1 : Kept.
0 : Empty or suppressed.
1 : To be processed (converted to either kept or supressed).
NOTE: The NMS first rounds points to integers, so NMS distance might not
be exactly dist_thresh. It also assumes points are within image boundaries.
Inputs
in_corners - 3xN numpy array with corners [x_i, y_i, confidence_i]^T.
H - Image height.
W - Image width.
dist_thresh - Distance to suppress, measured as an infinty norm distance.
Returns
nmsed_corners - 3xN numpy matrix with surviving corners.
nmsed_inds - N length numpy vector with surviving corner indices.
"""
grid = np.zeros((H, W)).astype(int) # Track NMS data.
inds = np.zeros((H, W)).astype(int) # Store indices of points.
# Sort by confidence and round to nearest int.
inds1 = np.argsort(-in_corners[2,:])
corners = in_corners[:,inds1]
rcorners = corners[:2,:].round().astype(int) # Rounded corners.
# Check for edge case of 0 or 1 corners.
if rcorners.shape[1] == 0:
return np.zeros((3,0)).astype(int), np.zeros(0).astype(int)
if rcorners.shape[1] == 1:
out = np.vstack((rcorners, in_corners[2])).reshape(3,1)
return out, np.zeros((1)).astype(int)
# Initialize the grid.
for i, rc in enumerate(rcorners.T):
grid[rcorners[1,i], rcorners[0,i]] = 1
inds[rcorners[1,i], rcorners[0,i]] = i
# Pad the border of the grid, so that we can NMS points near the border.
pad = dist_thresh
grid = np.pad(grid, ((pad,pad), (pad,pad)), mode='constant')
# Iterate through points, highest to lowest conf, suppress neighborhood.
count = 0
for i, rc in enumerate(rcorners.T):
# Account for top and left padding.
pt = (rc[0]+pad, rc[1]+pad)
if grid[pt[1], pt[0]] == 1: # If not yet suppressed.
grid[pt[1]-pad:pt[1]+pad+1, pt[0]-pad:pt[0]+pad+1] = 0
grid[pt[1], pt[0]] = -1
count += 1
# Get all surviving -1's and return sorted array of remaining corners.
keepy, keepx = np.where(grid==-1)
keepy, keepx = keepy - pad, keepx - pad
inds_keep = inds[keepy, keepx]
out = corners[:, inds_keep]
values = out[-1, :]
inds2 = np.argsort(-values)
out = out[:, inds2]
out_inds = inds1[inds_keep[inds2]]
return out, out_inds
def run(self, img):
""" Process a numpy image to extract points and descriptors.
Input
img - HxW numpy float32 input image in range [0,1].
Output
corners - 3xN numpy array with corners [x_i, y_i, confidence_i]^T.
desc - 256xN numpy array of corresponding unit normalized descriptors.
heatmap - HxW numpy heatmap in range [0,1] of point confidences.
"""
assert img.ndim == 2, 'Image must be grayscale.'
assert img.dtype == np.float32, 'Image must be float32.'
H, W = img.shape[0], img.shape[1]
inp = img.copy()
inp = (inp.reshape(1, H, W))
inp = torch.from_numpy(inp)
inp = torch.autograd.Variable(inp).view(1, 1, H, W)
if self.cuda:
inp = inp.cuda()
# Forward pass of network.
outs = self.net.forward(inp)
semi, coarse_desc = outs[0], outs[1]
# Convert pytorch -> numpy.
semi = semi.data.cpu().numpy().squeeze()
# --- Process points.
dense = np.exp(semi) # Softmax.
dense = dense / (np.sum(dense, axis=0)+.00001) # Should sum to 1.
# Remove dustbin.
nodust = dense[:-1, :, :]
# Reshape to get full resolution heatmap.
Hc = int(H / self.cell)
Wc = int(W / self.cell)
nodust = nodust.transpose(1, 2, 0)
heatmap = np.reshape(nodust, [Hc, Wc, self.cell, self.cell])
heatmap = np.transpose(heatmap, [0, 2, 1, 3])
heatmap = np.reshape(heatmap, [Hc*self.cell, Wc*self.cell])
xs, ys = np.where(heatmap >= self.conf_thresh) # Confidence threshold.
if len(xs) == 0:
return np.zeros((3, 0)), None, None
pts = np.zeros((3, len(xs))) # Populate point data sized 3xN.
pts[0, :] = ys
pts[1, :] = xs
pts[2, :] = heatmap[xs, ys]
pts, _ = self.nms_fast(pts, H, W, dist_thresh=self.nms_dist) # Apply NMS.
inds = np.argsort(pts[2,:])
pts = pts[:,inds[::-1]] # Sort by confidence.
# Remove points along border.
bord = self.border_remove
toremoveW = np.logical_or(pts[0, :] < bord, pts[0, :] >= (W-bord))
toremoveH = np.logical_or(pts[1, :] < bord, pts[1, :] >= (H-bord))
toremove = np.logical_or(toremoveW, toremoveH)
pts = pts[:, ~toremove]
# --- Process descriptor.
D = coarse_desc.shape[1]
if pts.shape[1] == 0:
desc = np.zeros((D, 0))
else:
# Interpolate into descriptor map using 2D point locations.
samp_pts = torch.from_numpy(pts[:2, :].copy())
samp_pts[0, :] = (samp_pts[0, :] / (float(W)/2.)) - 1.
samp_pts[1, :] = (samp_pts[1, :] / (float(H)/2.)) - 1.
samp_pts = samp_pts.transpose(0, 1).contiguous()
samp_pts = samp_pts.view(1, 1, -1, 2)
samp_pts = samp_pts.float()
if self.cuda:
samp_pts = samp_pts.cuda()
desc = torch.nn.functional.grid_sample(coarse_desc, samp_pts)
desc = desc.data.cpu().numpy().reshape(D, -1)
desc /= np.linalg.norm(desc, axis=0)[np.newaxis, :]
return pts, desc, heatmap
class PointTracker(object):
""" Class to manage a fixed memory of points and descriptors that enables
sparse optical flow point tracking.
Internally, the tracker stores a 'tracks' matrix sized M x (2+L), of M
tracks with maximum length L, where each row corresponds to:
row_m = [track_id_m, avg_desc_score_m, point_id_0_m, ..., point_id_L-1_m].
"""
def __init__(self, max_length, nn_thresh):
if max_length < 2:
raise ValueError('max_length must be greater than or equal to 2.')
self.maxl = max_length
self.nn_thresh = nn_thresh
self.all_pts = []
for n in range(self.maxl):
self.all_pts.append(np.zeros((2, 0)))
self.last_desc = None
self.tracks = np.zeros((0, self.maxl+2))
self.track_count = 0
self.max_score = 9999
def nn_match_two_way(self, desc1, desc2, nn_thresh):
"""
Performs two-way nearest neighbor matching of two sets of descriptors, such
that the NN match from descriptor A->B must equal the NN match from B->A.
Inputs:
desc1 - NxM numpy matrix of N corresponding M-dimensional descriptors.
desc2 - NxM numpy matrix of N corresponding M-dimensional descriptors.
nn_thresh - Optional descriptor distance below which is a good match.
Returns:
matches - 3xL numpy array, of L matches, where L <= N and each column i is
a match of two descriptors, d_i in image 1 and d_j' in image 2:
[d_i index, d_j' index, match_score]^T
"""
assert desc1.shape[0] == desc2.shape[0]
if desc1.shape[1] == 0 or desc2.shape[1] == 0:
return np.zeros((3, 0))
if nn_thresh < 0.0:
raise ValueError('\'nn_thresh\' should be non-negative')
# Compute L2 distance. Easy since vectors are unit normalized.
dmat = np.dot(desc1.T, desc2)
dmat = np.sqrt(2-2*np.clip(dmat, -1, 1))
# Get NN indices and scores.
idx = np.argmin(dmat, axis=1)
scores = dmat[np.arange(dmat.shape[0]), idx]
# Threshold the NN matches.
keep = scores < nn_thresh
# Check if nearest neighbor goes both directions and keep those.
idx2 = np.argmin(dmat, axis=0)
keep_bi = np.arange(len(idx)) == idx2[idx]
keep = np.logical_and(keep, keep_bi)
idx = idx[keep]
scores = scores[keep]
# Get the surviving point indices.
m_idx1 = np.arange(desc1.shape[1])[keep]
m_idx2 = idx
# Populate the final 3xN match data structure.
matches = np.zeros((3, int(keep.sum())))
matches[0, :] = m_idx1
matches[1, :] = m_idx2
matches[2, :] = scores
return matches
def get_offsets(self):
""" Iterate through list of points and accumulate an offset value. Used to
index the global point IDs into the list of points.
Returns
offsets - N length array with integer offset locations.
"""
# Compute id offsets.
offsets = []
offsets.append(0)
for i in range(len(self.all_pts)-1): # Skip last camera size, not needed.
offsets.append(self.all_pts[i].shape[1])
offsets = np.array(offsets)
offsets = np.cumsum(offsets)
return offsets
def update(self, pts, desc):
""" Add a new set of point and descriptor observations to the tracker.
Inputs
pts - 3xN numpy array of 2D point observations.
desc - DxN numpy array of corresponding D dimensional descriptors.
"""
if pts is None or desc is None:
print('PointTracker: Warning, no points were added to tracker.')
return
assert pts.shape[1] == desc.shape[1]
# Initialize last_desc.
if self.last_desc is None:
self.last_desc = np.zeros((desc.shape[0], 0))
# Remove oldest points, store its size to update ids later.
remove_size = self.all_pts[0].shape[1]
self.all_pts.pop(0)
self.all_pts.append(pts)
# Remove oldest point in track.
self.tracks = np.delete(self.tracks, 2, axis=1)
# Update track offsets.
for i in range(2, self.tracks.shape[1]):
self.tracks[:, i] -= remove_size
self.tracks[:, 2:][self.tracks[:, 2:] < -1] = -1
offsets = self.get_offsets()
# Add a new -1 column.
self.tracks = np.hstack((self.tracks, -1*np.ones((self.tracks.shape[0], 1))))
# Try to append to existing tracks.
matched = np.zeros((pts.shape[1])).astype(bool)
matches = self.nn_match_two_way(self.last_desc, desc, self.nn_thresh)
for match in matches.T:
# Add a new point to it's matched track.
id1 = int(match[0]) + offsets[-2]
id2 = int(match[1]) + offsets[-1]
found = np.argwhere(self.tracks[:, -2] == id1)
if found.shape[0] > 0:
matched[int(match[1])] = True
row = int(found)
self.tracks[row, -1] = id2
if self.tracks[row, 1] == self.max_score:
# Initialize track score.
self.tracks[row, 1] = match[2]
else:
# Update track score with running average.
# NOTE(dd): this running average can contain scores from old matches
# not contained in last max_length track points.
track_len = (self.tracks[row, 2:] != -1).sum() - 1.
frac = 1. / float(track_len)
self.tracks[row, 1] = (1.-frac)*self.tracks[row, 1] + frac*match[2]
# Add unmatched tracks.
new_ids = np.arange(pts.shape[1]) + offsets[-1]
new_ids = new_ids[~matched]
new_tracks = -1*np.ones((new_ids.shape[0], self.maxl + 2))
new_tracks[:, -1] = new_ids
new_num = new_ids.shape[0]
new_trackids = self.track_count + np.arange(new_num)
new_tracks[:, 0] = new_trackids
new_tracks[:, 1] = self.max_score*np.ones(new_ids.shape[0])
self.tracks = np.vstack((self.tracks, new_tracks))
self.track_count += new_num # Update the track count.
# Remove empty tracks.
keep_rows = np.any(self.tracks[:, 2:] >= 0, axis=1)
self.tracks = self.tracks[keep_rows, :]
# Store the last descriptors.
self.last_desc = desc.copy()
return
def get_tracks(self, min_length):
""" Retrieve point tracks of a given minimum length.
Input
min_length - integer >= 1 with minimum track length
Output
returned_tracks - M x (2+L) sized matrix storing track indices, where
M is the number of tracks and L is the maximum track length.
"""
if min_length < 1:
raise ValueError('\'min_length\' too small.')
valid = np.ones((self.tracks.shape[0])).astype(bool)
good_len = np.sum(self.tracks[:, 2:] != -1, axis=1) >= min_length
# Remove tracks which do not have an observation in most recent frame.
not_headless = (self.tracks[:, -1] != -1)
keepers = np.logical_and.reduce((valid, good_len, not_headless))
returned_tracks = self.tracks[keepers, :].copy()
return returned_tracks
def draw_tracks(self, out, tracks):
""" Visualize tracks all overlayed on a single image.
Inputs
out - numpy uint8 image sized HxWx3 upon which tracks are overlayed.
tracks - M x (2+L) sized matrix storing track info.
"""
# Store the number of points per camera.
pts_mem = self.all_pts
N = len(pts_mem) # Number of cameras/images.
# Get offset ids needed to reference into pts_mem.
offsets = self.get_offsets()
# Width of track and point circles to be drawn.
stroke = 1
# Iterate through each track and draw it.
for track in tracks:
clr = myjet[int(np.clip(np.floor(track[1]*10), 0, 9)), :]*255
for i in range(N-1):
if track[i+2] == -1 or track[i+3] == -1:
continue
offset1 = offsets[i]
offset2 = offsets[i+1]
idx1 = int(track[i+2]-offset1)
idx2 = int(track[i+3]-offset2)
pt1 = pts_mem[i][:2, idx1]
pt2 = pts_mem[i+1][:2, idx2]
p1 = (int(round(pt1[0])), int(round(pt1[1])))
p2 = (int(round(pt2[0])), int(round(pt2[1])))
cv2.line(out, p1, p2, clr, thickness=stroke, lineType=16)
# Draw end points of each track.
if i == N-2:
clr2 = (255, 0, 0)
cv2.circle(out, p2, stroke, clr2, -1, lineType=16)
class VideoStreamer(object):
""" Class to help process image streams. Three types of possible inputs:"
1.) USB Webcam.
2.) A directory of images (files in directory matching 'img_glob').
3.) A video file, such as an .mp4 or .avi file.
"""
def __init__(self, basedir, camid, height, width, skip, img_glob):
self.cap = []
self.camera = False
self.video_file = False
self.listing = []
self.sizer = [height, width]
self.i = 0
self.skip = skip
self.maxlen = 1000000
# If the "basedir" string is the word camera, then use a webcam.
if basedir == "camera/" or basedir == "camera":
print('==> Processing Webcam Input.')
self.cap = cv2.VideoCapture(camid)
self.listing = range(0, self.maxlen)
self.camera = True
else:
# Try to open as a video.
self.cap = cv2.VideoCapture(basedir)
lastbit = basedir[-4:len(basedir)]
if (type(self.cap) == list or not self.cap.isOpened()) and (lastbit == '.mp4'):
raise IOError('Cannot open movie file')
elif type(self.cap) != list and self.cap.isOpened() and (lastbit != '.txt'):
print('==> Processing Video Input.')
num_frames = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT))
self.listing = range(0, num_frames)
self.listing = self.listing[::self.skip]
self.camera = True
self.video_file = True
self.maxlen = len(self.listing)
else:
print('==> Processing Image Directory Input.')
search = os.path.join(basedir, img_glob)
self.listing = glob.glob(search)
self.listing.sort()
self.listing = self.listing[::self.skip]
self.maxlen = len(self.listing)
if self.maxlen == 0:
raise IOError('No images were found (maybe bad \'--img_glob\' parameter?)')
def read_image(self, impath, img_size):
""" Read image as grayscale and resize to img_size.
Inputs
impath: Path to input image.
img_size: (W, H) tuple specifying resize size.
Returns
grayim: float32 numpy array sized H x W with values in range [0, 1].
"""
grayim = cv2.imread(impath, 0)
if grayim is None:
raise Exception('Error reading image %s' % impath)
# Image is resized via opencv.
interp = cv2.INTER_AREA
grayim = cv2.resize(grayim, (img_size[1], img_size[0]), interpolation=interp)
grayim = (grayim.astype('float32') / 255.)
return grayim
def next_frame(self):
""" Return the next frame, and increment internal counter.
Returns
image: Next H x W image.
status: True or False depending whether image was loaded.
"""
if self.i == self.maxlen:
return (None, False)
if self.camera:
ret, input_image = self.cap.read()
if ret is False:
print('VideoStreamer: Cannot get image from camera (maybe bad --camid?)')
return (None, False)
if self.video_file:
self.cap.set(cv2.CAP_PROP_POS_FRAMES, self.listing[self.i])
input_image = cv2.resize(input_image, (self.sizer[1], self.sizer[0]),
interpolation=cv2.INTER_AREA)
input_image = cv2.cvtColor(input_image, cv2.COLOR_RGB2GRAY)
input_image = input_image.astype('float')/255.0
else:
image_file = self.listing[self.i]
input_image = self.read_image(image_file, self.sizer)
# Increment internal counter.
self.i = self.i + 1
input_image = input_image.astype('float32')
return (input_image, True)
if __name__ == '__main__':
# Parse command line arguments.
parser = argparse.ArgumentParser(description='PyTorch SuperPoint Demo.')
parser.add_argument('input', type=str, default='',
help='Image directory or movie file or "camera" (for webcam).')
parser.add_argument('--weights_path', type=str, default='superpoint_v1.pth',
help='Path to pretrained weights file (default: superpoint_v1.pth).')
parser.add_argument('--img_glob', type=str, default='*.png',
help='Glob match if directory of images is specified (default: \'*.png\').')
parser.add_argument('--skip', type=int, default=1,
help='Images to skip if input is movie or directory (default: 1).')
parser.add_argument('--show_extra', action='store_true',
help='Show extra debug outputs (default: False).')
parser.add_argument('--H', type=int, default=120,
help='Input image height (default: 120).')
parser.add_argument('--W', type=int, default=160,
help='Input image width (default:160).')
parser.add_argument('--display_scale', type=int, default=2,
help='Factor to scale output visualization (default: 2).')
parser.add_argument('--min_length', type=int, default=2,
help='Minimum length of point tracks (default: 2).')
parser.add_argument('--max_length', type=int, default=5,
help='Maximum length of point tracks (default: 5).')
parser.add_argument('--nms_dist', type=int, default=4,
help='Non Maximum Suppression (NMS) distance (default: 4).')
parser.add_argument('--conf_thresh', type=float, default=0.015,
help='Detector confidence threshold (default: 0.015).')
parser.add_argument('--nn_thresh', type=float, default=0.7,
help='Descriptor matching threshold (default: 0.7).')
parser.add_argument('--camid', type=int, default=0,
help='OpenCV webcam video capture ID, usually 0 or 1 (default: 0).')
parser.add_argument('--waitkey', type=int, default=1,
help='OpenCV waitkey time in ms (default: 1).')
parser.add_argument('--cuda', action='store_true',
help='Use cuda GPU to speed up network processing speed (default: False)')
parser.add_argument('--no_display', action='store_true',
help='Do not display images to screen. Useful if running remotely (default: False).')
parser.add_argument('--write', action='store_true',
help='Save output frames to a directory (default: False)')
parser.add_argument('--write_dir', type=str, default='tracker_outputs/',
help='Directory where to write output frames (default: tracker_outputs/).')
opt = parser.parse_args()
print(opt)
# code.interact( local=locals() )
# This class helps load input images from different sources.
vs = VideoStreamer(opt.input, opt.camid, opt.H, opt.W, opt.skip, opt.img_glob)
print('==> Loading pre-trained network.')
# This class runs the SuperPoint network and processes its outputs.
fe = SuperPointFrontend(weights_path=opt.weights_path,
nms_dist=opt.nms_dist,
conf_thresh=opt.conf_thresh,
nn_thresh=opt.nn_thresh,
cuda=opt.cuda)
print('==> Successfully loaded pre-trained network.')
# This class helps merge consecutive point matches into tracks.
tracker = PointTracker(opt.max_length, nn_thresh=fe.nn_thresh)
# Create a window to display the demo.
if not opt.no_display:
win = 'SuperPoint Tracker'
cv2.namedWindow(win)
else:
print('Skipping visualization, will not show a GUI.')
# Font parameters for visualizaton.
font = cv2.FONT_HERSHEY_DUPLEX
font_clr = (255, 255, 255)
font_pt = (4, 12)
font_sc = 0.4
# Create output directory if desired.
if opt.write:
print('==> Will write outputs to %s' % opt.write_dir)
if not os.path.exists(opt.write_dir):
os.makedirs(opt.write_dir)
print('==> Running Demo.')
while True:
start = time.time()
# Get a new image.
img, status = vs.next_frame()
if status is False:
break
# Get points and descriptors.
start1 = time.time()
pts, desc, heatmap = fe.run(img)
# import code
# code.interact( local=locals() )
end1 = time.time()
# Add points and descriptors to the tracker.
tracker.update(pts, desc)
# Get tracks for points which were match successfully across all frames.
tracks = tracker.get_tracks(opt.min_length)
# Primary output - Show point tracks overlayed on top of input image.
out1 = (np.dstack((img, img, img)) * 255.).astype('uint8')
tracks[:, 1] /= float(fe.nn_thresh) # Normalize track scores to [0,1].
tracker.draw_tracks(out1, tracks)
if opt.show_extra:
cv2.putText(out1, 'Point Tracks', font_pt, font, font_sc, font_clr, lineType=16)
# Extra output -- Show current point detections.
out2 = (np.dstack((img, img, img)) * 255.).astype('uint8')
for pt in pts.T:
pt1 = (int(round(pt[0])), int(round(pt[1])))
cv2.circle(out2, pt1, 1, (0, 255, 0), -1, lineType=16)
cv2.putText(out2, 'Raw Point Detections', font_pt, font, font_sc, font_clr, lineType=16)
# Extra output -- Show the point confidence heatmap.
if heatmap is not None:
min_conf = 0.001
heatmap[heatmap < min_conf] = min_conf
heatmap = -np.log(heatmap)
heatmap = (heatmap - heatmap.min()) / (heatmap.max() - heatmap.min() + .00001)
out3 = myjet[np.round(np.clip(heatmap*10, 0, 9)).astype('int'), :]
out3 = (out3*255).astype('uint8')
else:
out3 = np.zeros_like(out2)
cv2.putText(out3, 'Raw Point Confidences', font_pt, font, font_sc, font_clr, lineType=16)
# Resize final output.
if opt.show_extra:
out = np.hstack((out1, out2, out3))
out = cv2.resize(out, (3*opt.display_scale*opt.W, opt.display_scale*opt.H))
else:
out = cv2.resize(out1, (opt.display_scale*opt.W, opt.display_scale*opt.H))
# Display visualization image to screen.
if not opt.no_display:
cv2.imshow(win, out)
key = cv2.waitKey(opt.waitkey) & 0xFF
if key == ord('q'):
print('Quitting, \'q\' pressed.')
break
# Optionally write images to disk.
if opt.write:
out_file = os.path.join(opt.write_dir, 'frame_%05d.png' % vs.i)
print('Writing image to %s' % out_file)
cv2.imwrite(out_file, out)
end = time.time()
net_t = (1./ float(end1 - start))
total_t = (1./ float(end - start))
if opt.show_extra:
print('Processed image %d (net+post_process: %.2f FPS, total: %.2f FPS).'\
% (vs.i, net_t, total_t))
# Close any remaining windows.
cv2.destroyAllWindows()
print('==> Finshed Demo.')
================================================
FILE: scripts/unittest/rtry_superpoint.py
================================================
#!/usr/bin/env python
import numpy as np
import code
import cv2
import time
import json
import torch
# Stub to warn about opencv version.
if int(cv2.__version__[0]) < 3: # pragma: no cover
print('Warning: OpenCV 3 is not installed')
from demo_superpoint import *
class KSuperPointExp:
def __init__(self):
#------------------------------------------------------------------------
# Setup Neural Net
weights_path='superpoint_v1.pth'
nms_dist=4
conf_thresh=0.015
nn_thresh=0.7
cuda=True
print('==> Loading pre-trained network.')
# This class runs the SuperPoint network and processes its outputs.
self.fe = SuperPointFrontend(weights_path=weights_path,
nms_dist=nms_dist,
conf_thresh=conf_thresh,
nn_thresh=nn_thresh,
cuda=cuda)
print('==> Successfully loaded pre-trained network.')
#-----------------------------------------------------------------------
# Tracker - NN comparison for desc
max_length = 5 # default as in the demo file
self.tracker = PointTracker(max_length, nn_thresh=self.fe.nn_thresh)
def plot_pts( self, xcanvas, pts ):
# pts: [x_i, y_i, confidence_i]. 3xN
for i in range( pts.shape[1] ):
cv2.circle( xcanvas, (int(pts[0,i]), int(pts[1,i]) ), 1, (0,0,255), -1 )
return xcanvas
def plot_point_sets( self, im0, pts0, im1, pts1 ):
# pts: [x_i, y_i, confidence_i]. 3xN
# try:
xcanvas = np.concatenate( (im0, im1), axis=1 )
if pts0.shape[0] == 0:
return xcanvas
# except:
# code.interact( local=locals() )
assert( pts0.shape[1] == pts1.shape[1] )
assert( pts0.shape[0] == 3 or pts0.shape[0] == 2 )
assert( pts1.shape[0] == 3 or pts1.shape[0] == 2 )
for i in range( pts0.shape[1] ):
pt0 = (int(pts0[0,i]), int(pts0[1,i]) )
pt1 = (int(pts1[0,i] + im0.shape[1] ), int(pts1[1,i]) )
cv2.circle( xcanvas, pt0, 1, (0,255,0), -1)
cv2.circle( xcanvas, pt1, 1, (0,255,0), -1)
cv2.line( xcanvas, pt0, pt1, (0,255,0) )
return xcanvas
def read_image(self, impath, img_size):
""" Read image as grayscale and resize to img_size.
Inputs
impath: Path to input image.
img_size: (W, H) tuple specifying resize size.
Returns
grayim: float32 numpy array sized H x W with values in range [0, 1].
"""
grayim = cv2.imread(impath, 0)
if grayim is None:
raise Exception('Error reading image %s' % impath)
# Image is resized via opencv.
interp = cv2.INTER_AREA
grayim = cv2.resize(grayim, (img_size[1], img_size[0]), interpolation=interp)
grayim = (grayim.astype('float32') / 255.)
return grayim
def get_descriptor( self, image ):
""" Given an image returns the keypoints and descriptors.
Input
img - HxW numpy float32 input image in range [0,1].
Output
corners - 3xN numpy array with corners [x_i, y_i, confidence_i]^T.
desc - 256xN numpy array of corresponding unit normalized descriptors.
heatmap - HxW numpy heatmap in range [0,1] of point confidences.
"""
start1 = time.time()
pts, desc, heatmap = self.fe.run(image)
print 'superpoint computed in %4.2fms' %(1000. * (time.time() - start1))
return pts, desc, heatmap
def match_image_pair( self, fname0, fname1 ):
""" Given two image file names, read the two images (as per SuperPoint's requirement).
Computes descriptors and then does NN comparison.
"""
ksp_image0 = ksp.read_image(fname0, [240,320] )
ksp_image1 = ksp.read_image(fname1, [240,320] )
image0 = cv2.imread( fname0 )
image1 = cv2.imread( fname1 )
pts0, desc0, heatmap0 = ksp.get_descriptor( ksp_image0 )
pts1, desc1, heatmap1 = ksp.get_descriptor( ksp_image1 )
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) )
start_matching = time.time()
matches = self.tracker.nn_match_two_way( desc0, desc1, self.fe.nn_thresh )
sel_pts0 = np.array([ pts0[:,i] for i in matches[0,:].astype('int32') ]).transpose()
sel_pts1 = np.array([ pts1[:,i] for i in matches[1,:].astype('int32') ]).transpose()
print 'matching dones in %4.2fms' %(1000. * (time.time() - start_matching) )
print 'matches=%d' %( matches.shape[1] )
if matches.shape[1] == 0:
print 'NO MATCHES'
return None, None
# Plotting
# xcanvas0 = self.plot_pts(image0.copy(), pts0 )
# xcanvas1 = self.plot_pts(image1.copy(), pts1 )
# xcanvas_ = self.plot_point_sets( xcanvas0, sel_pts0, xcanvas1, sel_pts1 )
# xcanvas_ = self.plot_point_sets( image0.copy(), sel_pts0, image1.copy(), sel_pts1 )
# cv2.imshow( 'pts0', xcanvas0 )
# cv2.imshow( 'pts1', xcanvas1 )
# cv2.imshow( 'joint', xcanvas_ )
return sel_pts0[0:2,:].transpose() , sel_pts1[0:2,:].transpose()
# code.interact( local=locals() )
if __name__ == '__main__':
BASE = '/Bulk_Data/_tmp_cerebro/bb4_multiple_loops_in_lab/'
#
# Open Log File
#
LOG_FILE_NAME = BASE+'/log.json'
print 'Open file: ', LOG_FILE_NAME
with open(LOG_FILE_NAME) as data_file:
data = json.load(data_file)
#
# Init Keras Model
ksp = KSuperPointExp()
if True:
i = 534
im = cv2.imread( BASE+'%d.jpg' %(i) )
im_float = cv2.cvtColor( cv2.resize(im, (0,0), fx=0.5, fy=0.5), cv2.COLOR_BGR2GRAY ).astype('float32') / 255.
print 'im_float.shape=', im_float.shape, '\tdtype=', im_float.dtype
sup_pts, sup_desc, sup_heatmap = ksp.get_descriptor( im_float )
cv2.imshow( str(i), ksp.plot_pts( im, sup_pts*2 ) )
if True:
i = 1638
im = cv2.imread( BASE+'%d.jpg' %(i) )
im_float = cv2.cvtColor( cv2.resize(im, (0,0), fx=0.5, fy=0.5), cv2.COLOR_BGR2GRAY ).astype('float32') / 255.
print 'im_float.shape=', im_float.shape, '\tdtype=', im_float.dtype
sup_pts, sup_desc, sup_heatmap = ksp.get_descriptor( im_float )
cv2.imshow( str(i), ksp.plot_pts( im, sup_pts*2 ) )
cv2.waitKey(0)
if __name__ == "__m1ain__":
BASE = '/Bulk_Data/_tmp_cerebro/bb4_multiple_loops_in_lab/'
#
# Open Log File
#
LOG_FILE_NAME = BASE+'/log.json'
print 'Open file: ', LOG_FILE_NAME
with open(LOG_FILE_NAME) as data_file:
data = json.load(data_file)
#
# Init Keras Model
ksp = KSuperPointExp()
# Loops over all images and precomputes their netvlad vector
for i in range( len(data['DataNodes']) ):
a = data['DataNodes'][i]['isKeyFrame']
b = data['DataNodes'][i]['isImageAvailable']
c = data['DataNodes'][i]['isPoseAvailable']
d = data['DataNodes'][i]['isPtCldAvailable']
if not ( a==1 and b==1 and c==1 and d==1 ): #only process keyframes which have pose and ptcld info
continue
im = cv2.imread( BASE+'%d.jpg' %(i) )
im_float = cv2.cvtColor( cv2.resize(im, (0,0), fx=0.5, fy=0.5), cv2.COLOR_BGR2GRAY ).astype('float32') / 255.
print 'im_float.shape=', im_float.shape, '\tdtype=', im_float.dtype
start_time = time.time()
sup_pts, sup_desc, sup_heatmap = ksp.get_descriptor( im_float )
print '---', i , '\n',
print 'Done in %4.4fms' %( 1000. * (time.time() - start_time ) )
cv2.imshow( 'im_superpoints', ksp.plot_pts( im, sup_pts*2 ) )
cv2.imshow( 'im', im )
key = cv2.waitKey(10)
if key == ord( 'q' ):
break
================================================
FILE: scripts/whole_image_desc_compute_client.py
================================================
#!/usr/bin/env python
# Sample Client to call the `whole_image_descriptor_compute_server`
from cerebro.srv import *
import rospy
import numpy as np
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
rospy.wait_for_service( 'whole_image_descriptor_compute' )
try:
res = rospy.ServiceProxy( 'whole_image_descriptor_compute', WholeImageDescriptorCompute )
# X = np.zeros( (100, 100), dtype=np.uint8 )
X = cv2.resize( cv2.imread( '/app/lena.jpg' ), (752,480) )
i = CvBridge().cv2_to_imgmsg( X )
u = res( i, 23 )
print 'received: ', u
except rospy.ServiceException, e:
print 'failed', e
================================================
FILE: scripts/whole_image_desc_compute_server.py
================================================
#!/usr/bin/env python
from cerebro.srv import *
import rospy
from cv_bridge import CvBridge, CvBridgeError
import cv2
import numpy as np
import code
import time
import os
import scipy.io
import hashlib
import keras
from keras_helpers import *
from predict_utils import *
from TerminalColors import bcolors
tcol = bcolors()
import rospkg
THIS_PKG_BASE_PATH = rospkg.RosPack().get_path('cerebro')
from numpy.random import seed
seed(42)
class SampleGPUComputer:
def __init__(self):
self.model = keras.applications.vgg16.VGG16( weights=None)
self.model._make_predict_function()
self.model.summary()
def handle_req( self, req ):
print 'Handle request '
# print req
cv_image = CvBridge().imgmsg_to_cv2( req.ima )
print 'cv_image.shape', cv_image.shape
# cv2.imshow( 'cv_image from server', cv_image )
# cv2.waitKey(0)
#
# compute descriptor
#
# code.interact( local=locals() )
im = cv2.resize( cv_image, (224,224) )
self.model._make_predict_function()
preds = self.model.predict( np.expand_dims(im,0).astype('float32') )
print preds
#
# Return the descriptor in the message
#
result = WholeImageDescriptorComputeResponse()
result.desc = [ cv_image.shape[0], cv_image.shape[1] ]
return result
class ReljaNetVLAD:
def __init__(self, im_rows=600, im_cols=960, im_chnls=3):
## Build net
from keras.backend.tensorflow_backend import set_session
import tensorflow as tf
tf.set_random_seed(42)
config = tf.ConfigProto()
config.gpu_options.per_process_gpu_memory_fraction = 0.1
# config.gpu_options.visible_device_list = "0"
set_session(tf.Session(config=config))
self.im_rows = int(im_rows)
self.im_cols = int(im_cols)
self.im_chnls = int(im_chnls)
input_img = keras.layers.Input( batch_shape=(1,self.im_rows, self.im_cols, self.im_chnls ) )
cnn = make_from_vgg16( input_img, weights=None, layer_name='block5_pool' )
out, out_amap = NetVLADLayer(num_clusters = 64)( cnn )
model = keras.models.Model( inputs=input_img, outputs=out )
DATA_DIR = '/app_learning/cartwheel_train/relja_matlab_weight.dump/'
print 'Relja Data Dir: ', DATA_DIR
model.load_weights( DATA_DIR+'/matlab_model.keras' )
WPCA_M = scipy.io.loadmat( DATA_DIR+'/WPCA_1.mat' )['the_mat'] # 1x1x32768x4096
WPCA_b = scipy.io.loadmat( DATA_DIR+'/WPCA_2.mat' )['the_mat'] # 4096x1
WPCA_M = WPCA_M[0,0] # 32768x4096
WPCA_b = np.transpose(WPCA_b) #1x4096
self.model = model
self.WPCA_M = WPCA_M
self.WPCA_b = WPCA_b
self.model_type = 'relja_matlab_model'
# Doing this is a hack to force keras to allocate GPU memory. Don't comment this,
tmp_zer = np.zeros( (1,self.im_rows,self.im_cols,self.im_chnls), dtype='float32' )
tmp_zer_out = self.model.predict( tmp_zer )
tmp_zer_out = np.matmul( tmp_zer_out, self.WPCA_M ) + self.WPCA_b
tmp_zer_out /= np.linalg.norm( tmp_zer_out )
print 'model input.shape=', tmp_zer.shape, '\toutput.shape=', tmp_zer_out.shape
print 'model_type=', self.model_type
print '-----'
print '\tinput_image.shape=', tmp_zer.shape
print '\toutput.shape=', tmp_zer_out.shape
print '\tminmax=', np.min( tmp_zer_out ), np.max( tmp_zer_out )
print '\tnorm=', np.linalg.norm( tmp_zer_out )
print '\tdtype=', tmp_zer_out.dtype
print '-----'
def handle_req( self, req ):
## Get Image out of req
cv_image = CvBridge().imgmsg_to_cv2( req.ima )
print '[Handle Request] cv_image.shape', cv_image.shape, '\ta=', req.a, '\tt=', req.ima.header.stamp
assert (cv_image.shape[0] == self.im_rows and
cv_image.shape[1] == self.im_cols and
cv_image.shape[2] == self.im_chnls),\
"\n[whole_image_descriptor_compute_server] Input shape of the image \
does not match with the allocated GPU memory. Expecting an input image of \
size %dx%dx%d, but received : %s" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )
# cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )
# cv2.waitKey(10)
# cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )
## Compute Descriptor
start_time = time.time()
# Normalize image
cv_image_rgb = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
avg_image = [122.6778, 116.6522, 103.9997]
cv_image_rgb[:,:,0]= cv_image_rgb[:,:,0] - avg_image[0]
cv_image_rgb[:,:,1]= cv_image_rgb[:,:,1] - avg_image[1]
cv_image_rgb[:,:,2]= cv_image_rgb[:,:,2] - avg_image[2]
# predict
u = self.model.predict( np.expand_dims( cv_image_rgb.astype('float32'), 0 ) )
# WPCA
u = np.matmul( u, self.WPCA_M ) + self.WPCA_b
u /= np.linalg.norm( u )
print 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ),
print '\tdesc.shape=', u.shape,
print '\tinput_image.shape=', cv_image.shape,
print '\tminmax=', np.min( u ), np.max( u ),
print '\tmodel_type=', self.model_type,
print '\tdtype=', cv_image.dtype
## Populate output message
result = WholeImageDescriptorComputeResponse()
# result.desc = [ cv_image.shape[0], cv_image.shape[1] ]
result.desc = u[0,:]
result.model_type = self.model_type
return result
class NetVLADImageDescriptor:
def __init__(self, im_rows=600, im_cols=960, im_chnls=3):
## Build net
from keras.backend.tensorflow_backend import set_session
import tensorflow as tf
tf.set_random_seed(42)
config = tf.ConfigProto()
config.gpu_options.per_process_gpu_memory_fraction = 0.1
# config.gpu_options.visible_device_list = "0"
set_session(tf.Session(config=config))
# Blackbox 4
# self.im_rows = 512
# self.im_cols = 640
# self.im_chnls = 3
# point grey
# self.im_rows = 600
# self.im_cols = 960
# self.im_chnls = 3
# EuroC
# self.im_rows = 480
# self.im_cols = 752
# self.im_chnls = 3
self.im_rows = int(im_rows)
self.im_cols = int(im_cols)
self.im_chnls = int(im_chnls)
#----- @ INPUT LAYER
input_img = keras.layers.Input( shape=(self.im_rows, self.im_cols, self.im_chnls) )
#----- @ CNN
# cnn = make_from_vgg16( input_img, weights=None, layer_name='block5_pool' )
cnn = make_from_mobilenet( input_img, weights=None, layer_name='conv_pw_7_relu' )
#----- @ DOWN-SAMPLE LAYER (OPTINAL)
if False: #Downsample last layer (Reduce nChannels of the output.)
cnn_dwn = keras.layers.Conv2D( 256, (1,1), padding='same', activation='relu' )( cnn )
cnn_dwn = keras.layers.normalization.BatchNormalization()( cnn_dwn )
cnn_dwn = keras.layers.Conv2D( 64, (1,1), padding='same', activation='relu' )( cnn_dwn )
cnn_dwn = keras.layers.normalization.BatchNormalization()( cnn_dwn )
cnn = cnn_dwn
#----- @ NetVLADLayer
# out, out_amap = NetVLADLayer(num_clusters = 16)( cnn )
out = NetVLADLayer(num_clusters = 16)( cnn )
model = keras.models.Model( inputs=input_img, outputs=out )
model.summary()
# `model_visual_fname` as None will disable writing the image file.
model_visual_fname = None
# model_visual_fname = '/app/core.png'
if model_visual_fname is not None:
print 'Writing Model Visual to: ', model_visual_fname
keras.utils.plot_model( model, to_file=model_visual_fname, show_shapes=True )
#----- @ Load Model Weights
#TODO Read from config file the path of keras model
# model_file = '/app/catkin_ws/src/cerebro/scripts/keras.models/core_model.keras'
# model_type = 'test_keras_model'
# model_file = '/app_learning/cartwheel_train/models.keras/vgg16/block5_pool_k16_tripletloss2/core_model.keras'
# model_type = 'block5_pool_k16_tripletloss2'
# model_file = '/app_learning/cartwheel_train/models.keras/mobilenet_conv7_quash_chnls_allpairloss/core_model.keras'
# model_type = 'mobilenet_conv7_quash_chnls_allpairloss'
#model_file = '/app_learning/cartwheel_train/models.keras/mobilenet_conv7_allpairloss/core_model.keras'
#model_type = 'mobilenet_conv7_allpairloss'
# model_file = '/app_learning/cartwheel_train/models.keras/mobilenet_new/pw13_quash_chnls_k16_allpairloss/core_model.1800.keras'
# model_type = 'pw13_quash_chnls_k16_allpairloss'
# model_file = '/app_learning/cartwheel_train/models.keras/vgg16_new/block5_pool_k16_tripletloss2/core_model.keras'
# model_type = 'block5_pool_k16_tripletloss2'
# model_file = '/app_learning/cartwheel_train/models.keras/mobilenet_new/pw13_quash_chnls_k16_allpairloss/core_model.800.keras'
# model_type = 'pw13_quash_chnls_k16_allpairloss'
model_file = THIS_PKG_BASE_PATH+'/scripts/keras.models/mobilenet_conv7_allpairloss.keras'
model_type = 'mobilenet_conv7_allpairloss'
print 'model_file: ', model_file
model.load_weights( model_file )
# Write model as json to file, for info.
# if False:
# model.summary()
# keras.utils.plot_model( model, to_file='/tmp/xcore.png', show_shapes=True )
# with open('/tmp/xmodel.json', 'w') as outfile:
# json.dump(model.to_json(), outfile, indent=4 )
self.model = model
self.model_type = model_type
# ! Done...!
# Doing this is a hack to force keras to allocate GPU memory. Don't comment this,
tmp_zer = np.zeros( (1,self.im_rows,self.im_cols,self.im_chnls), dtype='float32' )
tmp_zer_out = self.model.predict( tmp_zer )
print 'model input.shape=', tmp_zer.shape, '\toutput.shape=', tmp_zer_out.shape
print 'model_type=', self.model_type
print '-----'
print '\tinput_image.shape=', tmp_zer.shape
print '\toutput.shape=', tmp_zer_out.shape
print '\tminmax=', np.min( tmp_zer_out ), np.max( tmp_zer_out )
print '\tdtype=', tmp_zer_out.dtype
print '-----'
def handle_req( self, req ):
## Get Image out of req
cv_image = CvBridge().imgmsg_to_cv2( req.ima )
print '[Handle Request] cv_image.shape', cv_image.shape, '\ta=', req.a, '\tt=', req.ima.header.stamp
if len(cv_image.shape) == 2:
cv_image = np.expand_dims( cv_image, -1 );
assert (cv_image.shape[0] == self.im_rows and
cv_image.shape[1] == self.im_cols and
cv_image.shape[2] == self.im_chnls),\
"\n[whole_image_descriptor_compute_server] Input shape of the image \
does not match with the allocated GPU memory. Expecting an input image of \
size %dx%dx%d, but received : %s" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )
# cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )
# cv2.waitKey(10)
# cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )
## Compute Descriptor
start_time = time.time()
u = self.model.predict( np.expand_dims( cv_image.astype('float32'), 0 ) )
print 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ),
print '\tdesc.shape=', u.shape,
print '\tinput_image.shape=', cv_image.shape,
print '\tminmax=', np.min( u ), np.max( u ),
print '\tnorm=', np.linalg.norm(u[0]),
print '\tmodel_type=', self.model_type,
print '\tdtype=', cv_image.dtype
## Populate output message
result = WholeImageDescriptorComputeResponse()
# result.desc = [ cv_image.shape[0], cv_image.shape[1] ]
result.desc = u[0,:]
result.model_type = self.model_type
return result
class JSONModelImageDescriptor:
"""
This class loads the net structure from the json file, model.json and
weights from core_model.??.keras. In the argument `kerasmodel_file`
you need to specify the core_model.??.keras full path (keras model file).
The model.json need to exist in that folder.
"""
def __init__(self, kerasmodel_file, im_rows=600, im_cols=960, im_chnls=3):
## Build net
from keras.backend.tensorflow_backend import set_session
import tensorflow as tf
tf.set_random_seed(42)
config = tf.ConfigProto()
config.gpu_options.per_process_gpu_memory_fraction = 0.2
# config.gpu_options.visible_device_list = "0"
set_session(tf.Session(config=config))
# Blackbox 4
# self.im_rows = 512
# self.im_cols = 640
# self.im_chnls = 3
# point grey
# self.im_rows = 600
# self.im_cols = 960
# self.im_chnls = 3
# EuroC
# self.im_rows = 480
# self.im_cols = 752
# self.im_chnls = 3
self.im_rows = int(im_rows)
self.im_cols = int(im_cols)
self.im_chnls = int(im_chnls)
LOG_DIR = '/'.join( kerasmodel_file.split('/')[0:-1] )
print '+++++++++++++++++++++++++++++++++++++++++++++++++++++++'
print '++++++++++ (JSONModelImageDescriptor) LOG_DIR=', LOG_DIR
print '++++++++++ im_rows=', im_rows, ' im_cols=', im_cols, ' im_chnls=', im_chnls
print '+++++++++++++++++++++++++++++++++++++++++++++++++++++++'
model_type = LOG_DIR.split('/')[-1]
# code.interact( local=locals() )
assert os.path.isdir( LOG_DIR ), "The LOG_DIR doesnot exist, or there is a permission issue. LOG_DIR="+LOG_DIR
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'
#----- @ Load Model Structure from JSON
# Load JSON formatted model
json_string = open_json_file( LOG_DIR+'/model.json' )
# print '======================='
# pprint.pprint( json_string, indent=4 )
# print '======================='
model = keras.models.model_from_json(str(json_string), custom_objects={'NetVLADLayer': NetVLADLayer, 'GhostVLADLayer':GhostVLADLayer} )
old_input_shape = model._layers[0].input_shape
model._layers[0]
print 'OLD MODEL: ', 'input_shape=', str(old_input_shape)
model.summary()
model_visual_fname = None
# model_visual_fname = '/app/core.png'
if model_visual_fname is not None:
print 'Writing Model Visual to: ', model_visual_fname
keras.utils.plot_model( model, to_file=model_visual_fname, show_shapes=True )
#----- @ Load Weights
assert os.path.isfile( kerasmodel_file ), 'The model weights file doesnot exists or there is a permission issue.'+"kerasmodel_file="+kerasmodel_file
model_fname = kerasmodel_file
print tcol.OKGREEN, 'Load model: ', model_fname, tcol.ENDC
model.load_weights( model_fname )
# Replace Input Layer
new_model = change_model_inputshape( model, new_input_shape=(1,self.im_rows,self.im_cols,self.im_chnls) )
new_input_shape = new_model._layers[0].input_shape
print 'OLD MODEL: ', 'input_shape=', str(old_input_shape)
print 'NEW MODEL: input_shape=', str(new_input_shape)
self.model = new_model
self.model_type = model_type
# Doing this is a hack to force keras to allocate GPU memory. Don't comment this,
tmp_zer = np.zeros( (1,self.im_rows,self.im_cols,self.im_chnls), dtype='float32' )
tmp_zer_out = self.model.predict( tmp_zer )
print 'model input.shape=', tmp_zer.shape, '\toutput.shape=', tmp_zer_out.shape
print 'model_type=', self.model_type
print '-----'
print '\tinput_image.shape=', tmp_zer.shape
print '\toutput.shape=', tmp_zer_out.shape
print '\tminmax=', np.min( tmp_zer_out ), np.max( tmp_zer_out )
print '\tnorm=', np.linalg.norm( tmp_zer_out )
print '\tdtype=', tmp_zer_out.dtype
print '-----'
def handle_req( self, req ):
""" The received image from CV bridge has to be [0,255]. In function makes it to
intensity range [-0.5 to 0.5]
"""
## Get Image out of req
cv_image = CvBridge().imgmsg_to_cv2( req.ima )
print '[Handle Request] cv_image.shape', cv_image.shape, '\ta=', req.a, '\tt=', req.ima.header.stamp
if len(cv_image.shape)==2:
# print 'Input dimensions are NxM but I am expecting it to be NxMxC, so np.expand_dims'
cv_image = np.expand_dims( cv_image, -1 )
elif len( cv_image.shape )==3:
pass
else:
assert( False )
assert (cv_image.shape[0] == self.im_rows and
cv_image.shape[1] == self.im_cols and
cv_image.shape[2] == self.im_chnls),\
"\n[whole_image_descriptor_compute_server] Input shape of the image \
does not match with the allocated GPU memory. Expecting an input image of \
size %dx%dx%d, but received : %s" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )
# cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )
# cv2.waitKey(10)
# cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )
## Compute Descriptor
start_time = time.time()
# i__image = np.expand_dims( cv_image.astype('float32'), 0 )
# i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)/255. [-0.5,0.5]
i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)*2.0/255. #[-1,1]
u = self.model.predict( i__image )
print tcol.HEADER, 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ), tcol.ENDC
print '\tinput_image.shape=', cv_image.shape,
print '\tinput_image dtype=', cv_image.dtype
print tcol.OKBLUE, '\tinput image (to neuralnet) minmax=', np.min( i__image ), np.max( i__image ), tcol.ENDC
print '\tdesc.shape=', u.shape,
print '\tdesc minmax=', np.min( u ), np.max( u ),
print '\tnorm=', np.linalg.norm(u[0])
print '\tmodel_type=', self.model_type
## Populate output message
result = WholeImageDescriptorComputeResponse()
# result.desc = [ cv_image.shape[0], cv_image.shape[1] ]
result.desc = u[0,:]
result.model_type = self.model_type
return result
class HDF5ModelImageDescriptor:
"""
This class loads the net structure from the .h5 file. This file contains
the model weights as well as architecture details.
In the argument `kerasmodel_file`
you need to specify the core_model.??.keras full path (keras model file).
"""
def __init__(self, kerasmodel_file, im_rows=600, im_cols=960, im_chnls=3):
## Build net
from keras.backend.tensorflow_backend import set_session
import tensorflow as tf
tf.set_random_seed(42)
config = tf.ConfigProto()
config.gpu_options.per_process_gpu_memory_fraction = 0.2
# config.gpu_options.visible_device_list = "0"
set_session(tf.Session(config=config))
keras.backend.tensorflow_backend.set_learning_phase(0)
self.request_count = 0
# Blackbox 4
# self.im_rows = 512
# self.im_cols = 640
# self.im_chnls = 3
# point grey
# self.im_rows = 600
# self.im_cols = 960
# self.im_chnls = 3
# EuroC
# self.im_rows = 480
# self.im_cols = 752
# self.im_chnls = 3
self.im_rows = int(im_rows)
self.im_cols = int(im_cols)
self.im_chnls = int(im_chnls)
LOG_DIR = '/'.join( kerasmodel_file.split('/')[0:-1] )
print '+++++++++++++++++++++++++++++++++++++++++++++++++++++++'
print '++++++++++ (HDF5ModelImageDescriptor) LOG_DIR=', LOG_DIR
print '++++++++++ im_rows=', im_rows, ' im_cols=', im_cols, ' im_chnls=', im_chnls
print '+++++++++++++++++++++++++++++++++++++++++++++++++++++++'
model_type = LOG_DIR.split('/')[-1]
assert os.path.isdir( LOG_DIR ), "The LOG_DIR doesnot exist, or there is a permission issue. LOG_DIR="+LOG_DIR
# See if cached file exists, if yes load it else do as usual
digest = '/tmp/'+str(hashlib.sha256(kerasmodel_file).hexdigest())+'.h5'
print 'Check for exisitance of file', digest
if os.path.isfile(digest):
print 'The cached file, ', digest, ' seems to exists, load that file'
print tcol.OKGREEN, 'Load model (from cache): ', digest, tcol.ENDC
model = keras.models.load_model( digest, custom_objects={'NetVLADLayer': NetVLADLayer, 'GhostVLADLayer':GhostVLADLayer} )
model.summary()
print tcol.OKGREEN, 'Load model (from cache): ', digest, tcol.ENDC
self.model = model
self.model_type = model_type
else:
# Load from HDF5
assert os.path.isfile( kerasmodel_file ), 'The model weights file doesnot exists or there is a permission issue.'+"kerasmodel_file="+kerasmodel_file
model_fname = kerasmodel_file
print tcol.OKGREEN, 'Load model: ', model_fname, tcol.ENDC
model = keras.models.load_model( model_fname, custom_objects={'NetVLADLayer': NetVLADLayer, 'GhostVLADLayer':GhostVLADLayer} )
old_input_shape = model._layers[0].input_shape
print 'OLD MODEL: ', 'input_shape=', str(old_input_shape)
model.summary()
model_visual_fname = None
if model_visual_fname is not None:
print 'Writing Model Visual to: ', model_visual_fname
keras.utils.plot_model( model, to_file=model_visual_fname, show_shapes=True )
# Replace Input Layer
new_model = change_model_inputshape( model, new_input_shape=(1,self.im_rows,self.im_cols,self.im_chnls) )
new_input_shape = new_model._layers[0].input_shape
print 'OLD MODEL: ', 'input_shape=', str(old_input_shape)
print 'NEW MODEL: input_shape=', str(new_input_shape)
self.model = new_model
self.model_type = model_type
# Write to cache
if True:
digest = '/tmp/'+str(hashlib.sha256(kerasmodel_file).hexdigest())+'.h5'
print tcol.WARNING, '}}}}}}}}\nWrite out cache in '+digest+'\n}}}}}}}}', tcol.ENDC
self.model.save( digest )
# Doing this is a hack to force keras to allocate GPU memory. Don't comment this,
tmp_zer = np.zeros( (1,self.im_rows,self.im_cols,self.im_chnls), dtype='float32' )
tmp_zer_out = self.model.predict( tmp_zer )
print 'model input.shape=', tmp_zer.shape, '\toutput.shape=', tmp_zer_out.shape
print 'model_type=', self.model_type
print '-----'
print '\tinput_image.shape=', tmp_zer.shape
print '\toutput.shape=', tmp_zer_out.shape
print '\tminmax=', np.min( tmp_zer_out ), np.max( tmp_zer_out )
print '\tnorm=', np.linalg.norm( tmp_zer_out )
print '\tdtype=', tmp_zer_out.dtype
print '-----'
def handle_req( self, req ):
""" The received image from CV bridge has to be [0,255]. In function makes it to
intensity range [-0.5 to 0.5]
"""
## Get Image out of req
cv_image = CvBridge().imgmsg_to_cv2( req.ima )
print '[HDF5ModelImageDescriptor Handle Request#%d] cv_image.shape' %(self.request_count), cv_image.shape, '\ta=', req.a, '\tt=', req.ima.header.stamp
if len(cv_image.shape)==2:
# print 'Input dimensions are NxM but I am expecting it to be NxMxC, so np.expand_dims'
cv_image = np.expand_dims( cv_image, -1 )
elif len( cv_image.shape )==3:
pass
else:
assert( False )
assert (cv_image.shape[0] == self.im_rows and
cv_image.shape[1] == self.im_cols and
cv_image.shape[2] == self.im_chnls),\
"\n[whole_image_descriptor_compute_server] Input shape of the image \
does not match with the allocated GPU memory. Expecting an input image of \
size %dx%dx%d, but received : %s" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )
# cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )
# cv2.waitKey(10)
# cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )
## Compute Descriptor
start_time = time.time()
# i__image = np.expand_dims( cv_image.astype('float32'), 0 )
# i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)/255. [-0.5,0.5]
i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)*2.0/255. #[-1,1]
u = self.model.predict( i__image )
print tcol.HEADER, 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ), tcol.ENDC
print '\tinput_image.shape=', cv_image.shape,
print '\tinput_image dtype=', cv_image.dtype
print tcol.OKBLUE, '\tinput image (to neuralnet) minmax=', np.min( i__image ), np.max( i__image ), tcol.ENDC
print '\tdesc.shape=', u.shape,
print '\tdesc minmax=', np.min( u ), np.max( u ),
print '\tnorm=', np.linalg.norm(u[0])
print '\tmodel_type=', self.model_type
self.request_count += 1
## Populate output message
result = WholeImageDescriptorComputeResponse()
# result.desc = [ cv_image.shape[0], cv_image.shape[1] ]
result.desc = u[0,:]
result.model_type = self.model_type
return result
rospy.init_node( 'whole_image_descriptor_compute_server' )
##
## Load the config file and read image row, col
##
fs_image_width = -1
fs_image_height = -1
fs_image_chnls = 1
if True: # read from param `config_file`
if not rospy.has_param( '~config_file'):
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'
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' )
if ( rospy.has_param( '~nrows') and rospy.has_param( '~ncols') ):
print tcol.OKGREEN, 'However, you seem to have set the parameters nrows and ncols, so will read those.', tcol.ENDC
else:
quit()
# quit only if you cannot see nrows and ncols
else:
config_file = rospy.get_param('~config_file')
print '++++++++\n++++++++ config_file: ', config_file
if not os.path.isfile(config_file):
print 'FATAL...cannot find config_file: ', config_file
rospy.logerr( '[whole_image_descriptor_compute_server]FATAL...cannot find config_file: '+ config_file )
quit()
print '++++++++ READ opencv-yaml file: ', config_file
fs = cv2.FileStorage(config_file, cv2.FILE_STORAGE_READ)
fs_image_width = int( fs.getNode( "image_width" ).real() )
fs_image_height = int( fs.getNode( "image_height" ).real() )
print '++++++++ opencv-yaml:: image_width=', fs_image_width, ' image_height=', fs_image_height
print '++++++++'
##
## Load nrows and ncols directly as config
##
if True: # read from param `nrows` and `ncols`
if fs_image_width < 0 :
if ( not rospy.has_param( '~nrows') or not rospy.has_param( '~ncols') or not rospy.has_param( '~nchnls') ):
print 'FATAL...cannot find param either of ~nrows, ~ncols, ~nchnls. This is needed to determine size of the input image to allocate GPU memory'
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' )
quit()
else:
fs_image_height = rospy.get_param('~nrows')
fs_image_width = rospy.get_param('~ncols')
fs_image_chnls = rospy.get_param('~nchnls')
print '~~~~~~~~~~~~~~~~'
print '~nrows = ', fs_image_height, '\t~ncols = ', fs_image_width, '\t~nchnls = ', fs_image_chnls
print '~~~~~~~~~~~~~~~~'
##
## Load Channels
##
if True:
if not rospy.has_param( '~nchnls' ):
rospy.logerr( "[whole_image_descriptor_compute_server] FATAL....cannot file cmd param nchnls.")
quit()
else:
fs_image_chnls = rospy.get_param('~nchnls')
##
## Start Server
##
# Something wrong with jsonfile loading or with change_model_inputshape().
if True:
# kerasmodel_file = '/models.keras/Apr2019/gray_conv6_K16Ghost1__centeredinput/core_model.%d.keras' %(500)
if rospy.has_param( '~kerasmodel_file'):
kerasmodel_file = rospy.get_param('~kerasmodel_file')
else:
print tcol.ERROR, 'FATAL...missing specification of model file. You need to specify ~kerasmodel_file', tcol.ENDC
quit()
# gpu_netvlad = JSONModelImageDescriptor( kerasmodel_file=kerasmodel_file, im_rows=fs_image_height, im_cols=fs_image_width, im_chnls=fs_image_chnls )
gpu_netvlad = HDF5ModelImageDescriptor( kerasmodel_file=kerasmodel_file, im_rows=fs_image_height, im_cols=fs_image_width, im_chnls=fs_image_chnls )
else:
#gpu_s = SampleGPUComputer()
gpu_netvlad = NetVLADImageDescriptor( im_rows=fs_image_height, im_cols=fs_image_width )
# gpu_netvlad = ReljaNetVLAD( im_rows=fs_image_height, im_cols=fs_image_width )
s = rospy.Service( 'whole_image_descriptor_compute', WholeImageDescriptorCompute, gpu_netvlad.handle_req )
print 'whole_image_descriptor_compute_server is running'
# image = cv2.resize( cv2.imread( '/app/lena.jpg'), (224,224) )
# print gpu_s.model.predict( np.expand_dims(image,0) )
rospy.spin()
================================================
FILE: src/Cerebro.cpp
================================================
#include "Cerebro.h"
//-------------------------------------------------------------//
//--- Setup Cerebro
//-------------------------------------------------------------//
Cerebro::Cerebro( ros::NodeHandle& nh )
{
b_run_thread = false;
b_descriptor_computer_thread = false;
this->nh = nh;
connected_to_descriptor_server = false;
descriptor_size_available = false;
}
void Cerebro::setDataManager( DataManager* dataManager )
{
this->dataManager = dataManager;
m_dataManager_available = true;
}
void Cerebro::setPublishers( const string base_topic_name )
{
string pub_loopedge_topic = base_topic_name+"/loopedge";
ROS_INFO( "[Cerebro::setPublishers] Publish %s", pub_loopedge_topic.c_str() );
pub_loopedge = nh.advertise( pub_loopedge_topic, 1000 );
m_pub_available = true;
}
//-------------------------------------------------------------//
//--- END Setup Cerebro
//-------------------------------------------------------------//
//------------------------------------------------------------------//
// per Keyframe Descriptor Computation
// Can have more threads to compute other aspects for keyframes
// like texts, objects visible etc. TODO
//------------------------------------------------------------------//
#define __Cerebro__descriptor_computer_thread( msg ) ;
// #define __Cerebro__descriptor_computer_thread( msg ) msg
#define __Cerebro__descriptor_computer_thread__imp( msg ) ;
// #define __Cerebro__descriptor_computer_thread__imp( msg ) msg;
void Cerebro::descriptor_computer_thread()
{
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");
assert( b_descriptor_computer_thread && "You need to call descriptor_computer_thread_enable() before spawning the thread\n" );
//---------
// Send a zeros image to the server just to know the descriptor size
int nrows=-1, ncols=-1, desc_size=-1;
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. ****
//----------
// Service Call
// Sample Code : https://github.com/mpkuse/cerebro/blob/master/src/unittest/unittest_rosservice_client.cpp
connected_to_descriptor_server = false;
descriptor_size_available = false;
int n_sec_wait_for_connection = 71;
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();
ros::ServiceClient client = nh.serviceClient( "/whole_image_descriptor_compute" );
client.waitForExistence( ros::Duration(n_sec_wait_for_connection, 0) ); //wait maximum 10 sec
if( !client.exists() ) {
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 );
return;
}
else std::cout << TermColor::GREEN() << "Connection to ros-service ``" << client.getService() << "`` established" << TermColor::RESET() << endl;
connected_to_descriptor_server = true;
ElapsedTime _est_desc_compute_time_ms;
{
auto _abs_cam = dataManager->getAbstractCameraRef();
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");
nrows = _abs_cam->imageHeight();
ncols = _abs_cam->imageWidth();
cv::Mat zero_image;
if( nChannels == 3 )
zero_image = cv::Mat::zeros( nrows, ncols, CV_8UC3 );
else if( nChannels == 1 )
zero_image = cv::Mat::zeros( nrows, ncols, CV_8UC1 );
else {
assert( false && "Invalid number of channels specified in Cerebro::descriptor_computer_thread() ");
cout << TermColor::RED() << "[ERROR] Ax Invalid number of channels specified in Cerebro::descriptor_computer_thread() " << endl << TermColor::RESET();
exit(3);
}
// create zero image sensor_msgs::Image
sensor_msgs::ImagePtr image_msg;
if( nChannels == 3 )
image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", zero_image).toImageMsg();
else if( nChannels == 1 )
image_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", zero_image).toImageMsg();
else {
assert( false && "Invalid number of channels specified in Cerebro::descriptor_computer_thread() ");
cout << TermColor::RED() << "[ERROR] Bx Invalid number of channels specified in Cerebro::descriptor_computer_thread() " << endl << TermColor::RESET();
exit(3);
}
image_msg->header.stamp = ros::Time::now();
cerebro::WholeImageDescriptorCompute srv; //service message
srv.request.ima = *image_msg;
srv.request.a = 986;
// make request to server
_est_desc_compute_time_ms.tic();
if( client.call( srv ) ) {
__Cerebro__descriptor_computer_thread(std::cout << "Received response from server\t";)
__Cerebro__descriptor_computer_thread(std::cout << "desc.size=" << srv.response.desc.size() << std::endl;)
assert( srv.response.desc.size() > 0 && "The received descriptor appear to be of zero length. This is a fatal error.\n" );
desc_size = int( srv.response.desc.size() );
}
}
assert( nrows > 0 && ncols > 0 && desc_size > 0 );
int estimated_descriptor_compute_time_ms = _est_desc_compute_time_ms.toc_milli();
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;
this->descriptor_size = int(desc_size);
descriptor_size_available = true;
ros::Rate rate(20);
auto data_map = dataManager->getDataMapRef();
auto img_data_mgr = dataManager->getImageManagerRef();
#if 1
//*****************
// If loadStateFromDisk, ie. if data_map already has a lot of items means that the
// state was preloaded. in this case just setup this thread appropriately.
//**************************
if( data_map->begin() == data_map->end() )
{
// This means the data_map is empty, which inturn means fresh run
cout << TermColor::iBLUE() << "[Cerebro::descriptor_computer_thread] Looks like data_map is empty which means this is a fresh run" << TermColor::RESET() << endl;;
}
else {
// This means, state was preloaded from file
cout << TermColor::iGREEN() << "[Cerebro::descriptor_computer_thread] Looks like data_map is NOT empty which means state was loaded from disk" << TermColor::RESET() << endl;;
// loop over data_map and make a note of all the timestamps where descriptor is available.
int n_whlimgdescavai = 0;
// img_data_mgr->print_status();
for( auto itd=data_map->begin() ; itd!=data_map->end() ; itd++ )
{
if( itd->second->isWholeImageDescriptorAvailable() ) {
n_whlimgdescavai++;
wholeImageComputedList_pushback( itd->first );
// Also make sure these images are rechable.
bool kxxx = img_data_mgr->isImageRetrivable( "left_image", itd->first );
if( kxxx == false ) // this is bad
{
cout << "[Cerebro::descriptor_computer_thread] THIS IS BAD. REPORT IT TO AUTHORS WITH CODE jewbcjsmn\nEXIT.....\n";
exit(1);
}
}
}
cout << "[Cerebro::descriptor_computer_thread]i find "<< n_whlimgdescavai << " datanodes where image descrptor is available and images are retrivable through the ImageDataManager\n";
}
#endif
ros::Time last_proc_timestamp =ros::Time();
int n_computed=0;
ElapsedTime _time;
int incoming_diff_ms = 0;
while( b_descriptor_computer_thread )
{
if( data_map->begin() == data_map->end() ) {
__Cerebro__descriptor_computer_thread( cout << "nothing to compute descriptor\n" );
std::this_thread::sleep_for( std::chrono::milliseconds( 1000 ) );
continue;
}
ros::Time lb = data_map->rbegin()->first - ros::Duration(10, 0); // look at recent 10sec.
auto S = data_map->lower_bound( lb );
auto E = data_map->end();
__Cerebro__descriptor_computer_thread(cout << "[Cerebro::descriptor_computer_thread]] S=" << S->first << " E=" << (data_map->rbegin())->first << endl;)
for( auto it = S; it != E ; it++ ) {
// cout << "[Cerebro::descriptor_computer_thread]try : " << it->first << "\t";
// cout << "isWholeImageDescriptorAvailable=" << it->second->isWholeImageDescriptorAvailable() << "\t";
// cout << "isKeyFrame=" << it->second->isKeyFrame() << "\t";
// cout << endl;
//descriptor does not exisit at this stamp, so compute it.
// Here I compute the whole image descriptor only at keyframes, you may try something like, if the pose is available compute it.
if( it->second->isWholeImageDescriptorAvailable() == false && it->second->isKeyFrame() && it->first > last_proc_timestamp )
{
__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_computed++;
incoming_diff_ms = (it->first - last_proc_timestamp).toSec() * 1000. ;
float skip_frac = 1.0 - incoming_diff_ms/float(estimated_descriptor_compute_time_ms) ;
__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; )
last_proc_timestamp = it->first;
// if( n_computed%2 == 0 ) // simple skip
if( n_computed>4 && ( rand()/ float(RAND_MAX) ) < skip_frac ) // dynamic skip
{
__Cerebro__descriptor_computer_thread( cout << "\t\t...SKIP...\n"; )
continue;
}
if( it->second->getNumberOfSuccessfullyTrackedFeatures() < 20 )
{
__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" ; )
continue;
}
_time.tic();
// use it->second->getImage() to compute descriptor. call the service
cv::Mat image_curr;
#if 0 // set this to 1 to get image from data_map (deprecated way), 0 to get image from image_manager.
// old code in which images were stored in nodes
image_curr = it->second->getImage();
#else
// using image data manager
{ //dont remove these braces, it is used for scoping and automatic deallocation
cv::Mat tmp_img;
bool getimstatus = img_data_mgr->getImage( "left_image", it->first, tmp_img );
if( getimstatus == false ) {
__Cerebro__descriptor_computer_thread( cout << "since I cannot getImage, goto hurr_here\n"; );
goto huur_here;
}
if( nChannels == 3 && tmp_img.channels() == 1 )
cv::cvtColor( tmp_img, image_curr, CV_GRAY2BGR );
else if( nChannels==1 && tmp_img.channels() == 3)
cv::cvtColor( tmp_img, image_curr, CV_BGR2GRAY );
else
image_curr = tmp_img;
}
__Cerebro__descriptor_computer_thread(
cout << "image from mgr info: " << MiscUtils::cvmat_info(image_curr) << endl;
cout << "nChannels=" << nChannels << endl;
)
#endif
sensor_msgs::ImagePtr image_msg;
if( nChannels == 3 ) {
image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_curr).toImageMsg();
}
else if( nChannels == 1 ) {
// cout << "cv_bridge mono8 encoding\n";
image_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", image_curr).toImageMsg();
}
else {
assert( false && "Invalid number of channels specified in Cerebro::descriptor_computer_thread() ");
cout << TermColor::RED() << "[ERROR] Cx Invalid number of channels specified in Cerebro::descriptor_computer_thread() " << endl << TermColor::RESET();
exit(3);
}
image_msg->header.stamp = ros::Time( it->first );
cerebro::WholeImageDescriptorCompute srv; //service message
srv.request.ima = *image_msg;
srv.request.a = 986;
if( client.call( srv ) ) {
// __Cerebro__descriptor_computer_thread(std::cout << "Received response from server\t";)
// __Cerebro__descriptor_computer_thread(std::cout << "desc.size=" << srv.response.desc.size() << std::endl;)
assert( srv.response.desc.size() > 0 && "The received descriptor appear to be of zero length. This is a fatal error.\n" );
VectorXd vec( srv.response.desc.size() ); // allocated a vector
for( int j=0 ; jsecond->setWholeImageDescriptor( vec );
wholeImageComputedList_pushback( it->first );
// __Cerebro__descriptor_computer_thread(cout << "Computed descriptor at t=" << it->first - dataManager->getPose0Stamp() << "\t" << it->first << endl;)
// __Cerebro__descriptor_computer_thread(std::cout << "Computed descriptor in (millisec) = " << _time.toc_milli() << endl;)
// __Cerebro__descriptor_computer_thread__imp(cout << TermColor::CYAN() << "Computed descriptor at t=" << it->first - dataManager->getPose0Stamp() << "\t" << it->first << TermColor::RESET() << endl);
estimated_descriptor_compute_time_ms = _time.toc_milli();
__Cerebro__descriptor_computer_thread__imp(
cout << "Computed descriptor at t=" << it->first << " of dim=" << srv.response.desc.size() << " in millisec=" << estimated_descriptor_compute_time_ms << endl;
)
}
else {
ROS_ERROR( "Failed to call ros service" );
}
}
huur_here:
int ddddd=0; //this is here for the goto statement.
}
rate.sleep();
}
cout << "[Cerebro::descriptor_computer_thread] Finished\n";
}
// these both functions are newly added. There are still some raw access
// limited only to the function ``descrip_N__dot__descrip_0_N()``.
const int Cerebro::wholeImageComputedList_size() const {
std::lock_guard lk(m_wholeImageComputedList);
return wholeImageComputedList.size();
}
const ros::Time Cerebro::wholeImageComputedList_at(int k) const
{
std::lock_guard lk(m_wholeImageComputedList);
assert( k>=0 && k lk(m_wholeImageComputedList);
this->wholeImageComputedList.push_back( __tx ); // note down where it was computed.
}
//------------------------------------------------------------------//
// END per Keyframe Descriptor Computation
//------------------------------------------------------------------//
//------------------------------------------------------------------//
//---------------- Populate Loop Candidates --------------------//
//------------------------------------------------------------------//
// #define __Cerebro__run__( msg ) msg ;
#define __Cerebro__run__( msg ) ;
// #define __Cerebro__run__debug( msg ) msg ;
#define __Cerebro__run__debug( msg ) ;
/// TODO: In the future more intelligent schemes can be experimented with. Besure to run those in new threads and disable this thread.
/// wholeImageComputedList is a list for which descriptors are computed. Similarly other threads can compute
/// scene-object labels, text etc etc in addition to currently computed whole-image-descriptor
void Cerebro::run()
{
descrip_N__dot__descrip_0_N();
// faiss__naive_loopcandidate_generator();
// faiss_clique_loopcandidate_generator();
// faiss_multihypothesis_tracking();
}
#ifdef HAVE_FAISS
///-----------------------------------------------------------------
/// Nearest neighbors search using facebook research's faiss library's IndexFlatIP
/// Roughly follows : https://github.com/mpkuse/vins_mono_debug_pkg/blob/master/src_place_recog/faiss_try1.py
/// This function is similar in functionality to `descrip_N__dot__descrip_0_N`
/// but using faiss
void Cerebro::faiss__naive_loopcandidate_generator()
{
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");
assert( b_run_thread && "you need to call run_thread_enable() before run() can start executing\n" );
//-----------------//
//---- Settings ---//
//-----------------//
const int start_adding_descriptors_to_index_after = 150;
const int LOCALITY_THRESH = 12;
const float DOT_PROD_THRESH = 0.9;
//------ END -----//
// wait until connected_to_descriptor_server=true and descriptor_size_available=true
if( wait_until__connectedToDescServer_and_descSizeAvailable( 71 ) == false ) {
cout << TermColor::RED() << "[Cerebro::faiss__naive_loopcandidate_generator ERROR] wait_until__connectedToDescServer_and_descSizeAvailable returned false implying a timeout.\n" << TermColor::RESET();
return;
}
__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; )
assert( this->descriptor_size> 0 );
cout << TermColor::GREEN() << "init a faiss::IndexFlatIP("<< this->descriptor_size << ")" << TermColor::RESET() << endl;
faiss::IndexFlatIP index(this->descriptor_size);
ros::Rate rate(10);
int l=0, last_l=0;
int l_last_added_to_index = 0;
auto data_map = dataManager->getDataMapRef();
while( b_run_thread )
{
l=wholeImageComputedList_size();
if( l - last_l < 3 ) {
__Cerebro__run__debug( cout << "[Cerebro::faiss__naive_loopcandidate_generator]nothing new\n"; );
rate.sleep();
continue;
}
__Cerebro__run__( cout << TermColor::RED() << "---" << TermColor::RESET() << endl; )
__Cerebro__run__( cout << "l=" << l << endl; )
__Cerebro__run__debug( cout << "[Cerebro::faiss__naive_loopcandidate_generator] data_map.size() = " << data_map->size() << "\tper_image_descriptor_size=" << this->descriptor_size << endl; )
// add
__Cerebro__run__(cout << TermColor::YELLOW();)
if( l> start_adding_descriptors_to_index_after ) {
for( int j=l_last_added_to_index ; jat( wholeImageComputedList_at( j ) )->getWholeImageDescriptor();
VectorXf X_float = X.cast();
float * X_raw = X_float.data();
#if 0 //see if the type casting was correct.
for( int g=0;g<5;g++ ) {
// cout << "(" << X(g) << "," << X_float(g) << ") ";
cout << "(" << X(g) << "," << X_raw[g] << ") ";
}
cout << endl;
#endif
index.add( 1, X_raw ); //-TODO
}
l_last_added_to_index = l-start_adding_descriptors_to_index_after;
} else {
__Cerebro__run__(cout << "not seen enough. so,, add nothing. Will start adding after "<< start_adding_descriptors_to_index_after<< " descriptors\n";)
}
__Cerebro__run__(cout << TermColor::RESET();)
// search
vector tmp_;
vector tmp_i;
for( int l_i=last_l ; l_iat( wholeImageComputedList_at( l_i ) )->getWholeImageDescriptor();
VectorXf X_float = X.cast();
float * X_raw = X_float.data();
float distances[5];
faiss::Index::idx_t labels[5];
ElapsedTime time_to_search = ElapsedTime();
index.search( 1, X_raw, 5, distances, labels ); //TODO
__Cerebro__run__(cout << "search done in (ms): " << time_to_search.toc_milli() << endl;)
for( int g=0 ; g<5; g++ ) {
__Cerebro__run__(
// cout << g << " labels=" << labels[g] << " distances=" << distances[g] << endl;
// cout << l_i << "<--("<< distances[g] << ")-->" << labels[g] << "\t";
printf( "%4d<--(%4.2f)-->%d\t", l_i, distances[g], labels[g] );
)
}
__Cerebro__run__(cout << endl;)
__Cerebro__run__(cout << TermColor::RESET();)
tmp_.push_back( distances[0] );
tmp_i.push_back( labels[0] );
}
int _n = tmp_.size();
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 )
{
__Cerebro__run__(
cout << TermColor::RED() << "loop found" << l-1 << "<--->" << tmp_i[2] << TermColor::RESET();
)
{
std::lock_guard lk_foundloops(m_foundLoops);
foundLoops.push_back( std::make_tuple( wholeImageComputedList_at(l-1), wholeImageComputedList_at(tmp_i[2]), tmp_[2] ) );
}
}
last_l = l;
rate.sleep();
}
cout << "[Cerebro::faiss__naive_loopcandidate_generator()] Finished\n";
}
// #define __faiss_clique_loopcandidate_generator__imp(msg) msg;
#define __faiss_clique_loopcandidate_generator__imp(msg) ;
// #define __faiss_clique_loopcandidate_generator__debug(msg) msg;
#define __faiss_clique_loopcandidate_generator__debug(msg) ;
// #define __faiss_clique_loopcandidate_generator__addtoindex(msg) msg;
#define __faiss_clique_loopcandidate_generator__addtoindex(msg) ;
#define __faiss_clique_loopcandidate_generator__search(msg) msg;
// #define __faiss_clique_loopcandidate_generator__search(msg) ;
void Cerebro::faiss_clique_loopcandidate_generator()
{
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");
assert( b_run_thread && "you need to call run_thread_enable() before run() can start executing\n" );
//-----------------//
//---- Settings ---//
//-----------------//
const int start_adding_descriptors_to_index_after = 150;
const int K_NEAREST_NEIGHBOURS=5;
const double DOT_PROD_THRESH = 0.85; //0.85
const int LOCALITY = 7;//7
const int reset_accumulation_every_n_frames = 4; //4
// wait until connected_to_descriptor_server=true and descriptor_size_available=true
if( wait_until__connectedToDescServer_and_descSizeAvailable( 71 ) == false ) {
cout << TermColor::RED() << "[Cerebro::faiss__naive_loopcandidate_generator ERROR] wait_until__connectedToDescServer_and_descSizeAvailable returned false implying a timeout.\n" << TermColor::RESET();
return;
}
__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; )
assert( this->descriptor_size> 0 );
// init faiss index
__faiss_clique_loopcandidate_generator__imp(
cout << TermColor::GREEN() << "init a faiss::IndexFlatIP("<< this->descriptor_size << ")" << TermColor::RESET() << endl;
)
faiss::IndexFlatIP index(this->descriptor_size);
ros::Rate rate(10);
auto data_map = dataManager->getDataMapRef();
int l=0, last_l=0, l_last_added_to_index=0;
std::map< int, std::map< int, float> > data_strc;
float * distances = new float[K_NEAREST_NEIGHBOURS];
faiss::Index::idx_t * labels = new faiss::Index::idx_t[K_NEAREST_NEIGHBOURS];
map retained; //< Accumulation map. reset every say 3 indices. tune this as per amount of computation available
while( b_run_thread ) {
l = wholeImageComputedList_size();
if( l<= last_l ) {
__faiss_clique_loopcandidate_generator__debug( cout << "[Cerebro::faiss_clique_loopcandidate_generator] nothing new\n";)
rate.sleep();
continue;
}
__faiss_clique_loopcandidate_generator__imp(
cout << TermColor::RED() << "--- " << TermColor::RESET() << l << "\t";
cout << "new [" << last_l << " to " << l << ")\n";
)
//--------- add
__faiss_clique_loopcandidate_generator__addtoindex(cout << TermColor::YELLOW();)
if( l>start_adding_descriptors_to_index_after) {
for( int j=l_last_added_to_index ; jat( wholeImageComputedList_at( j ) )->getWholeImageDescriptor();
VectorXf X_float = X.cast();
float * X_raw = X_float.data();
#if 0 //see if the type casting was correct.
for( int g=0;g<5;g++ ) {
// cout << "(" << X(g) << "," << X_float(g) << ") ";
cout << "(" << X(g) << "," << X_raw[g] << ") ";
}
cout << endl;
#endif
index.add( 1, X_raw );
}
l_last_added_to_index = l-start_adding_descriptors_to_index_after;
} else {
__faiss_clique_loopcandidate_generator__addtoindex(
cout << "not seen enough. so,, add nothing. Will start adding after "<< start_adding_descriptors_to_index_after<< " descriptors\n";
)
}
__faiss_clique_loopcandidate_generator__addtoindex(cout << TermColor::RESET();)
//----------- search
__faiss_clique_loopcandidate_generator__search(cout << TermColor::MAGENTA();)
for( int l_i = last_l ; l_i < l ; l_i++ )
{
__faiss_clique_loopcandidate_generator__search(
cout << "\t\tindex.search(" << l_i << ")\t";
cout << "index.search(" << wholeImageComputedList_at(l_i) << ")\t";
cout << "index.ntotal=" << index.ntotal << "\n";
)
if( index.ntotal < K_NEAREST_NEIGHBOURS )
break;
// l_i's descriptor
VectorXd X = data_map->at( wholeImageComputedList_at( l_i ) )->getWholeImageDescriptor();
VectorXf X_float = X.cast();
float * X_raw = X_float.data();
// ElapsedTime time_to_search = ElapsedTime();
index.search( 1, X_raw, K_NEAREST_NEIGHBOURS, distances, labels );
// cout << "search done in (ms): " << time_to_search.toc_milli() << endl;
__faiss_clique_loopcandidate_generator__search(
// Printing
for( int g=0 ; g DOT_PROD_THRESH )
cout << TermColor::GREEN();
else cout << TermColor::MAGENTA();
printf( "%4d<--(%4.2f)-->%d\t", l_i, distances[g], labels[g] );
cout << TermColor::RESET();
}
cout << endl;
)
// Go thru each nearest neighbours and add to list separated ones. vv same as 168, ignore
// for example, if the neighbours: `802<--(0.92)-->606 802<--(0.89)-->168 802<--(0.89)-->172 802<--(0.89)-->169 802<--(0.88)-->607`
// ^^ ^^ ^^same as 168, so ignore ^^ same as 606, ignore
// cout << "loop thru " << K_NEAREST_NEIGHBOURS << "\n";
for( int g=0 ; g < K_NEAREST_NEIGHBOURS ; g++ )
{
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.
break;
// cout << "g="<first=" << ity->first << endl;
if( (ity->first - labels[g]) < LOCALITY )
{
// cout << "\tcond satisfied\n";
duplicate = ity->first;
break;
}
}
if( duplicate != -1 ) {
// cout << "\tthis was not uniq so increment the key=" << duplicate << " by 1\n";
retained[ duplicate ]++;
} else {
// cout << "\tadd new key at " << labels[g] << endl;
retained[ labels[g] ] = 1;
}
}
if( retained.size() > 0 && l_i%reset_accumulation_every_n_frames == 0)
{
__faiss_clique_loopcandidate_generator__search(
cout << TermColor::iYELLOW() << "l_i=" << l_i << " retained (ie. added to `foundLoops`) cout all: ";
for( auto ity=retained.begin() ; ity!=retained.end() ; ity++ )
{
cout << ity->first << ":" << ity->second << ", ";
}
cout << TermColor::RESET() << endl;
)
//%%%%
//%%%% NOTE: If you put all the feasible candidates into the foundLoops it causes
//%%%% The pose computation to queue-up, since the number of candidates are too many.
//%%%% Usually addition of more than 2 candidates at a time spells trouble.
//%%%% I use simple heuristics when there are more than 3 items in the retained.
if( retained.size() == 1 )
{
__faiss_clique_loopcandidate_generator__search(
cout << "only 1 item in retained, pushback " << wholeImageComputedList_at(l-1) << "<--->" <<
wholeImageComputedList_at(retained.begin()->first) << endl;
)
std::lock_guard lk_foundloops(m_foundLoops);
// __faiss_clique_loopcandidate_generator__search( cout << "pushback: " << wholeImageComputedList_at(l-1) << "<--->" << wholeImageComputedList_at(retained.begin()->first) << endl; )
foundLoops.push_back( std::make_tuple( wholeImageComputedList_at(l-1),
wholeImageComputedList_at(retained.begin()->first ),
0.9 ) );
}
if( retained.size() > 1 )
{
int percent = 100. / retained.size(); //will retain this many
__faiss_clique_loopcandidate_generator__search( cout << "Retain %=" << percent << endl; )
for( auto ity=retained.begin() ; ity!=retained.end() ; ity++ )
{
if( rand()%100 < percent ) {
std::lock_guard lk_foundloops(m_foundLoops);
__faiss_clique_loopcandidate_generator__search( cout << "pushback: " << wholeImageComputedList_at(l-1) << "<--->" << wholeImageComputedList_at(ity->first) << endl; )
foundLoops.push_back( std::make_tuple( wholeImageComputedList_at(l-1),
wholeImageComputedList_at(ity->first ),
0.9 ) );
}
}
}
retained.clear();
}
}
last_l = l;
rate.sleep();
}
delete [] distances;
delete [] labels;
cout << "[Cerebro::faiss_clique_loopcandidate_generator] Finished Thread\n";
}
#define ___faiss_multihypothesis_tracking___add(msg) msg;
// #define ___faiss_multihypothesis_tracking___add(msg) ;
#define ___faiss_multihypothesis_tracking___search(msg) msg;
// #define ___faiss_multihypothesis_tracking___search(msg) ;
void Cerebro::faiss_multihypothesis_tracking()
{
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");
assert( b_run_thread && "you need to call run_thread_enable() before run() can start executing\n" );
//-----------------//
//---- Settings ---//
//-----------------//
const int start_adding_descriptors_to_index_after = 150;
const int K_NEAREST_NEIGHBOURS=5;
//-----------------//
//---- Wait ----//
//-----------------//
// wait until connected_to_descriptor_server=true and descriptor_size_available=true
if( wait_until__connectedToDescServer_and_descSizeAvailable( 71 ) == false ) {
cout << TermColor::RED() << "[Cerebro::faiss__naive_loopcandidate_generator ERROR] wait_until__connectedToDescServer_and_descSizeAvailable returned false implying a timeout.\n" << TermColor::RESET();
return;
}
cout << TermColor::GREEN() <<"[Cerebro::faiss_multihypothesis_tracking] descriptor_size=" << this->descriptor_size << " connected_to_descriptor_server && descriptor_size_available" << TermColor::RESET() << endl;
assert( this->descriptor_size> 0 );
//---------------------//
//--- init faiss index //
//---------------------//
cout << TermColor::GREEN() << "[[Cerebro::faiss_multihypothesis_tracking]] init a faiss::IndexFlatIP("<< this->descriptor_size << ")" << TermColor::RESET() << endl;
faiss::IndexFlatIP index(this->descriptor_size);
// init (misc)
ros::Rate rate(10);
auto data_map = dataManager->getDataMapRef();
HypothesisManager * hyp_manager = new HypothesisManager(); // there is a delete at the end of this function
// Start monitoring thread
hyp_manager->monitoring_thread_enable();
// hyp_manager->monitoring_thread_disable();
std::thread hyp_monitoring_th( &HypothesisManager::monitoring_thread, hyp_manager );
int l=0, last_l=0, l_last_added_to_index=0;
float * distances = new float[K_NEAREST_NEIGHBOURS];
faiss::Index::idx_t * labels = new faiss::Index::idx_t[K_NEAREST_NEIGHBOURS];
//---------------------//
// While
// index.add( i-150 )
// index.search( i )
//---------------------//
while( b_run_thread ) {
l = wholeImageComputedList_size();
if( l<= last_l ) {
cout << "[Cerebro::faiss_multihypothesis_tracking] nothing new\n";
rate.sleep();
continue;
}
// looks like new descriptors are available.
cout << TermColor::RED() << "--- " << TermColor::RESET() << l << "\t";
cout << "new [" << last_l << " to " << l << ")\n";
//--------- add
___faiss_multihypothesis_tracking___add(cout << TermColor::YELLOW();)
if( l>start_adding_descriptors_to_index_after) {
for( int j=l_last_added_to_index ; jat( wholeImageComputedList_at( j ) )->getWholeImageDescriptor();
VectorXf X_float = X.cast();
float * X_raw = X_float.data();
#if 0 //see if the type casting was correct.
for( int g=0;g<5;g++ ) {
// cout << "(" << X(g) << "," << X_float(g) << ") ";
cout << "(" << X(g) << "," << X_raw[g] << ") ";
}
cout << endl;
#endif
index.add( 1, X_raw );
}
l_last_added_to_index = l-start_adding_descriptors_to_index_after;
} else {
___faiss_multihypothesis_tracking___add(
cout << "not seen enough. so,, add nothing to the index. Will start adding after "<< start_adding_descriptors_to_index_after<< " descriptors\n";
)
}
___faiss_multihypothesis_tracking___add(cout << TermColor::RESET();)
//----------- search
cout << TermColor::MAGENTA();
for( int l_i = last_l ; l_i < l ; l_i++ ) {
cout << "\t\tindex.search(" << l_i << ")\t";
cout << "index.search(" << wholeImageComputedList_at(l_i) << ")\t";
cout << "index.ntotal=" << index.ntotal << "\n";
if( index.ntotal < K_NEAREST_NEIGHBOURS )
break;
// l_i's descriptor
VectorXd X = data_map->at( wholeImageComputedList_at( l_i ) )->getWholeImageDescriptor();
VectorXf X_float = X.cast();
float * X_raw = X_float.data();
// ElapsedTime time_to_search = ElapsedTime();
index.search( 1, X_raw, K_NEAREST_NEIGHBOURS, distances, labels );
// cout << "search done in (ms): " << time_to_search.toc_milli() << endl;
// Loop over all the nearest neighbours
cout << "\t\t";
for( int g=0 ; g 0.85 )
cout << TermColor::GREEN();
else cout << TermColor::MAGENTA();
printf( "g=%d: %4d<--(%4.2f)-->%d\t", g, l_i, distances[g], labels[g] );
cout << TermColor::RESET();
if( distances[g] > 0.85 ) {
// Send (l_i, labels[g], distances[g] ) to HypothesisManager
hyp_manager->add_node( l_i, labels[g], distances[g] );
}
}
cout << endl ;
// Loop through each hypothesis and decrement the time-to-live
hyp_manager->digest();
}
last_l = l;
rate.sleep();
}
hyp_manager->monitoring_thread_disable();
hyp_monitoring_th.join();
delete [] distances;
delete [] labels;
delete hyp_manager;
cout << "[Cerebro::faiss_multihypothesis_tracking] Finished Thread\n";
}
#endif //HAVE_FAISS
///-----------------------------------------------------------------
///-----------------------------------------------------------------
/// This implements a simple loopclosure detection scheme based on dot-product of descriptor-vectors.
///
// 1 will plot the result of dot product as image. 0 will not plot to image
// #define __Cerebro__descrip_N__dot__descrip_0_N__implotting 0
#define __Cerebro__descrip_N__dot__descrip_0_N__implotting 1
void Cerebro::descrip_N__dot__descrip_0_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");
assert( b_run_thread && "you need to call run_thread_enable() before run() can start executing\n" );
cout << TermColor::GREEN() << "[Cerebro::descrip_N__dot__descrip_0_N] Start thread" << TermColor::RESET() << endl;
//---
//--- Main settings for this function
//---
int LOCALITY_THRESH = 12;
float DOT_PROD_THRESH = 0.85;
const int start_adding_descriptors_to_index_after = 50;
ros::Rate rate(10);
#if 0
// wait until connected_to_descriptor_server=true and descriptor_size_available=true
int wait_itr = 0;
while( true ) {
if( this->connected_to_descriptor_server && this->descriptor_size_available)
break;
__Cerebro__run__(cout << wait_itr << " [Cerebro::descrip_N__dot__descrip_0_N]waiting for `descriptor_size_available` to be true\n";)
rate.sleep();
wait_itr++;
if( wait_itr > 157 ) {
__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(); )
return;
}
}
#endif
if( wait_until__connectedToDescServer_and_descSizeAvailable( 71 ) == false ) {
cout << TermColor::RED() << "[ Cerebro::descrip_N__dot__descrip_0_N ERROR] wait_until__connectedToDescServer_and_descSizeAvailable returned false implying a timeout.\n" << TermColor::RESET();
return;
}
__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; )
assert( this->descriptor_size> 0 );
int l=0, last_l=0;
int last_processed=0;
MatrixXd M = MatrixXd::Zero( this->descriptor_size, 29000 ); // TODO: Need dynamic allocation here.
cout << "[Cerebro::descrip_N__dot__descrip_0_N] M.rows = " << M.rows() << " M.cols=" << M.cols() << endl;
cout << "[Cerebro::descrip_N__dot__descrip_0_N] TODO: Need dynamic allocation here.\n";
#if __Cerebro__descrip_N__dot__descrip_0_N__implotting > 0
// Plotting image
Plot2Mat handle(320,240, cv::Scalar(80,80,80) );
// Plot2Mat handle;
handle.setYminmax( -0.1, 1.1);
#endif
while( b_run_thread )
{
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.
l = wholeImageComputedList_size();
if( l - last_l < 3 ) {
__Cerebro__run__debug( cout << "[Cerebro::descrip_N__dot__descrip_0_N]nothing new\n"; );
rate.sleep();
continue;
}
__Cerebro__run__( cout << TermColor::RED() << "---" << TermColor::RESET() << endl; )
__Cerebro__run__( cout << "l=" << l << endl; )
__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; )
///------------ Extract v, v-1, v-2. The latest 3 descriptors.
VectorXd v, vm, vmm;
ros::Time i_v, i_vm, i_vmm;
{
// std::lock_guard lk(m_wholeImageComputedList);
assert( data_map.count( wholeImageComputedList_at(l-1) ) > 0 &&
data_map.count( wholeImageComputedList_at(l-2) ) > 0 &&
data_map.count( wholeImageComputedList_at(l-3) ) > 0 &&
"either of l, l-1, l-2 is not available in the datamap"
);
v = data_map->at( wholeImageComputedList_at(l-1) )->getWholeImageDescriptor();
vm = data_map->at( wholeImageComputedList_at(l-2) )->getWholeImageDescriptor();
vmm = data_map->at( wholeImageComputedList_at(l-3) )->getWholeImageDescriptor();
i_v = wholeImageComputedList_at(l-1);
i_vm = wholeImageComputedList_at(l-2);
i_vmm = wholeImageComputedList_at(l-3);
}
// This is very inefficient. Better to have a matrix-vector product and not getWholeImageDescriptor() all the time.
assert( M.rows() == v.size() );
assert( l < M.cols() );
////-------------- Fill descriptors [last_l, l) into M.
{
// std::lock_guard lk(m_wholeImageComputedList); //no need for this as we switched to _at functions which are threadsafe
for( int _s=last_l ; _sat( wholeImageComputedList_at(_s) )->getWholeImageDescriptor();
__Cerebro__run__debug(
cout << "M.col(" << _s << ") = data_map[ " << wholeImageComputedList_at(_s) << " ]. \t";
cout << " isWholeImageDescriptorAvailable = " << data_map->at( wholeImageComputedList_at(_s) )->isWholeImageDescriptorAvailable() << endl;
)
}
}
//////////////////////////////////////
////----------- DOT PRODUCT----------
/////////////////////////////////////
int k = l - start_adding_descriptors_to_index_after; // given a stamp, l, get another stamp k. better make this to 200.
//usable size of M is 8192xl, let k (k 5 ) {
ElapsedTime timer;
timer.tic();
// dot
VectorXd u = v.transpose() * M.leftCols( k );
VectorXd um = vm.transpose() * M.leftCols( k );
VectorXd umm = vmm.transpose() * M.leftCols( k );
__Cerebro__run__( cout << " ";)
__Cerebro__run__( cout << " ";)
__Cerebro__run__( cout << " ";)
__Cerebro__run__( cout << "Done in (ms): " << timer.toc_milli() << endl; )
// max coefficient and the index of maxcoeff.
double u_max = u.maxCoeff();
double um_max = um.maxCoeff();
double umm_max = umm.maxCoeff();
int u_argmax=-1, um_argmax=-1, umm_argmax=-1;
for( int ii=0 ; ii 0 && um_argmax > 0 && umm_argmax > 0 );
#if __Cerebro__descrip_N__dot__descrip_0_N__implotting > 0
// plot
handle.resetCanvas();
handle.plot( u );
#endif
// Criteria for a recognized place
if( abs(u_argmax - um_argmax) < LOCALITY_THRESH && abs(u_argmax-umm_argmax) < LOCALITY_THRESH && u_max > DOT_PROD_THRESH )
{
// std::lock_guard lk(m_wholeImageComputedList); //no need of lock here as we switched to _at functions for whlist
__Cerebro__run__(
cout << TermColor::RED() << "Loop FOUND!! a_idx=" << l-1 << "<-----> (" << u_argmax << "," << um_argmax << "," << umm_argmax << ")"<< TermColor::RESET() << endl;
cout << TermColor::RED() << "loop FOUND!! "
<< "t_l=" << wholeImageComputedList_at(l-1)
<< " "
<< "t_argmax=" << wholeImageComputedList_at(u_argmax)
<< TermColor::RESET() << endl;
)
#if __Cerebro__descrip_N__dot__descrip_0_N__implotting > 0
handle.mark( u_argmax );
#endif
//TODO :
// publish the image pair based on a config_file_flag
// read flags for publish image, threshold(0.92), locality threshold (8) from file.
{
std::lock_guard lk_foundloops(m_foundLoops);
foundLoops.push_back( std::make_tuple( wholeImageComputedList_at(l-1), wholeImageComputedList_at(u_argmax), u_max ) );
}
}
#if __Cerebro__descrip_N__dot__descrip_0_N__implotting > 0
cv::imshow("canvas", handle.getCanvasConstPtr() );
cv::waitKey(20);
#endif
}
else {
__Cerebro__run__(
cout << "[Cerebro::descrip_N__dot__descrip_0_N] do nothing. not seen enough yet.\n";
)
}
last_l = l;
rate.sleep();
}
cout << TermColor::RED() << "[Cerebro::descrip_N__dot__descrip_0_N] Finished thread" << TermColor::RESET() << endl;
}
//------------------------------------------------------------------//
//---------------- END Populate Loop Candidates --------------------//
//------------------------------------------------------------------//
const int Cerebro::foundLoops_count() const
{
std::lock_guard lk(m_foundLoops);
return foundLoops.size();
}
const std::tuple Cerebro::foundLoops_i( int i) const
{
std::lock_guard lk(m_foundLoops);
assert( i >= 0 && i lk(m_foundLoops);
json jsonout_obj;
auto data_map = dataManager->getDataMapRef();
int n_foundloops = foundLoops.size();
for( int i=0 ; i(u);
ros::Time t_prev = std::get<1>(u);
double score = std::get<2>(u);
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" );
int idx_1 = std::distance( data_map->begin(), data_map->find( t_curr ) );
int idx_2 = std::distance( data_map->begin(), data_map->find( t_prev ) );
// cout << "loop#" << i << " of " << n_foundloops << ": ";
// cout << t_curr << "(" << score << ")" << t_prev << endl;
// cout << idx_1 << "<--->" << idx_2 << endl;
json _cur_json_obj;
_cur_json_obj["time_sec_a"] = t_curr.sec;
_cur_json_obj["time_nsec_a"] = t_curr.nsec;
_cur_json_obj["time_sec_b"] = t_prev.sec;
_cur_json_obj["time_nsec_b"] = t_prev.nsec;
_cur_json_obj["time_double_a"] = t_curr.toSec();
_cur_json_obj["time_double_b"] = t_prev.toSec();
_cur_json_obj["global_a"] = idx_1;
_cur_json_obj["global_b"] = idx_2;
_cur_json_obj["score"] = score;
jsonout_obj.push_back( _cur_json_obj );
}
return jsonout_obj;
}
//--------------------------------------------------------------//
//------------------ Geometry Thread ---------------------------//
//--------------------------------------------------------------//
// #define __Cerebro__loopcandi_consumer__(msg) msg;
#define __Cerebro__loopcandi_consumer__(msg) ;
// ^This will also imshow image-pairs with gms-matches marked.
// #define __Cerebro__loopcandi_consumer__IMP( msg ) msg;
#define __Cerebro__loopcandi_consumer__IMP( msg ) ;
// ^Important Text only printing
#define __Cerebro__loopcandi_consumer__IMSHOW 0 // will not produce the images (ofcourse will not show as well)
// #define __Cerebro__loopcandi_consumer__IMSHOW 1 // produce the images and log them, will not imshow
// #define __Cerebro__loopcandi_consumer__IMSHOW 2 // produce the images and imshow them, don't log
// Just uncomment it to disable consistency check.
// #define __Cerebro__loopcandi_consumer__no_pose_consistency_check
void Cerebro::loopcandiate_consumer_thread()
{
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");
assert( b_loopcandidate_consumer && "you need to call loopcandidate_consumer_enable() before loopcandiate_consumer_thread() can start executing\n" );
cout << TermColor::GREEN() << "[Cerebro::loopcandiate_consumer_thread] Start thread" << TermColor::RESET() << endl;
// init StereoGeometry
bool stereogeom_status = init_stereogeom();
if( !stereogeom_status ) {
assert( false && "[Cerebro::loopcandiate_consumer_thread()] cannot init_stereogeom\n");
return;
}
// stereogeom->set_K( 375.0, 375.0, 376.0, 240.0 ); // set this to something sensible, best is to use left camera's K
// init pt-feature-matcher
ros::Rate rate(1);
int prev_count = 0;
int new_count = 0;
ElapsedTime timer;
while( b_loopcandidate_consumer )
{
new_count = foundLoops_count();
if( new_count == prev_count ) {
rate.sleep();
continue;
}
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::iGREEN() ;
cout << "I see "<< new_count - prev_count << " new candidates. from i=["<< prev_count << "," << new_count-1 << "]\n";
// cout << "Cerebro::loopcandiate_consumer_thread(); #loops=" << new_count << TermColor::RESET() << endl;
cout << TermColor::RESET() ;
)
for( int j= prev_count ; j<=new_count-1 ; j++ )
{
ProcessedLoopCandidate proc_candi;
timer.tic() ;
#ifdef __Cerebro__loopcandi_consumer__no_pose_consistency_check
bool proc___status = process_loop_candidate_imagepair( j, proc_candi );
#else
bool proc___status = process_loop_candidate_imagepair_consistent_pose_compute( j, proc_candi );
#endif
__Cerebro__loopcandi_consumer__IMP(
cout << "\t" << timer.toc_milli() << "(ms) !! process_loop_candidate_imagepair()\n";
)
// the data is in the `proc_candi` which is put into a vector which is a class variable.
if( proc___status )
{
std::lock_guard lk(m_processedLoops);
// publish proc_candi
cerebro::LoopEdge loopedge_msg;
#ifdef __Cerebro__loopcandi_consumer__no_pose_consistency_check
bool __makemsg_status = proc_candi.makeLoopEdgeMsg( loopedge_msg );
#else
bool __makemsg_status = proc_candi.makeLoopEdgeMsgWithConsistencyCheck( loopedge_msg );
#endif
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::iBLUE() << "__makemsg_status: " << __makemsg_status << " publish loopedge_msg" << TermColor::RESET() << endl;
)
// publish only if msg was successfully made ==> all the candidate relative poses are consistent.
if( __makemsg_status )
{
__Cerebro__loopcandi_consumer__IMP( cout << TermColor::GREEN() << "publish loopedge_msg" << TermColor::RESET() << endl; )
pub_loopedge.publish( loopedge_msg );
} else {
__Cerebro__loopcandi_consumer__IMP( cout << TermColor::RED() << "not publishing because __makemsg_status was false" << TermColor::RESET() << endl; )
}
// irrespective of where the poses are consistent or not add it to the list.
processedloopcandi_list.push_back( proc_candi );
__Cerebro__loopcandi_consumer__IMP(cout << "\tAdded to `processedloopcandi_list`" << endl;)
}
else {
__Cerebro__loopcandi_consumer__IMP( cout << "\tNot added to `processedloopcandi_list`, status was false\n" << endl; )
}
}
prev_count = new_count;
rate.sleep();
}
cout << TermColor::RED() << "[Cerebro::loopcandiate_consumer_thread] disable called, quitting loopcandiate_consumer_thread" << TermColor::RESET() << endl;
}
const int Cerebro::processedLoops_count() const
{
std::lock_guard lk(m_processedLoops);
return processedloopcandi_list.size();
}
const ProcessedLoopCandidate& Cerebro::processedLoops_i( int i ) const
{
std::lock_guard lk(m_processedLoops);
assert( i>=0 && i< processedloopcandi_list.size() && "[processedLoops_i] input i is not in the correct range" );
if( i>=0 && i< processedloopcandi_list.size() ) {
return processedloopcandi_list[ i ];
}
else {
cout << TermColor::RED() << "[Cerebro::processedLoops_i] error you requested for processedloopcandidate idx=" << i << " but the length of the vector was " << processedloopcandi_list.size() << endl;
exit(1);
}
}
bool Cerebro::init_stereogeom()
{
auto left_camera = dataManager->getAbstractCameraRef(0);
if( !dataManager->isAbstractCameraSet(1) )
{
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");
return false;
}
auto right_camera = dataManager->getAbstractCameraRef(1);
__Cerebro__loopcandi_consumer__(
cout << TermColor::iGREEN() ;
cout << "[Cerebro::loopcandiate_consumer_thread] left_camera\n" << left_camera->parametersToString() << endl;
cout << "[Cerebro::loopcandiate_consumer_thread] right_camera\n" << right_camera->parametersToString() << endl;
cout << TermColor::RESET();
)
Matrix4d right_T_left;
if( !dataManager->isCameraRelPoseSet( std::make_pair(1,0)) )
{
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`");
return false;
}
right_T_left = dataManager->getCameraRelPose( std::make_pair(1,0) );
__Cerebro__loopcandi_consumer__(
cout << TermColor::iGREEN() ;
cout << "[Cerebro::loopcandiate_consumer_thread] right_T_left: " << PoseManipUtils::prettyprintMatrix4d( right_T_left ) << endl;
cout << TermColor::RESET();
)
stereogeom = std::make_shared( left_camera,right_camera, right_T_left );
return true;
}
bool Cerebro::retrive_stereo_pair( DataNode* node, cv::Mat& left_image, cv::Mat& right_image, bool bgr2gray )
{
#if 0
// raw stereo-images in gray
if( !(node->isImageAvailable()) || !(node->isImageAvailable(1)) ) {
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;
return false;
}
const cv::Mat& bgr_left_image = node->getImage();
const cv::Mat& bgr_right_image = node->getImage(1);
#else
auto img_data_mgr = dataManager->getImageManagerRef();
if( img_data_mgr->isImageRetrivable( "left_image", node->getT() )==false )
{
cout << TermColor::RED() << "[Cerebro::retrive_stereo_pair] img_data_mgr->isImageRetrivable( \"left_image\"," << node->getT() << ") returned false" << TermColor::RESET() << endl;
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;
return false;
}
if ( img_data_mgr->isImageRetrivable( "right_image", node->getT() ) == false )
{
cout << TermColor::RED() << "[Cerebro::retrive_stereo_pair] img_data_mgr->isImageRetrivable( \"right_image\"," << node->getT() << ") returned false" << TermColor::RESET() << endl;
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;
// img_data_mgr->print_status();
// cout << TermColor::RED() << "[Cerebro::retrive_stereo_pair] img_data_mgr->isImageRetrivable( \"right_image\"," << node->getT() << ") returned false" << TermColor::RESET() << endl;
return false;
}
cv::Mat bgr_left_image, bgr_right_image;
img_data_mgr->getImage( "left_image", node->getT(), bgr_left_image );
img_data_mgr->getImage( "right_image", node->getT(), bgr_right_image );
#if 0
cout << "bgr_left_image: " << MiscUtils::cvmat_info( bgr_left_image ) << "\n";
cout << "bgr_right_image: " << MiscUtils::cvmat_info( bgr_right_image ) << "\n";
#endif
if( !(bgr_left_image.data) || !(bgr_right_image.data) )
{
cout << "[Cerebro::retrive_stereo_pair] Invalid images bfhjreturn false.\n";
return false;
}
#endif
if( bgr2gray ) {
if( bgr_left_image.channels() > 1 )
cv::cvtColor( bgr_left_image, left_image, CV_BGR2GRAY );
else
left_image = bgr_left_image;
if( bgr_right_image.channels() > 1 )
cv::cvtColor( bgr_right_image, right_image, CV_BGR2GRAY );
else
right_image = bgr_right_image;
}
else {
left_image = bgr_left_image;
right_image = bgr_right_image;
}
#if 0
cout << "[Cerebro::retrive_stereo_pair]left_image: " << MiscUtils::cvmat_info( left_image ) << "\n";
cout << "[Cerebro::retrive_stereo_pair]right_image: " << MiscUtils::cvmat_info( right_image ) << "\n";
cout << endl;
#endif
return true;
}
bool Cerebro::process_loop_candidate_imagepair_consistent_pose_compute( int ii, ProcessedLoopCandidate& proc_candi )
{
auto u = foundLoops_i( ii );
ros::Time t_curr = std::get<0>(u);
ros::Time t_prev = std::get<1>(u);
double score = std::get<2>(u);
auto data_map = dataManager->getDataMapRef();
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" );
int idx_1 = std::distance( data_map->begin(), data_map->find( t_curr ) );
int idx_2 = std::distance( data_map->begin(), data_map->find( t_prev ) );
DataNode * node_1 = data_map->find( t_curr )->second;
DataNode * node_2 = data_map->find( t_prev )->second;
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::BLUE() << "{"<" << idx_2 << TermColor::RESET() << "\tt_curr=" << t_curr << " <--> t_prev=" << t_prev << endl;
)
// if( node_1->getNumberOfSuccessfullyTrackedFeatures() < 20 || node_2->getNumberOfSuccessfullyTrackedFeatures() < 20 ) {
// __Cerebro__loopcandi_consumer__IMP( "[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute] skip processing this pair because it is a kidnaped node\n" );
// return false;
// }
cv::Mat a_imleft_raw, a_imright_raw;
bool ret_status_a = retrive_stereo_pair( node_1, a_imleft_raw, a_imright_raw );
if( !ret_status_a )
return false;
cv::Mat b_imleft_raw, b_imright_raw;
bool ret_status_b = retrive_stereo_pair( node_2, b_imleft_raw, b_imright_raw );
if( !ret_status_b )
return false;
//------------------------------------------------
//------ 3d points from frame_a
//------------------------------------------------
__Cerebro__loopcandi_consumer__( cout << "[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute] 3d points from frame_a\n"; )
cv::Mat a_imleft_srectified, a_imright_srectified;
cv::Mat a_3dImage;
MatrixXd a_3dpts;
cv::Mat a_disp_viz;
stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images(
a_imleft_raw, a_imright_raw,
a_imleft_srectified,a_imright_srectified, a_3dpts, a_3dImage, a_disp_viz );
//-----------------------------------------------------
//--------------- 3d points from frame_b
//-----------------------------------------------------
__Cerebro__loopcandi_consumer__( cout << "[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute] 3d points from frame_b\n"; )
cv::Mat b_imleft_srectified, b_imright_srectified;
cv::Mat b_3dImage;
MatrixXd b_3dpts;
cv::Mat b_disp_viz;
stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images(
b_imleft_raw, b_imright_raw,
b_imleft_srectified, b_imright_srectified, b_3dpts, b_3dImage, b_disp_viz );
//---------------------------------------------------------------------
//------------ point matches between a_left, b_left
//---------------------------------------------------------------------
__Cerebro__loopcandi_consumer__( cout << "[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute]point matches between a_left, b_left\n"; )
MatrixXd uv, uv_d; // u is from frame_a; ud is from frame_b
ElapsedTime timer;
timer.tic();
StaticPointFeatureMatching::gms_point_feature_matches(a_imleft_srectified, b_imleft_srectified, uv, uv_d );
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" ;
__Cerebro__loopcandi_consumer__( cout << msg_pf_matches << endl; )
if( uv.cols() < 150 ) {
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::RED() << "[Cerebro::process_loop_candidate_imagepair_consistent_pose_compute]too few gms matches between node#" << idx_1 << " and node#" << idx_2;
cout << " contains pf_matches=" << uv.cols() << ", thresh=150, so I am rejecting this loopcandidate." << TermColor::RESET() << endl;
)
return false;
}
//----------------------------------------------------------------------
//-------------- PNP and P3P
//----------------------------------------------------------------------
Matrix4d odom_b_T_a = node_2->getPose().inverse() * node_1->getPose();
proc_candi = ProcessedLoopCandidate( ii, node_1, node_2 );
proc_candi.idx_from_datamanager_1 = idx_1;
proc_candi.idx_from_datamanager_2 = idx_2;
proc_candi.pf_matches = uv.cols();
//----- Option-A:
__Cerebro__loopcandi_consumer__IMP( cout << TermColor::BLUE() << "Option-A" << TermColor::RESET() << endl; )
std::vector feature_position_uv;
std::vector feature_position_uv_d;
std::vector world_point_uv;
StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity(
stereogeom, uv, a_3dImage, uv_d,
feature_position_uv, feature_position_uv_d, world_point_uv);
Matrix4d op1__b_T_a; string pnp__msg;
#if 1
//--
float pnp_goodness = StaticTheiaPoseCompute::PNP( world_point_uv, feature_position_uv_d, op1__b_T_a, pnp__msg );
//--END
#else
//--
op1__b_T_a = odom_b_T_a; // setting initial guess as odometry rel pose with translation as zero
// op1__b_T_a(0,3) = 0.0; op1__b_T_a(1,3) = 0.0; op1__b_T_a(2,3) = 0.0;
float pnp_goodness = StaticCeresPoseCompute::PNP( world_point_uv, feature_position_uv_d, op1__b_T_a, pnp__msg );
//--END
#endif
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::YELLOW() << "pnp_goodness=" << pnp_goodness << " op1__b_T_a = " << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a ) << TermColor::RESET() << endl;
)
__Cerebro__loopcandi_consumer__(
cout << pnp__msg << endl;
)
// reprojection debug image for op-1
// plot( PI( op1__b_T_a * world_point_uv ) ) on imB
#if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)
{
MatrixXd PI_world_point_uv, PI_world_point_uv_odom;
GeometryUtils::idealProjection( stereogeom->get_K(), op1__b_T_a, world_point_uv, PI_world_point_uv );
GeometryUtils::idealProjection( stereogeom->get_K(), odom_b_T_a, world_point_uv, PI_world_point_uv_odom );
cv::Mat pi_dst_img;
MiscUtils::plot_point_sets( b_imleft_srectified, PI_world_point_uv, pi_dst_img, cv::Scalar(0,255,0), false );
MiscUtils::plot_point_sets( pi_dst_img, PI_world_point_uv_odom, cv::Scalar(255,0,0), false );
MiscUtils::plot_point_sets( pi_dst_img, uv, cv::Scalar(0,0,255), false );
MiscUtils::plot_point_sets( pi_dst_img, uv_d, cv::Scalar(255,0,255), false );
cv::resize(pi_dst_img,pi_dst_img, cv::Size(), 0.6, 0.6 );
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");
#if __Cerebro__loopcandi_consumer__IMSHOW == 1
proc_candi.debug_images.push_back( pi_dst_img );
proc_candi.debug_images_titles.push_back( "reprojection_debug_image_1" );
#endif
#if __Cerebro__loopcandi_consumer__IMSHOW == 2
cv::imshow( "plot( PI( op1__b_T_a * world_point_uv ) ) on imB", pi_dst_img );
#endif
}
#endif
//----- Option-B:
__Cerebro__loopcandi_consumer__IMP( cout << TermColor::BLUE() << "Option-B" << TermColor::RESET() << endl; )
std::vector world_point_uv_d;
StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity(
stereogeom, uv_d, b_3dImage, uv,
feature_position_uv_d, feature_position_uv, world_point_uv_d);
Matrix4d op2__a_T_b, op2__b_T_a; string pnp__msg_option_B;
#if 1
//--
float pnp_goodness_optioN_B = StaticTheiaPoseCompute::PNP( world_point_uv_d, feature_position_uv, op2__a_T_b, pnp__msg_option_B );
//--END
#else
//--
op2__a_T_b = odom_b_T_a.inverse(); //initial guess same as odometry
// op2__a_T_b(0,3)=0.0;op2__a_T_b(1,3)=0.0;op2__a_T_b(2,3)=0.0;
float pnp_goodness_optioN_B = StaticCeresPoseCompute::PNP( world_point_uv_d, feature_position_uv, op2__a_T_b, pnp__msg_option_B );
//--END
#endif
op2__b_T_a = op2__a_T_b.inverse();
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::YELLOW() << pnp_goodness_optioN_B << " op2__a_T_b = " << PoseManipUtils::prettyprintMatrix4d( op2__a_T_b ) << TermColor::RESET() << endl;
cout << TermColor::YELLOW() << pnp_goodness_optioN_B << " op2__b_T_a = " << PoseManipUtils::prettyprintMatrix4d( op2__b_T_a ) << TermColor::RESET() << endl;
)
__Cerebro__loopcandi_consumer__(
cout << pnp__msg_option_B << endl;
)
// reprojection debug image for op-2
// plot( PI( op2__a_T_b * world_point_uv_d ) ) on imA
#if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)
{
MatrixXd PI_world_point_uvd, PI_world_point_uvd_odom;
GeometryUtils::idealProjection( stereogeom->get_K(), op2__a_T_b, world_point_uv_d, PI_world_point_uvd );
GeometryUtils::idealProjection( stereogeom->get_K(), odom_b_T_a.inverse(), world_point_uv_d, PI_world_point_uvd_odom );
cv::Mat pi_dst_img;
MiscUtils::plot_point_sets( a_imleft_srectified, PI_world_point_uvd, pi_dst_img, cv::Scalar(0,255,0), false );
MiscUtils::plot_point_sets( pi_dst_img, PI_world_point_uvd_odom, cv::Scalar(255,0,0), false );
MiscUtils::plot_point_sets( pi_dst_img, uv, cv::Scalar(0,0,255), false );
MiscUtils::plot_point_sets( pi_dst_img, uv_d, cv::Scalar(255,0,255), false );
cv::resize(pi_dst_img,pi_dst_img, cv::Size(), 0.6, 0.6 );
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");
#if __Cerebro__loopcandi_consumer__IMSHOW == 1
proc_candi.debug_images.push_back( pi_dst_img );
proc_candi.debug_images_titles.push_back( "reprojection_debug_image_2" );
#endif
#if __Cerebro__loopcandi_consumer__IMSHOW == 2
cv::imshow( "plot( PI( op2__a_T_b * world_point_uv_d ) ) on imA", pi_dst_img );
#endif
}
#endif
//----- Option-C
__Cerebro__loopcandi_consumer__IMP( cout << TermColor::BLUE() << "Option-C" << TermColor::RESET() << endl; )
vector< Vector3d> uv_X;
vector< Vector3d> uvd_Y;
StaticPointFeatureMatching::make_3d_3d_collection__using__pfmatches_and_disparity( uv, a_3dImage, uv_d, b_3dImage,
uv_X, uvd_Y );
Matrix4d icp_b_T_a; string p3p__msg;
#if 1
//--
float p3p_goodness = StaticTheiaPoseCompute::P3P_ICP( uv_X, uvd_Y, icp_b_T_a, p3p__msg );
//--END
#else
//--
icp_b_T_a = odom_b_T_a;
// icp_b_T_a(0,3) = 0.0; icp_b_T_a(1,3) = 0.0; icp_b_T_a(2,3) = 0.0;
float p3p_goodness = StaticCeresPoseCompute::P3P_ICP( uv_X, uvd_Y, icp_b_T_a, p3p__msg );
//--END
#endif
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::YELLOW() << p3p_goodness << " icp_b_T_a = " << PoseManipUtils::prettyprintMatrix4d( icp_b_T_a ) << TermColor::RESET() << endl;
)
__Cerebro__loopcandi_consumer__(
cout << p3p__msg << endl;
)
// reprojection debug image for op-3
// plot( PI( icp_b_T_a * world_point_uv ) ) on imB
#if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)
{
MatrixXd PI_world_point_uv, PI_world_point_uv_odom;
GeometryUtils::idealProjection( stereogeom->get_K(), icp_b_T_a, world_point_uv, PI_world_point_uv );
GeometryUtils::idealProjection( stereogeom->get_K(), odom_b_T_a, world_point_uv, PI_world_point_uv_odom );
cv::Mat pi_dst_img;
MiscUtils::plot_point_sets( b_imleft_srectified, PI_world_point_uv, pi_dst_img, cv::Scalar(0,255,0), false );
MiscUtils::plot_point_sets( pi_dst_img, PI_world_point_uv_odom, cv::Scalar(255,0,0), false );
MiscUtils::plot_point_sets( pi_dst_img, uv, cv::Scalar(0,0,255), false );
MiscUtils::plot_point_sets( pi_dst_img, uv_d, cv::Scalar(255,0,255), false );
cv::resize(pi_dst_img,pi_dst_img, cv::Size(), 0.6, 0.6 );
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");
#if __Cerebro__loopcandi_consumer__IMSHOW == 1
proc_candi.debug_images.push_back( pi_dst_img );
proc_candi.debug_images_titles.push_back( "reprojection_debug_image_3" );
#endif
#if __Cerebro__loopcandi_consumer__IMSHOW == 2
cv::imshow( "plot( PI( icp_b_T_a * world_point_uv ) ) on imB", pi_dst_img );
#endif
}
#endif
// if( isnan( op1__b_T_a ) ) {
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 ) {
__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"; );
return false;
}
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::YELLOW() << "odom_b_T_a = " << PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) << TermColor::RESET() << endl;
cout << "|op1 - op2|" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a.inverse() * op2__b_T_a ) << endl;
cout << "|op1 - icp|" << PoseManipUtils::prettyprintMatrix4d( op1__b_T_a.inverse() * icp_b_T_a ) << endl;
cout << "|op2 - icp|" << PoseManipUtils::prettyprintMatrix4d( op2__b_T_a.inverse() * icp_b_T_a ) << endl;
)
//-----------------------------------------------------------------
// Fill the output
//-----------------------------------------------------------------
// final pose
// proc_candi._3d2d__2T1 = op1__b_T_a;
// proc_candi.isSet_3d2d__2T1 = true;
// proc_candi._3d2d__2T1__ransac_confidence = pnp_goodness;
// Fill up all the poses which were computed
// option-A
proc_candi.opX_b_T_a.push_back( op1__b_T_a );
proc_candi.opX_goodness.push_back( pnp_goodness );
proc_candi.opX_b_T_a_name.push_back( "op1__b_T_a" );
proc_candi.opX_b_T_a_debugmsg.push_back( pnp__msg );
// Option-B
proc_candi.opX_b_T_a.push_back( op2__b_T_a );
proc_candi.opX_goodness.push_back( pnp_goodness_optioN_B );
proc_candi.opX_b_T_a_name.push_back( "op2__b_T_a" );
proc_candi.opX_b_T_a_debugmsg.push_back( pnp__msg_option_B );
// Option-C
proc_candi.opX_b_T_a.push_back( icp_b_T_a );
proc_candi.opX_goodness.push_back( p3p_goodness );
proc_candi.opX_b_T_a_name.push_back( "icp_b_T_a" );
proc_candi.opX_b_T_a_debugmsg.push_back( p3p__msg );
//-----------------------------------------------------------------
// Build Viz Images
//-----------------------------------------------------------------
#if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)
///------A : pf matching
cv::Mat dst_feat_matches;
MiscUtils::plot_point_pair( a_imleft_srectified, uv, idx_1,
b_imleft_srectified, uv_d, idx_2,
dst_feat_matches, 3, msg_pf_matches+";#pf-matches: "+to_string( uv.cols() ) );
cv::resize(dst_feat_matches, dst_feat_matches, cv::Size(), 0.5, 0.5 );
///----------B : Disparities
cv::Mat dst_disp;
MiscUtils::side_by_side( a_disp_viz, b_disp_viz, dst_disp );
MiscUtils::append_status_image( dst_disp, "a="+ to_string(idx_1)+" b="+to_string(idx_2), .8 );
cv::resize(dst_disp, dst_disp, cv::Size(), 0.5, 0.5 );
// cv::imshow( "dst_disp", dst_disp );
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 );
MiscUtils::append_status_image( dst_disp, "odom_b_T_a:"+PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) );
///---------C: Reprojections
//TODO
#if __Cerebro__loopcandi_consumer__IMSHOW == 1
proc_candi.debug_images.push_back( dst_feat_matches );
proc_candi.debug_images_titles.push_back( "apf_matches" );
proc_candi.debug_images.push_back( dst_disp );
proc_candi.debug_images_titles.push_back( "dsparity_and_poses" );
#endif
#if __Cerebro__loopcandi_consumer__IMSHOW == 2
cv::imshow( "dst_feat_matches", dst_feat_matches );
cv::imshow( "dst_disp", dst_disp );
cv::waitKey(30);
#endif
#endif
return true;
}
#if 0
bool Cerebro::process_loop_candidate_imagepair( int ii, ProcessedLoopCandidate& proc_candi )
{
auto u = foundLoops_i( ii );
ros::Time t_curr = std::get<0>(u);
ros::Time t_prev = std::get<1>(u);
double score = std::get<2>(u);
auto data_map = dataManager->getDataMapRef();
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" );
int idx_1 = std::distance( data_map->begin(), data_map->find( t_curr ) );
int idx_2 = std::distance( data_map->begin(), data_map->find( t_prev ) );
DataNode * node_1 = data_map->find( t_curr )->second;
DataNode * node_2 = data_map->find( t_prev )->second;
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::BLUE() << "{"<" << idx_2 << TermColor::RESET() << endl;
)
// cv::imshow( "im_1", im_1);
// cv::imshow( "im_2", im_2);
//----------------------------------------
//---------Disparity of `idx_1`
//----------------------------------------
cv::Mat grey_im_1_left, grey_im_1_right;
bool ret_status = retrive_stereo_pair( node_1, grey_im_1_left, grey_im_1_right );
if( !ret_status )
return false;
// will get 3d points, stereo-rectified image, and disparity false colormap
MatrixXd _3dpts__im1; //4xN. 3d points of im1
cv::Mat _3dImage__im1;
cv::Mat imleft_srectified, imright_srectified;
cv::Mat disparity_for_visualization;
ElapsedTime timer;
timer.tic();
stereogeom->get_srectifiedim_and_3dpoints_and_3dmap_and_disparity_from_raw_images( grey_im_1_left, grey_im_1_right,
imleft_srectified,imright_srectified,
_3dpts__im1, _3dImage__im1, disparity_for_visualization );
__Cerebro__loopcandi_consumer__(
cout << timer.toc_milli() << " (ms)!! get_srectifiedim_and_3dpoints_and_disparity_from_raw_images\n";
)
//---------------- END Disparity of `idx_1`
//--------------------------------------------------------------
//------------ srectified of idx_2 image pair needed
//--------------------------------------------------------------
cv::Mat grey_im_2_left, grey_im_2_right;
bool ret_status_2 = retrive_stereo_pair( node_2, grey_im_2_left, grey_im_2_right );
if( !ret_status_2 )
return false;
cv::Mat im2_left_srectified, im2_right_srectified;
timer.tic();
// TODO: If feasible can also compute depth of im2, so that the pose computed can be verified for consistency.
// TODO: accept the pose if pnp( 3d of idx_1, 2d of idx_2 ) === pnp( 2d of idx_1, 3d of idx_2 )
stereogeom->do_stereo_rectification_of_raw_images( grey_im_2_left, grey_im_2_right,
im2_left_srectified, im2_right_srectified );
__Cerebro__loopcandi_consumer__(
cout << timer.toc_milli() << " (ms)!! do_stereo_rectification_of_raw_images\n";
)
// cv::imshow( "im2_left_srectified", im2_left_srectified );
//------------- END srectified of idx_2 image pair needed
//-----------------------------------------------------------------------
//----------point_feature_matches for `idx_1` <--> `idx_2`
// gms matcher
//-----------------------------------------------------------------------
MatrixXd uv, uv_d; // u is from frame_a; ud is from frame_b
timer.tic();
StaticPointFeatureMatching::gms_point_feature_matches( imleft_srectified, im2_left_srectified, uv, uv_d );
auto elapsed_time_gms = timer.toc_milli();
__Cerebro__loopcandi_consumer__(
cout << elapsed_time_gms << " (ms)!! gms_point_feature_matches\n";
)
//--------------------- END Matcher ----------------------------------
//---------------- make collection of 3d 2d points
// 3d of `idx_1` <---> 2d of `idx_2`
std::vector feature_position_uv;
std::vector feature_position_uv_d;
std::vector world_point;
StaticPointFeatureMatching::make_3d_2d_collection__using__pfmatches_and_disparity( stereogeom, uv, _3dImage__im1, uv_d,
feature_position_uv, feature_position_uv_d, world_point );
// make_3d_2d_collection__using__pfmatches_and_disparity( uv, _3dImage__im1, uv_d,
// feature_position_uv, feature_position_uv_d, world_point );
int n_valid_depths = world_point.size();
// TODO 2d of idx_1 <---> 3d of idx_2
// See if the matching can be rejected due to insufficient # of matches
if( uv.cols() < 150 ) {
__Cerebro__loopcandi_consumer__IMP(
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;
)
return false;
}
if( n_valid_depths < 150 ) {
__Cerebro__loopcandi_consumer__IMP(
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;
)
return false;
}
// 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
//------------------------------------------
//-------------- theia::pnp
//------------------------------------------
Matrix4d b_T_a; //< RESULT
string pnp__msg = string(""); //< msg about pnp
#if 1
//--- DlsPnp
std::vector solution_rotations;
std::vector solution_translations;
timer.tic();
theia::DlsPnp( feature_position_uv_d, world_point, &solution_rotations, &solution_translations );
auto elapsed_dls_pnp = timer.toc_milli() ;
__Cerebro__loopcandi_consumer__(
cout << elapsed_dls_pnp << " : (ms) : theia::DlsPnp done in\n";
cout << "solutions count = " << solution_rotations.size() << " " << solution_translations.size() << endl;
)
if( solution_rotations.size() == 0 ) {
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::RED() << " theia::DlsPnp returns no solution" << TermColor::RESET() << endl;
)
return false;
}
if( solution_rotations.size() > 1 ) {
__Cerebro__loopcandi_consumer__IMP(
cout << TermColor::RED() << " theia::DlsPnp returns multiple solution" << TermColor::RESET() << endl;
)
return false;
}
// retrive solution
b_T_a = Matrix4d::Identity();
b_T_a.topLeftCorner(3,3) = solution_rotations[0].toRotationMatrix();
b_T_a.col(3).topRows(3) = solution_translations[0];
__Cerebro__loopcandi_consumer__IMP(
// cout << "solution_T " << b_T_a << endl;
cout << TermColor::GREEN() << "DlsPnp (b_T_a): " << PoseManipUtils::prettyprintMatrix4d( b_T_a ) << TermColor::RESET() << endl;
// out_b_T_a = b_T_a;
pnp__msg += "DlsPnp (b_T_a): " + PoseManipUtils::prettyprintMatrix4d( b_T_a ) + ";";
pnp__msg += " elapsed_dls_pnp="+to_string(elapsed_dls_pnp)+";";
)
#endif
#if 1
//--- DlsPnpWithRansac
__Cerebro__loopcandi_consumer__( timer.tic() );
// prep data
vector data_r;
for( int i=0 ; i ransac_estimator(params, dlspnp_estimator);
// Initialize must always be called!
ransac_estimator.Initialize();
theia::RansacSummary summary;
ransac_estimator.Estimate(data_r, &best_rel_pose, &summary);
auto elapsed_dls_pnp_ransac=timer.toc_milli();
__Cerebro__loopcandi_consumer__IMP(
cout << elapsed_dls_pnp_ransac << "(ms)!! DlsPnpWithRansac ElapsedTime includes data prep\n";
cout << "Ransac:";
// for( int i=0; iget_K() * _X_i; //< scaling with camera-intrinsic matrix
Vector3d _tmp;
_tmp << feature_position_uv_d[i], 1.0 ;
_detectedpts_of_B.col(i) = stereogeom->get_K() * _tmp;
_tmp << feature_position_uv[i], 1.0 ;
_detectedpts_of_A.col(i) = stereogeom->get_K() * _tmp;
Vector3d delta = _3dpts_of_A_projectedonB.col(i) - _detectedpts_of_B.col(i);
if( abs(delta(0)) < 2. && abs(delta(1)) < 2. ) {
// cout << " delta=" << delta.transpose();
n_good++;
}
else {
// cout << TermColor::RED() << "delta=" << delta.transpose() << TermColor::RESET();
}
// cout << endl;
}
// cout << "n_good=" <isPoseAvailable() && node_2->isPoseAvailable() ) {
Matrix4d odom_wTA = node_1->getPose();
// cout << "wTa(odom)" << PoseManipUtils::prettyprintMatrix4d( wTa ) << endl;
Matrix4d odom_wTB = node_2->getPose();
// cout << "wTb(odom)" << PoseManipUtils::prettyprintMatrix4d( wTb ) << endl;
Matrix4d odom_b_T_a = odom_wTB.inverse() * odom_wTA;
__Cerebro__loopcandi_consumer__(
cout << "odom_b_T_a" << PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) << endl;
)
pnp__msg += "odom_b_T_a "+PoseManipUtils::prettyprintMatrix4d( odom_b_T_a ) +";";
auto a_timestamp = node_1->getT();
auto b_timestamp = node_2->getT();
pnp__msg += "a.timestamp: " + to_string(a_timestamp.toSec());
pnp__msg += " b.timestamp: " + to_string(b_timestamp.toSec()) + ";";
// PI( odom_b_T_a * a_X )
for( int i=0 ; iget_K() * _X_i; //< scaling with camera-intrinsic matrix
}
}
else {
cout << TermColor::RED() << "[Cerebro::process_loop_candidate_imagepair] In plotting part needs poses from DataManager which is not available." << TermColor::RESET() << endl;
}
#endif
// plot( B, ud )
cv::Mat __dst;
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()) );
// cv::imshow( "plot( B, ud )", __dst );
// plot( B, _3dpts_of_A_projectedonB )
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)" );
// MiscUtils::plot_point_sets( b_imleft_srectified, _3dpts_of_A_projectedonB, __dst, cv::Scalar( 0,255,255), false, "_3dpts_of_A_projectedonB" );
MiscUtils::plot_point_sets( __dst, _detectedpts_of_A, cv::Scalar( 255,255,255), false, ";;_detectedpts_of_A(white)" );
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)" );
// status image for __dst.
string status_msg = "im2_left_srectified;";
status_msg += "_detectedpts_of_B (red);";
status_msg += "_3dpts_of_A_projectedonB, PI( b_T_a * X),(yellow);";
status_msg += "_detectedpts_of_A(white);";
status_msg += "_3dpts_of_A_projectedonB_with_odom_rel_pose, PI( odom_b_T_a * X),(blue);";
MiscUtils::append_status_image( __dst, status_msg+";"+pnp__msg );
// proc_candi.pnp_verification_image = __dst;
proc_candi.debug_images.push_back( __dst );
proc_candi.debug_images_titles.push_back( "pnp_verification_image" );
#if __Cerebro__loopcandi_consumer__IMSHOW == 2
cv::imshow( "im2_left_srectified", __dst );
#endif
#endif
// disparity and other images
#if (__Cerebro__loopcandi_consumer__IMSHOW == 1) || (__Cerebro__loopcandi_consumer__IMSHOW == 2)
// proc_candi.node_1_disparity_viz = disparity_for_visualization; // this is disparity of im1. see code way above
proc_candi.debug_images.push_back( disparity_for_visualization );
proc_candi.debug_images_titles.push_back( "node_1_disparity_viz" );
#if __Cerebro__loopcandi_consumer__IMSHOW == 2
// cv::imshow( "imleft_srectified", imleft_srectified );
// cv::imshow( "imright_srectified", imright_srectified );
cv::imshow( "im1_disparity_for_visualization", disparity_for_visualization );
#endif
#endif
// Done
#if __Cerebro__loopcandi_consumer__IMSHOW == 2
cv::waitKey(50);
#endif
return true;
}
#endif
//////////// END pose computation //////////////////////////////
//--------------------------------------------------------------//
//------------------ END Geometry Thread -----------------------//
//--------------------------------------------------------------//
///////////////////////////////////////////////////////////
//---------------------- KIDNAP -------------------------//
///////////////////////////////////////////////////////////
// Kidnap identification thread. This thread monitors dataManager->getDataMapRef().size
// for every new node added if there are zero tracked features means that, I have
// been kidnaped. It however declares kidnap only after 2 sec of kidnaped
// #define __Cerebro__kidnaped_thread__( msg ) msg;
#define __Cerebro__kidnaped_thread__( msg ) ;
// #define __Cerebro__kidnaped_thread__debug( msg ) msg;
#define __Cerebro__kidnaped_thread__debug( msg ) ;
void Cerebro::kidnaped_thread( int loop_rate_hz )
{
if( loop_rate_hz <= 0 || loop_rate_hz >= 30 ) {
cout << TermColor::RED() << "[Cerebro::kidnaped_thead] Invalid loop_rate_hz. Expected to be between [1,30]\n" << TermColor::RESET() << endl;
return;
}
cout << TermColor::GREEN() << "Start Cerebro::kipnaped_thead\n" << TermColor::RESET() << endl;
if( dataManager->isKidnapIndicatorPubSet() == false ) {
cout << TermColor::RED() << "FATAL ERROR in [Cerebro::kidnaped_thread] kidnap indicator ros::Publishers were not set\n";
exit(2);
}
//---
//--- Main Settings for this thread
//---
const int THRESH_N_FEATS = 15; //declare kidnap if number of tracked features fall below 15
const ros::Duration WAIT_BEFORE_DECLARING_AS_KIDNAP = ros::Duration(3.0); // wait this many seconds, of low feature tracking count before declaring kidnap
ros::Rate loop_rate( loop_rate_hz );
int prev_count = 0, new_count = 0;
ros::Time last_known_keyframe;
// TODO: Move both of these to class variables and make them atomic. is_kidnapped_start is valid only when is_kidnapped==true
bool is_kidnapped = false;
bool is_kidnapped_more_than_n_sec = false;
ros::Time is_kidnapped_start;
// Related to handling to playing multiple bags one-after-another.
bool first_data_received = false;
bool waiting_for_next_bag_to_start = false;
while( b_kidnaped_thread_enable ) {
// Book Keeping
prev_count = new_count;
loop_rate.sleep();
auto data_map = dataManager->getDataMapRef(); //< map< ros::Time, DataNode*>
new_count = data_map->size();
if( new_count <= prev_count ) {
__Cerebro__kidnaped_thread__(cout << "[Cerebro::kidnaped_thread]Nothing new\n";)
continue;
}
ros::Time lb = data_map->rbegin()->first - ros::Duration(5, 0); // look at recent 5sec.
// auto S = data_map.begin();
auto S = data_map->lower_bound( lb );
auto E = data_map->end();
__Cerebro__kidnaped_thread__debug( cout << "[Cerebro::kidnaped_thread] S=" << S->first << " E=" << E->first << endl; )
for( auto it = S ; it != E ; it++ )
{
#if 0
if( it->second->isImageAvailable() ) {
cout << TermColor::GREEN();
} else { cout << TermColor::BLUE() ; }
if( it->second->isPoseAvailable() && it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) {
cout << "A";
}
if( !it->second->isPoseAvailable() && it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) {
cout << "B";
}
if( it->second->isPoseAvailable() && ! (it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) ) {
cout << "C";
}
if( !it->second->isPoseAvailable() && ! (it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) ){
cout << "D";
}
cout << TermColor::RESET() ;
#endif
int n_feats = it->second->getNumberOfSuccessfullyTrackedFeatures();
if( it->first <= last_known_keyframe || n_feats < 0 )
continue;
__Cerebro__kidnaped_thread__debug(
if( n_feats > 50 )
cout <first << ": n_feats=" << TermColor::iGREEN() << n_feats << TermColor::RESET() << endl;
if( n_feats > 30 && n_feats<=50 )
cout <first << ": n_feats=" << TermColor::GREEN() << n_feats << TermColor::RESET() << endl;
if( n_feats > 20 && n_feats<=30 )
cout <first << ": n_feats=" << TermColor::YELLOW() << n_feats << TermColor::RESET() << endl;
if( n_feats > 10 && n_feats<=20 )
cout <first << ": n_feats=" << TermColor::RED() << n_feats << TermColor::RESET() << endl;
if( n_feats<=10 )
cout <first << ": n_feats=" << TermColor::iRED() << n_feats << TermColor::RESET() << endl;
)
last_known_keyframe = it->first;
if( is_kidnapped==false && n_feats < THRESH_N_FEATS ) {
is_kidnapped = true;
is_kidnapped_start = it->first;
__Cerebro__kidnaped_thread__(
cout << TermColor::RED() << "I am kidnapped t=" << it->first << TermColor::RESET() << endl;
cout << "I think so because the number of tracked features (from feature tracker) have fallen to only "
<< n_feats << ", the threshold was " << THRESH_N_FEATS
<< ". However, I will wait for " << WAIT_BEFORE_DECLARING_AS_KIDNAP << " sec to declare kidnapped to vins_estimator." << endl;
)
}
__Cerebro__kidnaped_thread__(
if( is_kidnapped ) {
cout << "is_kidnapped=true t=" << it->first << " n_feats=" << n_feats << endl;
}
)
if( is_kidnapped && !is_kidnapped_more_than_n_sec && (it->first - is_kidnapped_start) > WAIT_BEFORE_DECLARING_AS_KIDNAP )
{
__Cerebro__kidnaped_thread__(
cout << "Kidnapped for more than " << WAIT_BEFORE_DECLARING_AS_KIDNAP << "sec. I am quite confident that I have been kidnapped\n";
cout << "PUBLISH FALSE\n";
)
is_kidnapped_more_than_n_sec = true;
dataManager->PUBLISH__FALSE( is_kidnapped_start );
}
if( is_kidnapped && n_feats > THRESH_N_FEATS ) {
__Cerebro__kidnaped_thread__(
cout << TermColor::GREEN() << "Looks like i have been unkidnapped t=" << it->first << TermColor::RESET() << endl;
)
if( is_kidnapped_more_than_n_sec )
{
// publish true to vins_estimator to indicate that it may resume the estimation with a new co-ordinate system.
dataManager->PUBLISH__TRUE( it->first );
}
is_kidnapped = false;
is_kidnapped_more_than_n_sec = false;
}
}
// cout << endl;
}
cout << TermColor::RED() << "Cerebro::kipnaped_thead Ends\n" << TermColor::RESET() << endl;
}
bool Cerebro::kidnap_info( int i, ros::Time& start_, ros::Time& end_ )
{
std::lock_guard lk(mutex_kidnap);
if( i>=0 && i lk(mutex_kidnap);
json json_obj;
json_obj["meta_info"]["state_is_kidnapped"] = (bool) state_is_kidnapped;
json_obj["meta_info"]["len_start_of_kidnap"] = start_of_kidnap.size();
json_obj["meta_info"]["len_end_of_kidnap"] = end_of_kidnap.size();
for( int i=0 ; i lk(mutex_kidnap);
return start_of_kidnap.size();
}
bool Cerebro::is_kidnapped(){ state_is_kidnapped; }
#define __CEREBRO_kidnap_callbacks(msg) msg;
// #define __CEREBRO_kidnap_callbacks(msg) ;
void Cerebro::kidnap_bool_callback( const std_msgs::BoolConstPtr& rcvd_bool )
{
return; // ignore this.
__CEREBRO_kidnap_callbacks( cout << TermColor::iCYAN() << "[Cerebro::kidnap_bool_callback] msg=" << (bool)rcvd_bool->data << TermColor::RESET() << endl; )
}
void Cerebro::kidnap_header_callback( const std_msgs::HeaderConstPtr& rcvd_header )
{
__CEREBRO_kidnap_callbacks(
cout << TermColor::iCYAN() << "[Cerebro::kidnap_header_callback]" ;
cout << rcvd_header->stamp << ":" << rcvd_header->frame_id ;
cout << TermColor::RESET() << endl;
)
if( rcvd_header->frame_id == "kidnapped" ) {
std::lock_guard lk(mutex_kidnap);
this->state_is_kidnapped = true;
start_of_kidnap.push_back( rcvd_header->stamp );
__CEREBRO_kidnap_callbacks(
cout << TermColor::iCYAN() << "start_of_kidnap.push_back("<< rcvd_header->stamp << ")" << TermColor::RESET() << endl;
)
return;
}
if( rcvd_header->frame_id == "unkidnapped" ) {
std::lock_guard lk(mutex_kidnap);
this->state_is_kidnapped = false;
end_of_kidnap.push_back( rcvd_header->stamp );
__CEREBRO_kidnap_callbacks(
cout << TermColor::iCYAN() << "end_of_kidnap.push_back("<< rcvd_header->stamp << ")" << TermColor::RESET() << endl;
)
return;
}
assert( false && "in Cerebro::kidnap_header_callback. rcvd_header->frame_id is something other than `kidnapped` or `unkidnapped`.");
}
///////////////////////////////////////////////////////////
//---------------------- KIDNAP -------------------------//
///////////////////////////////////////////////////////////
//-----------------------------------------------------------------
// Cerebro Private Utils
//-----------------------------------------------------------------
// private function for descriptor_dot_product thread
bool Cerebro::wait_until__connectedToDescServer_and_descSizeAvailable( int timeout_in_sec ) //blocking call
{
assert( timeout_in_sec > 2 );
ros::Rate rate(10);
// wait until connected_to_descriptor_server=true and descriptor_size_available=true
int wait_itr = 0;
while( true ) {
if( this->connected_to_descriptor_server && this->descriptor_size_available)
break;
__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";)
rate.sleep();
wait_itr++;
if( wait_itr > timeout_in_sec*10 ) {
__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(); )
return false;
}
}
return true;
}
//-----------------------------------------------------------------
// END Cerebro Private Utils
//-----------------------------------------------------------------
================================================
FILE: src/Cerebro.h
================================================
#pragma once
/** Cerebro Class
This class is suppose to be the main-brain of this package.
It has to run its own threads (should not block)
It can access DataManager::camera, DataManager::imu_T_cam, DataManager::data_map.
Author : Manohar Kuse
Created : 29th Oct, 2018
*/
#include
#include
#include
#include
#include
#include "PinholeCamera.h"
#include "DataManager.h"
#include "ProcessedLoopCandidate.h"
#include "DlsPnpWithRansac.h"
#include "HypothesisManager.h"
#include "utils/TermColor.h"
#include "utils/ElapsedTime.h"
#include "utils/Plot2Mat.h"
#include "utils/CameraGeometry.h"
#include "utils/PointFeatureMatching.h"
#include "utils/nlohmann/json.hpp"
using json = nlohmann::json;
// ROS-Service Defination
#include
// #include
#include
#include
using namespace Eigen;
using namespace std;
//comment this out to remove dependence on faiss.
// If using faiss, also remember to link to libfaiss.so. See my CMakeList file to know how to do it.
// #define HAVE_FAISS
#ifdef HAVE_FAISS
// faiss is only used for generating loopcandidates.
// only function that uses faiss is `Cerebro::faiss__naive_loopcandidate_generator()`
// Although faiss is strictly not needed, it is a much // faster way for nearest neighbour search. If you don't want it
// just use the naive implementation: descrip_N__dot__descrip_0_N().
// Note: Cerebro::descrip_N__dot__descrip_0_N() and Cerebro::faiss__naive_loopcandidate_generator()
// are exactly same in functionality.
#include
#endif //HAVE_FAISS
class Cerebro
{
//-------------- Constructor --------------------//
public:
Cerebro( ros::NodeHandle& nh ); // TODO removal of nh argument.
void setDataManager( DataManager* dataManager );
void setPublishers( const string base_topic_name );
private:
// global private variables
bool m_dataManager_available=false;
DataManager * dataManager;
ros::NodeHandle nh; ///< This is here so that I can set new useful publishers
bool m_pub_available = false;
ros::Publisher pub_loopedge;
//-------------- END Constructor --------------------//
//--------------- Descriptor Computation Thread ------------------//
public:
// This monitors the dataManager->data_map and makes sure the descriptor are uptodate.
// descriptors are computed by an external ros-service. in the future can have
// more similar threads to compute object bounding boxes, text and other perception related services.
void descriptor_computer_thread_enable() { b_descriptor_computer_thread = true; }
void descriptor_computer_thread_disable() { b_descriptor_computer_thread = false; }
void descriptor_computer_thread();
private:
atomic b_descriptor_computer_thread;
atomic connected_to_descriptor_server;
atomic descriptor_size_available;
atomic descriptor_size;
// Storage for Intelligence
mutable std::mutex m_wholeImageComputedList;
vector wholeImageComputedList; ///< A list of stamps where descriptors are computed and available.
void wholeImageComputedList_pushback( const ros::Time __tx ); //this is kept private on purpose so that others from outside cannot pushback here.
public:
const int wholeImageComputedList_size() const; //size of the list. threadsafe
const ros::Time wholeImageComputedList_at(int k) const; //< returns kth element of the list. threadsafe
//--------------- END Descriptor Computation Thread ------------------//
//---------------- Populate Loop Candidates --------------------//
public:
// This is supposed to be run in a separate thread.
void run_thread_enable() { b_run_thread = true; }
void run_thread_disable() { b_run_thread = false; }
void run(); //< The loopcandidate (geometrically unverified) producer.
//### Method-A : Naive method of dot product DIY
void descrip_N__dot__descrip_0_N(); //< Naive method of dot product DIY
#ifdef HAVE_FAISS
//### Method-B : same functionality to descrip_N__dot__descrip_0_N() but uses facebook's faiss library
void faiss__naive_loopcandidate_generator();
//### Method-C:
//### In this we get say 5 nearest neighbour for every l_i.
//### Collect nearest neigbours for say 4 consecutive l_i's.
//### Nearby predictions are merged.
void faiss_clique_loopcandidate_generator();
//### Method-D:
// Uses a separate hypothesis manager. My multihyp framework.
// elaborate scheme, still under development.
void faiss_multihypothesis_tracking();
#endif //HAVE_FAISS
private:
// private things to run thread
atomic b_run_thread;
bool wait_until__connectedToDescServer_and_descSizeAvailable( int timeout_in_sec ); //blocking call
public:
// These are loop candidates. These calls are thread-safe
// producer: `run()`
// user: `Visualization::publish_loopcandidates`
const int foundLoops_count() const ;
const std::tuple foundLoops_i( int i) const;
json foundLoops_as_JSON();
private:
mutable std::mutex m_foundLoops;
vector< std::tuple > foundLoops; // a list containing loop pairs. this is populated by `run()`
//---------------- END Populate Loop Candidates --------------------//
//------------------ Geometry Thread ---------------------------//
// calls this->foundLoops_count() and this->foundLoops_i() and uses dataManager
// to geometric verify and to compute the poses of loop-pairs.
public:
void loopcandidate_consumer_enable() { b_loopcandidate_consumer=true; }
void loopcandidate_consumer_disable() { b_loopcandidate_consumer=false; }
void loopcandiate_consumer_thread();
const int processedLoops_count() const;
const ProcessedLoopCandidate& processedLoops_i( int i ) const;
// TODO: have a function which returns a json of the info in processedloopcandi_list.
private:
atomic b_loopcandidate_consumer;
// helpers
// Processed foundLoops_i[ j ] and writes the info in the object `proc_candi`
// bool process_loop_candidate_imagepair( int j, ProcessedLoopCandidate& proc_candi );
// This function processes the jth loopcandidate and fills in the ProcessedLoopCandidate.
// The return status means that some poses were computed. It doesn't mean the poses were consistent.
// Infact, nothing about consistency is performed here. It just computes relative poses using 3 indipendent way.
bool process_loop_candidate_imagepair_consistent_pose_compute( int j, ProcessedLoopCandidate& proc_candi ); //< enhanced version of the above
bool init_stereogeom(); // expected to be called in loopcandiate_consumer_thread. this sets the variable `stereogeom`
bool retrive_stereo_pair( DataNode* node, cv::Mat& left_image, cv::Mat& right_image, bool bgr2gray=true );
std::shared_ptr stereogeom;
mutable std::mutex m_processedLoops;
vector< ProcessedLoopCandidate > processedloopcandi_list;
//------------------ END Geometry Thread ---------------------------//
//--------------- kidnaped identification thread ------------------//
public:
void kidnaped_thread( int loop_rate_hz=5);
void kidnaped_thread_enable() {b_kidnaped_thread_enable=true;};
void kidnaped_thread_disable() {b_kidnaped_thread_enable=false;};
bool is_kidnapped(); // gives the current (kidnap) status. threadsafe
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
json kidnap_info_as_json();
int n_kidnaps(); //< will with return length of `start_of_kidnap` current state has to be inferred by call to `is_kidnapped()`
// kidnap callbacks
void kidnap_bool_callback( const std_msgs::BoolConstPtr& rcvd_header ) ;
void kidnap_header_callback( const std_msgs::HeaderConstPtr& rcvd_header ) ;
private:
atomic b_kidnaped_thread_enable;
std::mutex mutex_kidnap;
atomic< bool > state_is_kidnapped;
vector< ros::Time > start_of_kidnap;
vector< ros::Time > end_of_kidnap;
ros::Publisher rcvd_flag_pub;
ros::Publisher kidnap_indicator_header_pub;
bool is_kidnapn_indicator_set = false;
};
================================================
FILE: src/DataManager.cpp
================================================
#include "DataManager.h"
DataManager::DataManager(ros::NodeHandle &nh )
//: out_stream(ofstream("/dev/pts/0",ios::out) )
{
this->nh = nh;
}
DataManager::DataManager(const DataManager &obj)
//: out_stream(ofstream("/dev/pts/0",ios::out) )
{
cout << "Copy constructor allocating ptr." << endl;
}
//////////////////////////////////////////////////////////////////////////////
//////////////////// Setters and Getters for global info /////////////////////
//////////////////////////////////////////////////////////////////////////////
// void DataManager::setCamera( const PinholeCamera& camera )
// {
// this->camera = camera;
//
// cout << "--- Camera Params from DataManager ---\n";
// this->camera.printCameraInfo();
// // cout << "K\n" << this->camera.e_K << endl;
// // cout << "D\n" << this->camera.e_D << endl;
// cout << "--- END\n";
// }
void DataManager::setAbstractCamera( camodocal::CameraPtr abs_camera, short cam_id )
{
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" );
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");
// this->abstract_camera = abs_camera;
this->all_abstract_cameras[cam_id] = abs_camera;
cout << "--- Abstract CameraParams(cam_id=" << cam_id << ") from DataManager ---\n";
// cout << this->abstract_camera->parametersToString() << endl;
cout << this->all_abstract_cameras[cam_id]->parametersToString() << endl;
cout << "--- END\n";
}
camodocal::CameraPtr DataManager::getAbstractCameraRef(short cam_id) const
{
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" );
// assert( abstract_camera && "[DataManager::getAbstractCameraRef] you are requesting a camera reference before setting.\n" );
// return abstract_camera;
assert( isAbstractCameraSet(cam_id) && "[DataManager::getAbstractCameraRef] you are requesting a camera reference before setting.\n");
// return this->all_abstract_cameras[cam_id]; //old non-const access
return this->all_abstract_cameras.at(cam_id);
}
bool DataManager::isAbstractCameraSet(short cam_id) const
{
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" );
if( this->all_abstract_cameras.count( cam_id ) > 0 ) {
return true;
// if( this->all_abstract_cameras[cam_id] )
// return true;
// else
// return false;
}
else {
return false;
}
}
vector DataManager::getAbstractCameraKeys() const {
vector keys;
for( auto it=this->all_abstract_cameras.begin(); it!=all_abstract_cameras.end() ; it++ ) {
keys.push_back( it->first );
}
return keys;
}
void DataManager::setCameraRelPose( Matrix4d a_T_b, std::pair pair_a_b )
{
// assert a and b abstract cameras exisits
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" );
cout << "---DataManager::setCameraRelPose---\n";
cout << "setting "<< pair_a_b.first << "_T_" << pair_a_b.second << " :::> " << PoseManipUtils::prettyprintMatrix4d( a_T_b ) << endl;
cam_relative_poses[ pair_a_b ] = a_T_b;
cout << "---DONE---\n";
}
bool DataManager::isCameraRelPoseSet( std::pair pair_a_b ) const
{
if( this->cam_relative_poses.count( pair_a_b ) > 0 )
return true;
else
return false;
}
const Matrix4d& DataManager::getCameraRelPose( std::pair pair_a_b ) const
{
assert( isCameraRelPoseSet( pair_a_b ) && "[DataManager::getCameraRelPose] make sure the rel cam pose you are requesting is available\n" );
if( !isCameraRelPoseSet( pair_a_b) ) {
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 );
exit(2);
}
// return this->cam_relative_poses[ pair_a_b ]; //old non const
return this->cam_relative_poses.at( pair_a_b );
}
vector< std::pair > DataManager::getCameraRelPoseKeys()
{
vector< pair > keys;
for( auto it=this->cam_relative_poses.begin(); it!=cam_relative_poses.end() ; it++ ) {
keys.push_back( it->first );
}
return keys;
}
const Matrix4d& DataManager::getIMUCamExtrinsic() const
{
std::lock_guard lk(global_vars_mutex);
assert( imu_T_cam_available && "[DataManager::getIMUCamExtrinsic] you request the value before setting it\n");
return imu_T_cam;
}
bool DataManager::isIMUCamExtrinsicAvailable() const
{
std::lock_guard lk(global_vars_mutex);
return imu_T_cam_available;
}
const ros::Time DataManager::getIMUCamExtrinsicLastUpdated() const
{
std::lock_guard lk(global_vars_mutex);
return imu_T_cam_stamp;
}
void DataManager::print_datamap_status( string fname ) const
{
ofstream myfile;
myfile.open (fname);
myfile << "#Nodes=" << data_map->size();
if( data_map->size() > 0 ) {
myfile << "\tBEGIN="<< data_map->begin()->first;
myfile << "\tEND="<< data_map->rbegin()->first;
}
myfile << endl;
// TODO other global info like imu_T_cam. etc.
for( auto it = data_map->begin() ; it!= data_map->end() ; it++ )
{
string s = "|";
s+= (it->second->isKeyFrame())?TermColor::iGREEN():"";
s+= (it->second->isPoseAvailable())?"P":"~";
s+= (it->second->isWholeImageDescriptorAvailable())?"D":"~";
myfile << s << TermColor::RESET();
}
myfile << endl;
myfile.close();
}
//////////////////////////////////////////////////////////////////////////////
/////////////////// Kidnap Indicator Publisher ///////////////////////////////
//////////////////////////////////////////////////////////////////////////////
bool DataManager::isKidnapIndicatorPubSet() const
{
return (bool)is_kidnapn_indicator_set ;
}
void DataManager::setKidnapIndicatorPublishers( ros::Publisher& pub_bool, ros::Publisher& pub_header )
{
rcvd_flag_pub = pub_bool;
kidnap_indicator_header_pub = pub_header;
is_kidnapn_indicator_set = true;
}
void DataManager::PUBLISH__TRUE( const ros::Time _t ) const
{
assert( is_kidnapn_indicator_set );
cout << TermColor::RED() << "[Cerebro::PUBLISH__TRUE] PUBLISH TRUE (t= " << _t << " to indicate the vins_estimator to start again.\n" << TermColor::RESET();
std_msgs::Bool bool_msg; bool_msg.data = true;
rcvd_flag_pub.publish( bool_msg );
// publish header msg
std_msgs::Header header_msg;
header_msg.stamp = _t;
header_msg.frame_id = "unkidnapped";
kidnap_indicator_header_pub.publish( header_msg );
}
void DataManager::PUBLISH__FALSE( const ros::Time _t ) const
{
assert( is_kidnapn_indicator_set );
cout << TermColor::RED() << "[Cerebro::PUBLISH__FALSE] PUBLISH FALSE (t= " << _t << " to indicate the vins_estimator to stop.\n" << TermColor::RESET();
// publish False (bool msg)
std_msgs::Bool bool_msg; bool_msg.data = false;
rcvd_flag_pub.publish( bool_msg );
// publish header message
std_msgs::Header header_msg;
header_msg.stamp = _t;
header_msg.frame_id = "kidnapped";
kidnap_indicator_header_pub.publish( header_msg );
}
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////// call backs //////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// #define __DATAMANAGER_CALLBACK_PRINT( u ) u;
#define __DATAMANAGER_CALLBACK_PRINT( u ) ;
void DataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr msg )
{
if( pose_0_available == false ) { //record the 1st pose
pose_0 = msg->header.stamp;
pose_0_available = true;
}
// __DATAMANAGER_CALLBACK_PRINT( cout << TermColor::BLUE() << "[cerebro/camera_pose_callback del]" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )
__DATAMANAGER_CALLBACK_PRINT( cout << TermColor::BLUE() << "[cerebro/camera_pose_callback]" << msg->header.stamp << "\t" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )
// push this to queue. Another thread will associate the data
pose_buf.push( msg );
return;
}
// currently not in use.
void DataManager::keyframe_pose_callback( const nav_msgs::Odometry::ConstPtr msg )
{
//__DATAMANAGER_CALLBACK_PRINT( cout << "[cerebro/camera_pose_callback del]" << msg->header.stamp - pose_0 << endl; )
__DATAMANAGER_CALLBACK_PRINT( cout << TermColor::iBLUE() << "[cerebro/keyframe_pose_callback]" << msg->header.stamp << "\t" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )
// push this to queue. Another thread will associate the data
kf_pose_buf.push( msg );
}
void DataManager::raw_image_callback( const sensor_msgs::ImageConstPtr& msg )
{
// __DATAMANAGER_CALLBACK_PRINT( cout << TermColor::GREEN() << "[cerebro/raw_image_callback del]" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )
__DATAMANAGER_CALLBACK_PRINT( cout << TermColor::GREEN() << "[cerebro/raw_image_callback]" << msg->header.stamp << "\t" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )
if( this->last_image_time != ros::Time() &&
(msg->header.stamp.toSec() - this->last_image_time.toSec() > 1.0
||
msg->header.stamp.toSec() < this->last_image_time.toSec())
)
{
cout << TermColor::iBLUE()
<< "+++++++++++++[DataManager::raw_image_callback] "
<< " curr_image.stamp - prev_image.stamp > 1.0.\n"
<< "This means an unstable stream or more usually means a new bag has started with --skip-empty in rosbagplay.\n"
<< "I will publish a FALSE t=" << 10000 << " then wait for 500ms and publish TRUE t=" << 10000
<< " TODO Complete this implementation and verify correctness."
<< TermColor::RESET() << endl;
cout << TermColor::BLUE() << "PUBLISH__FALSE\n" << TermColor::RESET();
PUBLISH__FALSE( this->last_image_time );
cout << TermColor::BLUE() << "Sleep 500ms\n" << TermColor::RESET();
// sleep( 500 ms );
std::this_thread::sleep_for (std::chrono::milliseconds(500));
cout << TermColor::BLUE() << "PUBLISH__TRUE\n" << TermColor::RESET();
PUBLISH__TRUE( msg->header.stamp );
}
img_buf.push( msg );
this->last_image_time = msg->header.stamp;
return;
}
void DataManager::raw_image_callback_1( const sensor_msgs::ImageConstPtr& msg )
{
// __DATAMANAGER_CALLBACK_PRINT( cout << "[cerebro/raw_image_callback_1 del]" << msg->header.stamp-pose_0 << endl; )
__DATAMANAGER_CALLBACK_PRINT( cout << "[cerebro/raw_image_callback_1]" << msg->header.stamp << "\t" << msg->header.stamp-pose_0 << endl; )
img_1_buf.push( msg );
return;
}
void DataManager::depth_image_callback( const sensor_msgs::ImageConstPtr& msg )
{
__DATAMANAGER_CALLBACK_PRINT(
cout << "[cerebro/depth_image_callback]" << msg->header.stamp << "\t" << msg->header.stamp-pose_0 << "\t";
cout << "depth image encoding: " << msg->encoding << endl;
)
depth_im_buf.push( msg );
return ;
}
void DataManager::extrinsic_cam_imu_callback( const nav_msgs::Odometry::ConstPtr msg )
{
// __DATAMANAGER_CALLBACK_PRINT( cout << "[cerebro/extrinsic_cam_imu_callback del]" << msg->header.stamp-pose_0 << endl; )
__DATAMANAGER_CALLBACK_PRINT( cout << "[cerebro/extrinsic_cam_imu_callback]" << msg->header.stamp << "\t" << msg->header.stamp-pose_0 << endl; )
extrinsic_cam_imu_buf.push( msg );
}
// there will be 5 channels. ch[0]: un, ch[1]: vn, ch[2]: u, ch[3]: v. ch[4]: globalid of the feature.
// cout << "\tpoints.size() : "<< msg->points.size(); // this will be N (say 92)
// cout << "\tchannels.size() : "<< msg->channels.size(); //this will be N (say 92)
// cout << "\tchannels[0].size() : "<< msg->channels[0].values.size(); //this will be 5.
// cout << "\n";
// An Example Keypoint msg
// ---
// header:
// seq: 40
// stamp:
// secs: 1523613562
// nsecs: 530859947
// frame_id: world
// points:
// -
// x: -7.59081602097
// y: 7.11367511749
// z: 2.85602664948
// .
// .
// .
// -
// x: -2.64935922623
// y: 0.853760659695
// z: 0.796766400337
// channels:
// -
// name: ''
// values: [-0.06108921766281128, 0.02294199913740158, 310.8721618652344, 260.105712890625, 2.0]
// .
// .
// .
// -
// name: ''
// values: [-0.47983112931251526, 0.8081198334693909, 218.95481872558594, 435.47357177734375, 654.0]
// -
// name: ''
// values: [0.07728647440671921, 1.0073764324188232, 344.2176208496094, 473.7791442871094, 660.0]
// -
// name: ''
// values: [-0.6801641583442688, 0.10506453365087509, 159.75746154785156, 279.6077575683594, 663.0]
void DataManager::ptcld_callback( const sensor_msgs::PointCloud::ConstPtr msg )
{
// __DATAMANAGER_CALLBACK_PRINT( cout << "[cerebro/ptcld_callback]" << msg->header.stamp << endl; )
__DATAMANAGER_CALLBACK_PRINT( cout << TermColor::YELLOW() << "[cerebro/ptcld_callback]" << msg->header.stamp << "\t" << msg->header.stamp-pose_0 << TermColor::RESET() << endl; )
ptcld_buf.push( msg );
return;
}
// currently not in use
void DataManager::tracked_feat_callback( const sensor_msgs::PointCloud::ConstPtr msg )
{
__DATAMANAGER_CALLBACK_PRINT( cout << "[cerebro/tracked_feat_callback]" << msg->header.stamp << "\t" << msg->header.stamp-pose_0 << endl; )
trackedfeat_buf.push( msg );
}
// #define __json_debuggin(msg) msg;
#define __json_debuggin(msg) ;
json DataManager::asJson()
{
json obj;
auto __data_map = this->getDataMapRef();
IOFormat CSVFormat(StreamPrecision, DontAlignCols, ", ", ",\t\n");
__json_debuggin( cout << "[DataManager::asJson] Start loop through data_map\n"; )
#if 1
// int max_dist = std::distance( __data_map.begin() , __data_map.end() ); //berks__old
int max_dist = std::distance( __data_map->begin() , __data_map->end() );
// for( auto it = __data_map.begin() ; it!= __data_map.end() ; it++ ) //berks__old
for( auto it = __data_map->begin() ; it!= __data_map->end() ; it++ )
{
// int seq_id = std::distance( __data_map.begin() , it ); // berks__old
int seq_id = std::distance( __data_map->begin() , it );
__json_debuggin(
cout << "[DataManager::asJson] " << seq_id << " max dist=" << max_dist << endl;)
json curr_obj;
curr_obj["stamp"] = it->first.toSec();
curr_obj["stamp_relative"] = ( it->first - this->getPose0Stamp() ).toSec() ;
curr_obj["seq"] = seq_id;
__json_debuggin( cout << "A\n"; )
DataNode * __u = it->second;
curr_obj["getT"] = __u->getT().toSec();
#if 0
curr_obj["getT_image"] = __u->getT_image().toSec();
#endif
// curr_obj["getT_image_1"] = __u->getT_image(1).toSec();
curr_obj["getT_pose"] = __u->getT_pose().toSec();
curr_obj["getT_uv"] = __u->getT_uv().toSec();
__json_debuggin( cout << "B\n"; )
curr_obj["isKeyFrame"] = __u->isKeyFrame();
#if 0
curr_obj["isImageAvailable"] = __u->isImageAvailable();
curr_obj["isImageAvailable_1"] = __u->isImageAvailable(1);
#endif
curr_obj["isPoseAvailable"] = __u->isPoseAvailable();
curr_obj["isPtCldAvailable"] = __u->isPtCldAvailable();
curr_obj["isUnVnAvailable"] = __u->isUnVnAvailable();
curr_obj["isUVAvailable"] = __u->isUVAvailable();
curr_obj["isFeatIdsAvailable"] = __u->isFeatIdsAvailable();
curr_obj["getNumberOfSuccessfullyTrackedFeatures"] = __u->getNumberOfSuccessfullyTrackedFeatures();
curr_obj["isWholeImageDescriptorAvailable"] = __u->isWholeImageDescriptorAvailable();
__json_debuggin( cout << "C\n"; )
if( __u->isPoseAvailable() )
{
const Matrix4d wTc = __u->getPose();
json pose_ifo;
pose_ifo["rows"] = wTc.rows();
pose_ifo["cols"] = wTc.cols();
pose_ifo["stamp"] = __u->getT_pose().toSec();
std::stringstream ss;
ss << wTc.format(CSVFormat);
pose_ifo["data"] = ss.str();
pose_ifo["data_pretty"] = PoseManipUtils::prettyprintMatrix4d(wTc);
curr_obj["w_T_c"] = pose_ifo;
}
__json_debuggin( cout << "D\n"; )
obj["DataNodes"].push_back( curr_obj );
}
#endif
__json_debuggin( cout << "[DataManager::asJson] End loop through data_map\n"; )
// Global Data
__json_debuggin( cout << "[DataManager::asJson] Logging Global data\n"; )
obj["global"]["isPose0Available"] = this->isPose0Available();
obj["global"]["Pose0Stamp"] = this->getPose0Stamp().toSec();
obj["global"]["isIMUCamExtrinsicAvailable"] = this->isIMUCamExtrinsicAvailable();
if( this->isIMUCamExtrinsicAvailable() )
{
const Matrix4d __imu_T_cam = this->getIMUCamExtrinsic();
json pose_ifo;
pose_ifo["rows"] = __imu_T_cam.rows();
pose_ifo["cols"] = __imu_T_cam.cols();
pose_ifo["stamp"] = this->getIMUCamExtrinsicLastUpdated().toSec();
std::stringstream ss;
ss << __imu_T_cam.format(CSVFormat);
pose_ifo["data"] = ss.str();
pose_ifo["data_pretty"] = PoseManipUtils::prettyprintMatrix4d(__imu_T_cam);
obj["global"]["Pose0Stamp"] = pose_ifo;
}
__json_debuggin( cout << "[DataManager::asJson] Done asJson()\n"; )
return obj;
}
string DataManager::print_queue_size( int verbose=1 ) const
{
std::stringstream buffer;
if( verbose == 1)
{
buffer << "img_buf=" << img_buf.size() << "\t";
buffer << "img_1_buf=" << img_1_buf.size() << "\t";
buffer << "depth_im_buf=" << depth_im_buf.size() << "\t";
buffer << "pose_buf=" << pose_buf.size() << "\t";
buffer << "kf_pose_buf=" << kf_pose_buf.size() << "\t";
buffer << "ptcld_buf=" << ptcld_buf.size() << "\t";
buffer << "trackedfeat_buf=" << trackedfeat_buf.size() << "\t";
buffer << "extrinsic_cam_imu_buf=" << extrinsic_cam_imu_buf.size() << "\t";
buffer << endl;
}
if( verbose == 2 )
{
buffer << "img_buf=" << img_buf.size() << " (";
if( img_buf.size() > 0 ) {
buffer << std::fixed << std::setprecision(4) << img_buf.front()->header.stamp-pose_0 << "-->";
buffer << std::fixed << std::setprecision(4) << img_buf.back()->header.stamp-pose_0 << ";";
}
buffer << ")\t";
buffer << "img_1_buf=" << img_1_buf.size() << " (";
if( img_1_buf.size() > 0 ) {
buffer << std::fixed << std::setprecision(4) << img_1_buf.front()->header.stamp-pose_0 << "-->";
buffer << std::fixed << std::setprecision(4) << img_1_buf.back()->header.stamp-pose_0 << ";";
}
buffer << ")\t";
buffer << "depth_im_buf=" << depth_im_buf.size() << " (";
if( depth_im_buf.size() > 0 ) {
buffer << std::fixed << std::setprecision(4) << depth_im_buf.front()->header.stamp-pose_0 << "-->";
buffer << std::fixed << std::setprecision(4) << depth_im_buf.back()->header.stamp-pose_0 << ";";
}
buffer << ")\t";
buffer << "pose_buf=" << pose_buf.size() << " (";
if( pose_buf.size() > 0 ) {
buffer << std::fixed << std::setprecision(4) << pose_buf.front()->header.stamp-pose_0 << "-->";
buffer << std::fixed << std::setprecision(4) << pose_buf.back()->header.stamp-pose_0 << ";";
}
buffer << ")\t";
buffer << "kf_pose_buf=" << kf_pose_buf.size() << " (";
if( kf_pose_buf.size() > 0 ) {
buffer << std::fixed << std::setprecision(4) << kf_pose_buf.front()->header.stamp-pose_0 << "-->";
buffer << std::fixed << std::setprecision(4) << kf_pose_buf.back()->header.stamp-pose_0 << ";";
}
buffer << ")\t";
buffer << "ptcld_buf=" << ptcld_buf.size() << " (";
if( ptcld_buf.size() > 0 ) {
buffer << std::fixed << std::setprecision(4) << ptcld_buf.front()->header.stamp-pose_0 << "-->";
buffer << std::fixed << std::setprecision(4) << ptcld_buf.back()->header.stamp-pose_0 << ";";
}
buffer << ")\t";
buffer << "trackedfeat_buf=" << trackedfeat_buf.size() << " (";
if( trackedfeat_buf.size() > 0 ) {
buffer << std::fixed << std::setprecision(4) << trackedfeat_buf.front()->header.stamp-pose_0 << "-->";
buffer << std::fixed << std::setprecision(4) << trackedfeat_buf.back()->header.stamp-pose_0 << ";";
}
buffer << ")\t";
buffer << "extrinsic_cam_imu_buf=" << extrinsic_cam_imu_buf.size() << " (";
if( extrinsic_cam_imu_buf.size() > 0 ) {
buffer << std::fixed << std::setprecision(4) << extrinsic_cam_imu_buf.front()->header.stamp-pose_0 << "-->";
buffer << std::fixed << std::setprecision(4) << extrinsic_cam_imu_buf.back()->header.stamp-pose_0 << ";";
}
buffer << ")\t";
buffer << "\n";
}
if( verbose == 3 )
{
buffer << "img_buf.len=" << img_buf.size() << "\t";
buffer << "pose_buf.len=" << pose_buf.size() << "\t";
buffer << "kf_pose_buf.len=" << kf_pose_buf.size() << "\t";
buffer << "ptcld_buf.len=" << ptcld_buf.size() << "\t";
buffer << "trackedfeat_buf.len=" << trackedfeat_buf.size() << "\t";
buffer << "extrinsic_cam_imu_buf.len=" << extrinsic_cam_imu_buf.size() << "\t";
buffer << endl;
if( img_buf.size() > 0 )
buffer << "img_buf.back.t=" << img_buf.back()->header.stamp-pose_0 << "\t";
if( pose_buf.size() > 0 )
buffer << "pose_buf.back.t=" << pose_buf.back()->header.stamp-pose_0 << "\t";
if( kf_pose_buf.size() > 0 )
buffer << "kf_pose_buf.back.t=" << kf_pose_buf.back()->header.stamp-pose_0 << "\t";
if( ptcld_buf.size() > 0 )
buffer << "ptcld_buf.back.t=" << ptcld_buf.back()->header.stamp-pose_0 << "\t";
if( trackedfeat_buf.size() > 0 )
buffer << "trackedfeat_buf.back.t=" << trackedfeat_buf.back()->header.stamp-pose_0 << "\t";
if( extrinsic_cam_imu_buf.size() > 0 )
buffer << "extrinsic_cam_imu_buf.back.t=" << extrinsic_cam_imu_buf.back()->header.stamp-pose_0 << "\t";
buffer << endl;
if( img_buf.size() > 0 )
buffer << "img_buf.front.t=" << img_buf.front()->header.stamp-pose_0 << "\t";
if( pose_buf.size() > 0 )
buffer << "pose_buf.front.t=" << pose_buf.front()->header.stamp-pose_0 << "\t";
if( kf_pose_buf.size() > 0 )
buffer << "kf_pose_buf.front.t=" << kf_pose_buf.front()->header.stamp-pose_0 << "\t";
if( ptcld_buf.size() > 0 )
buffer << "ptcld_buf.front.t=" << ptcld_buf.front()->header.stamp-pose_0 << "\t";
if( trackedfeat_buf.size() > 0 )
buffer << "trackedfeat_buf.front.t=" << trackedfeat_buf.front()->header.stamp-pose_0 << "\t";
if( extrinsic_cam_imu_buf.size() > 0 )
buffer << "extrinsic_cam_imu_buf.front.t=" << extrinsic_cam_imu_buf.front()->header.stamp-pose_0 << "\t";
buffer << endl;
}
return buffer.str();
}
//////////////////////////// Callback ends /////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
/////////////////////////// Thread mains /////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
#define _XOUT_ myfile
void DataManager::trial_thread( )
{
cout << TermColor::GREEN() << "Start DataManager::trial_thread "<< TermColor::RESET() << endl;
ros::Rate looprate(10);
#if 0
ofstream myfile;
myfile.open (fname);
cout << "[DataManager::trial_thread] Dump output to file: " << fname << endl;
while( b_trial_thread )
{
_XOUT_ << "trial_thread\n";
// berks__old
// auto S = data_map.begin(); //.lower_bound( lb );
// auto E = data_map.end();
auto S = data_map->begin();
auto E = data_map->end();
_XOUT_ << "S=" << S->first << " E=" << E->first << endl;
int n_green = 0, n_cyan = 0 , n_blue = 0;
for( auto it = S ; it != E ; it++ )
{
#if 1
if( it->second->isImageAvailable() && it->second->isImageAvailable(1) ) {
_XOUT_ << TermColor::GREEN();n_green++;
} else {
if( it->second->isImageAvailable() ) {
_XOUT_ << TermColor::CYAN() ;n_cyan++;
}
else {
_XOUT_ << TermColor::BLUE() ;n_blue++;
}
}
if( it->second->isPoseAvailable() && it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) {
_XOUT_ << "A" ;
// cout << it->second->getNumberOfSuccessfullyTrackedFeatures() << ",";
}
if( !it->second->isPoseAvailable() && it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) {
_XOUT_ << "B" ;
// cout << it->second->getNumberOfSuccessfullyTrackedFeatures() << ","; // this means there thise node has no pose and no tracked features data
}
if( it->second->isPoseAvailable() && ! (it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) ) {
_XOUT_ << "C" ;
// cout << it->second->getNumberOfSuccessfullyTrackedFeatures() << ",";
}
if( !it->second->isPoseAvailable() && ! (it->second->getNumberOfSuccessfullyTrackedFeatures() < 0 ) ){
_XOUT_ << "D"; // has no pose data but has tracked features. Pose actually takes some time to arrive. tracked features will be avaiaable first,
}
_XOUT_ << TermColor::RESET() ;
#endif
}
_XOUT_ << endl;
_XOUT_ << "n_green=" << n_green << " ";
_XOUT_ << "n_cyan=" << n_cyan << " ";
_XOUT_ << "n_blue=" << n_blue << " ";
_XOUT_ << endl;
looprate.sleep();
}
myfile.close();
#endif
while( b_trial_thread )
{
img_data_mgr->print_status( "/dev/pts/23");
this->print_datamap_status( "/dev/pts/24" );
looprate.sleep();
}
cout << TermColor::RED() << "END DataManager::trial_thread "<< TermColor::RESET() << endl;
}
// #define ___clean_up_cout(msg) msg;
#define ___clean_up_cout(msg) ;
void DataManager::clean_up_useless_images_thread()
{
//---
// Settings
//---
int keep_last_n_sec_in_ram = 5; //to be safe keep 10. 3-5 usually is fine.
cout << TermColor::GREEN() << "[DataManager::clean_up_useless_images_thread] Start thread "<< TermColor::RESET() << endl;
ros::Rate looprate(0.3);
// data_association_thread_disable();
while( b_clean_up_useless_images_thread )
{
looprate.sleep();
// if( data_map.begin() == data_map.end() ) { //berks__old
if( data_map->begin() == data_map->end() ) {
cout << TermColor::CYAN() << "[clean_up_useless_images_thread] no nodes" << TermColor::RESET() << endl;
continue;
}
// berks__old
// auto S = data_map.begin();
// auto E = data_map.upper_bound( data_map.rbegin()->first - ros::Duration( 3.0 ) );
auto S = data_map->upper_bound( data_map->rbegin()->first - ros::Duration( keep_last_n_sec_in_ram+5 ) );
// auto S = data_map->begin();
auto E = data_map->upper_bound( data_map->rbegin()->first - ros::Duration( keep_last_n_sec_in_ram ) );
int q=0;
___clean_up_cout( cout << "[DataManager::clean_up_useless_images_thread] " << S->first << " to " << E->first << "\t"; )
___clean_up_cout( cout << S->first-getPose0Stamp() << " to " << E->first - getPose0Stamp() << endl; )
// for( auto it = data_map.begin() ; it->first < E->first ; it++ ) { //berks__old
for( auto it = S ; it->first < E->first ; it++ ) {
#if 0 //old, where images where stored inside datanode.
if( it->second->isImageAvailable() && !it->second->isKeyFrame() ) {
// if( it->second->isImageAvailable() ) {
___clean_up_cout(
cout << TermColor::CYAN() << "deallocate_all_images with t=" << it->first << " " << q++ << TermColor::RESET() << endl;
it->second->print_image_cvinfo();
)
it->second->deallocate_all_images();
}
else {
___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; )
}
#endif
if( it->second->isKeyFrame() ) {
img_data_mgr->stashImage( "left_image", it->first );
img_data_mgr->stashImage( "right_image", it->first );
img_data_mgr->stashImage( "depth_image", it->first );
} else {
img_data_mgr->rmImage( "left_image", it->first );
img_data_mgr->rmImage( "right_image", it->first );
img_data_mgr->rmImage( "depth_image", it->first );
}
}
}
cout << TermColor::RED() << "[DataManager::clean_up_useless_images_thread] Finished Thread"<< TermColor::RESET() << endl;
}
// #define __DataManager__data_association_thread__( msg ) msg;
#define __DataManager__data_association_thread__( msg ) ;
void DataManager::data_association_thread( int max_loop_rate_in_hz )
{
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" );
cout << TermColor::GREEN() << "[DataManager::data_association_thread] Start thread" << TermColor::RESET() << endl;
assert( b_data_association_thread && "You have not enabled thread execution. Call function DataManager::data_association_thread_enable() before you spawn this thread\n");
float requested_loop_time_ms = 1000. / max_loop_rate_in_hz;
while( b_data_association_thread )
{
auto loop_start_at = std::chrono::high_resolution_clock::now();
//------------------------ Process here--------------------------------
// std::this_thread::sleep_for( std::chrono::milliseconds(2000) );
__DataManager__data_association_thread__(
cout << "---\n" ;
cout << print_queue_size(2 ) ; //< sizes of buffer queues
cout << "\t\tSize_of_data_map = "+ to_string( data_map->size() ) + "\n" ;
)
// deqeue all raw images and make DataNodes of each of them, s
while( img_buf.size() > 0 ) {
sensor_msgs::ImageConstPtr img_msg = img_buf.front();
img_buf.pop();
__DataManager__data_association_thread__(
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;
)
DataNode * n = new DataNode( img_msg->header.stamp );
#if 0
n->setImageFromMsg( img_msg );
#else
img_data_mgr->setNewImageFromMsg( "left_image", img_msg );
#endif
// data_map.insert( std::make_pair(img_msg->header.stamp, n) ); //berks__old
data_map->insert( std::make_pair(img_msg->header.stamp, n) );
}
// dequeue additional raw images
while( img_1_buf.size() > 0 ) {
sensor_msgs::ImageConstPtr img_1_msg = img_1_buf.front();
ros::Time t = img_1_msg->header.stamp;
__DataManager__data_association_thread__(
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;
)
// if( data_map.count(t) > 0 ) { //berks__old
// data_map.at( t )->setImageFromMsg( img_1_msg, 1 );
// }
if( data_map->count(t) > 0 ) {
#if 0
data_map->at( t )->setImageFromMsg( img_1_msg, 1 );
#else
img_data_mgr->setNewImageFromMsg( "right_image", img_1_msg );
#endif
img_1_buf.pop();
}
else {
ros::Time newest_T = data_map->rbegin()->first;
__DataManager__data_association_thread__( cout << "newest_T=" << newest_T << endl; ) ;
if( newest_T.toNSec() > t.toNSec() )
{
cout << "newest_T is head of the t, so just drop this image";
img_1_buf.pop();
} else {
// 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");
__DataManager__data_association_thread__(
cout << TermColor::RED() << "[DataManager::data_association_thread]";
cout << "attempting to set additional image_1 with t=" << t << " aka " << t-pose_0 << " into datanode. ";
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`.";
cout << TermColor::RESET() << endl ;
cout << TermColor::CYAN() << "so dont pop from queue.\n" << TermColor::RESET();
)
break;
}
}
}
// depth image
while( depth_im_buf.size() > 0 ) {
sensor_msgs::ImageConstPtr depth_image_msg = depth_im_buf.front();
ros::Time t = depth_image_msg->header.stamp;
__DataManager__data_association_thread__(
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;
)
// if( data_map.count(t) > 0 ) { //berks__old
// data_map.at( t )->setImageFromMsg( img_1_msg, 1 );
// }
if( data_map->count(t) > 0 ) {
img_data_mgr->setNewImageFromMsg( "depth_image", depth_image_msg );
depth_im_buf.pop();
}
else {
ros::Time newest_T = data_map->rbegin()->first;
__DataManager__data_association_thread__( cout << "newest_T=" << newest_T << endl; ) ;
if( newest_T.toNSec() > t.toNSec() )
{
cout << "newest_T is ahead of the t, so just drop this depth-image";
depth_im_buf.pop();
} else {
// 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");
__DataManager__data_association_thread__(
cout << TermColor::RED() << "[DataManager::data_association_thread]";
cout << "attempting to set additional depth_image with t=" << t << " aka " << t-pose_0 << " into datanode. ";
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`.";
cout << TermColor::RESET() << endl ;
cout << TermColor::CYAN() << "so dont pop from queue.\n" << TermColor::RESET();
)
break;
}
}
}
// dequeue all poses and set them to data_map
while( pose_buf.size() > 0 ) {
nav_msgs::Odometry::ConstPtr pose_msg = pose_buf.front();
ros::Time t = pose_msg->header.stamp;
__DataManager__data_association_thread__(
cout << ">> Attempt adding poped() pose in data_map with t=" << pose_msg->header.stamp << " ie. #####> " << pose_msg->header.stamp -pose_0 << endl;
)
// find the DataNode with this timestamp
//berks__old
// if( data_map.count( t ) > 0 ) {
// // a Node seem to exist with this t.
// data_map.at( t )->setPoseFromMsg( pose_msg );
// }
if( data_map->count( t ) > 0 ) {
// a Node seem to exist with this t.
data_map->at( t )->setPoseFromMsg( pose_msg );
pose_buf.pop();
}
else {
if( t > data_map->rbegin()->first ) {
__DataManager__data_association_thread__(
cout << "\tpose's t was not yet found in datamap. data_map->rbegin()->first=" << data_map->rbegin()->first << " ";
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";
);
break;
}
// try range search
// t-delta <= x <= t+delta. x is the map key.
__DataManager__data_association_thread__( cout << "\tsince the key (for associating pose with data_map) was not found in data_map do range_search\n"; )
//berks__old
auto __it = data_map->begin();
for( __it = data_map->begin() ; __it != data_map->end() ; __it++ ) {
ros::Duration diff = __it->first-t;
if( (diff.sec == 0 && abs(diff.nsec) < 1000000) || (diff.sec == -1 && diff.nsec > (1000000000-1000000) ) )
break;
}
// berks__old
if( __it == data_map->end() ) {
cout << TermColor::RED() << "\t`data_association_thread`:pose:(not fouind) range search failed AAA FATAL \n";
assert( false && "\tnot fouind\n");
exit(2);
}
__DataManager__data_association_thread__(
cout << "\tfind pose->t=" << t << " in data_map\n";
std::cout << "\tinsert at position " << (__it->first) << '\n';
)
if( true )
{
// berks__old
data_map->at( __it->first )->setPoseFromMsg( pose_msg );
pose_buf.pop();
}
else {
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;
assert( false && "[DataManager::data_association_thread] data_map does not seem to contain the t of pose_msg. This cannot be happening\n");
exit(2);
}
} // else of if (data_map->count(t) > 0
}
// dequeue all point clouds (these are at keyframes)
while( ptcld_buf.size() > 0 ) {
sensor_msgs::PointCloudConstPtr ptcld_msg = ptcld_buf.front();
ros::Time t = ptcld_msg->header.stamp;
__DataManager__data_association_thread__(
cout << ">> Attempt adding poped() pointcloud in data_map at t=" << t << " ie. #####> " << t - pose_0 << endl;
)
// find the DataNode with this timestamp
// berks__old
if( data_map->count( t ) > 0 ) {
// a Node seem to exist with this t.
//NOte: the ``/vins_estimator/keyframe_point`` and ``/feature_tracker/feature``
// are in different formats so be careful. vins_estimator/keyframe_point has n channels. while feature_tracker/feature has just 5 channels.
// but they both convey the same info, so be careful what exact you want.
#if 0
data_map.at( t )->setPointCloudFromMsg( ptcld_msg );
data_map.at( t )->setUnVnFromMsg( ptcld_msg );
data_map.at( t )->setUVFromMsg( ptcld_msg );
data_map.at( t )->setTrackedFeatIdsFromMsg( ptcld_msg );
data_map.at( t )->setAsKeyFrame();
ptcld_buf.pop();
#else
__DataManager__data_association_thread__(
cout << "setNumberOfSuccessfullyTrackedFeatures " << t << " " << ptcld_msg->points.size() << endl;
)
//berks__old
data_map->at( t )->setNumberOfSuccessfullyTrackedFeatures( ptcld_msg->points.size() );
data_map->at( t )->setAsKeyFrame();
ptcld_buf.pop();
#endif
}
else {
if( t > data_map->rbegin()->first ) {
__DataManager__data_association_thread__(
cout << "ptcld's t was not yet found in datamap. data_map->rbegin()->first=" << data_map->rbegin()->first << " ";
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";
);
break;
}
// try range search
// t-delta <= x <= t+delta. x is the map key.
__DataManager__data_association_thread__( cout << "\tsince the key was not found (for ptcld) in data_map do range_search\n"; )
// berks__old
auto __it = data_map->begin();
for( __it = data_map->begin() ; __it != data_map->end() ; __it++ ) {
ros::Duration diff = __it->first-t;
if( (diff.sec == 0 && abs(diff.nsec) < 1000000) || (diff.sec == -1 && diff.nsec > (1000000000-1000000) ) )
break;
}
// berks__old
if( __it == data_map->end() ) {
cout << TermColor::RED() << "\trange search failed BBB. if this occurs please report to authors (Manohar Kuse)\n";
assert( false && "\tnot fouind\n");
exit(2);
}
__DataManager__data_association_thread__(
cout << "\tfind ptcld->t=" << t << " in data_map\n";
std::cout << "\tinsert ptcld at position " << (__it->first) << '\n';
)
if( true ) {
#if 0
data_map.at( __it->first )->setPointCloudFromMsg( ptcld_msg );
data_map.at( __it->first )->setUnVnFromMsg( ptcld_msg );
data_map.at( __it->first )->setUVFromMsg( ptcld_msg );
data_map.at( __it->first )->setTrackedFeatIdsFromMsg( ptcld_msg );
data_map.at( __it->first )->setAsKeyFrame();
ptcld_buf.pop();
#else
//berks__old
data_map->at( __it->first )->setNumberOfSuccessfullyTrackedFeatures( ptcld_msg->points.size() );
data_map->at( __it->first )->setAsKeyFrame();
ptcld_buf.pop();
#endif
}
else {
__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 ;)
assert( false && "[DataManager::data_association_thread] data_map does not seem to contain the t of ptcld_msg. This cannot be happening\n");
exit(2);
}
}
}
// Deal with cam_imu_extrinsic. Store the last in global variable of this class.
bool flag = false;
nav_msgs::OdometryConstPtr __msg;
while( extrinsic_cam_imu_buf.size() > 0 ) {
// dump all
__msg = extrinsic_cam_imu_buf.front();
extrinsic_cam_imu_buf.pop();
flag = true;
}
if( flag ) {
std::lock_guard lk(global_vars_mutex);
PoseManipUtils::geometry_msgs_Pose_to_eigenmat( __msg->pose.pose, imu_T_cam );
imu_T_cam_available = true;
imu_T_cam_stamp = __msg->header.stamp;
}
//--------------------------- Done processing--------------------------
auto loop_end_at = std::chrono::high_resolution_clock::now();
std::chrono::duration ellapsed = loop_end_at-loop_start_at;
// sleep_for( requested_loop_time - ellapsed )
int sleep_for = int(requested_loop_time_ms - (float) ellapsed.count()) ;
__DataManager__data_association_thread__( cout << "Loop iteration done in "<< ellapsed.count() << " ms; sleep_for=" << sleep_for << " ms" << endl; )
if( sleep_for > 0 )
std::this_thread::sleep_for( std::chrono::milliseconds( sleep_for ) );
else {
__DataManager__data_association_thread__( cout << "Queueing in thread `data_association_thread`\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() );
}
}
cout << TermColor::RED() << "[DataManager::data_association_thread] Finished thread" << TermColor::RESET() << endl;
}
// will have DataNodes and ImageDataManager to disk for reloading later.
// Camera related info is not to be stored. Just assume next time the camera relatives are the same as the map
bool DataManager::saveStateToDisk( const string save_folder_name )
{
cout << TermColor::GREEN();
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
cout << "^^^^^^^^^^ DataManager::saveStateToDisk ^^^^^^^^^^\n";
cout << "^^^^^^^^^^ DIR=" << save_folder_name ;
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
cout << TermColor::RESET();
// TODO: rm -rf save_folder_name ; mkdir save_folder_name
string system_cmd0 = string( "rm -rf ") + save_folder_name + " && mkdir "+ save_folder_name;
const int rm_dir_err0 = RawFileIO::exec_cmd( system_cmd0 );
if ( rm_dir_err0 == -1 )
{
cout << TermColor::RED() << "[DataManager::saveStateToDiskr] Cannot mkdir folder: " << save_folder_name << "!\n" << TermColor::RESET() << endl;
cout << "So not saveing state to disk...return false\n";
return false;
}
//
// ---- Save std::map
json x_datamap;
auto __data_map = this->getDataMapRef();
IOFormat CSVFormat(FullPrecision, DontAlignCols, ", ", "\n");
cout << "data_map to json\n";
int n_isPoseAvailable = 0;
int n_isWholeImageDescriptorAvailable = 0;
int n_keyframes = 0;
for( auto it = __data_map->begin() ; it!= __data_map->end() ; it++ )
{
int seq_id = std::distance( __data_map->begin() , it );
// cout << "seq_id=" << seq_id << endl;
json curr_obj;
// curr_obj["stamp"] = it->first.toSec();
curr_obj["stampNSec"] = it->first.toNSec();
curr_obj["stamp_relative"] = ( it->first - this->getPose0Stamp() ).toSec() ;
curr_obj["seq"] = seq_id;
DataNode * __u = it->second;
if( __u->isPoseAvailable() )
{
const Matrix4d wTc = __u->getPose();
json pose_ifo;
pose_ifo["rows"] = wTc.rows();
pose_ifo["cols"] = wTc.cols();
// pose_ifo["stamp"] = __u->getT_pose().toSec();
pose_ifo["stampNSec"] = __u->getT_pose().toNSec();
std::stringstream ss;
ss << wTc.format(CSVFormat);
pose_ifo["data"] = ss.str();
pose_ifo["data_pretty"] = PoseManipUtils::prettyprintMatrix4d(wTc);
curr_obj["w_T_c"] = pose_ifo;
n_isPoseAvailable++;
}
if( __u->isWholeImageDescriptorAvailable() )
{
const VectorXd wh_img_desc = __u->getWholeImageDescriptor();
json desc_ifo;
desc_ifo["rows"] = wh_img_desc.rows();
desc_ifo["cols"] = wh_img_desc.cols();
std::stringstream ss;
ss << wh_img_desc.format(CSVFormat);
desc_ifo["data"] = ss.str();
curr_obj["wholeImageDescriptor"] = desc_ifo;
n_isWholeImageDescriptorAvailable++;
}
if( __u->isKeyFrame() )
n_keyframes++;
curr_obj["isKeyFrame"] = __u->isKeyFrame();
curr_obj["getNumberOfSuccessfullyTrackedFeatures"] = __u->getNumberOfSuccessfullyTrackedFeatures();
curr_obj["isWholeImageDescriptorAvailable"] = __u->isWholeImageDescriptorAvailable();
curr_obj["isPoseAvailable"] = __u->isPoseAvailable();
x_datamap["DataNodes"].push_back( curr_obj );
}
cout << "\t n_isPoseAvailable=" << n_isPoseAvailable << endl;
cout << "\t n_isWholeImageDescriptorAvailable=" << n_isWholeImageDescriptorAvailable << endl;
cout << "\t n_keyframes=" << n_keyframes << endl;
cout << "DONE, data_map to json\n";
//
// --- Misc Variables in class DataManager
//
// --- save img_data_mgr
// - go over all status and stash all the images that remain on RAM.
// - save ImageDataManager::status to json
cout << "img_data_mgr->stashAll()\n";
json img_mgr_json = img_data_mgr->stashAll();
cout << "DONE, img_data_mgr->stashAll()\n";
x_datamap["ImageDataManager"] = img_mgr_json;
// mv stash dir to `save_folder_name`
string system_cmd = string("mv ") + img_data_mgr->getStashDir() + " " + save_folder_name;
const int rm_dir_err = RawFileIO::exec_cmd( system_cmd );
if ( rm_dir_err == -1 )
{
cout << TermColor::RED() << "[DataManager::saveStateToDiskr] Error moving stash directory!\n" << TermColor::RESET() << endl;
}
img_data_mgr->set_rm_stash_dir_in_destructor_as_false();
//
// --- Save JSON
cout << "x_datamap[\"DataNodes\"].size()=" << x_datamap["DataNodes"].size() << endl;
cout << "x_datamap[\"ImageDataManager\"].size()=" << x_datamap["ImageDataManager"].size() << endl;
RawFileIO::write_string( save_folder_name+"/state.json", x_datamap.dump(4) );
cout << "DONE........ DataManager::saveStateToDisk ^^^^^^^^^^\n";
return true;
}
bool DataManager::loadStateFromDisk( const string save_folder_name )
{
cout << TermColor::GREEN();
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
cout << "^^^^^^^^^^ DataManager::loadStateFromDisk ^^^^^^^^^^\n";
cout << "^^^^^^^^^^ DIR=" << save_folder_name ;
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
cout << TermColor::RESET();
//---
// Load JSON
string json_fname = save_folder_name+"/state.json";
cout << TermColor::GREEN() << "[DataManager::loadStateFromDisk]Open file: " << json_fname << TermColor::RESET() << endl;
std::ifstream json_fileptr(json_fname);
if( !json_fileptr )
{
ROS_ERROR( "[DataManager::loadStateFromDisk]Cannot load from previous state" );
cout << TermColor::RED() << "[DataManager::loadStateFromDisk]Fail to open state.json file, perhaps it doesnt exist. Cannot load from previous state.\nEXIT(1)"<< endl;
exit(1);
}
json json_obj;
json_fileptr >> json_obj;
cout << "[DataManager::loadStateFromDisk]Successfully opened file and loaded data "<< json_fname << endl;
json_fileptr.close();
//---
// Setup DataNodes
int n_datanodes = json_obj["DataNodes"].size();
int n_isPoseAvailable = 0;
int n_isWholeImageDescriptorAvailable = 0;
int n_keyframes = 0;
cout << "[DataManager::loadStateFromDisk]There appear to be " << n_datanodes << " datanodes\n";
for( int i=0 ; isetPose( stamp_pose,__wtc );
n_isPoseAvailable++;
}
// descriptor
if( isWholeImageDescriptorAvailable )
{
// setWholeImageDescriptor()
VectorXd __wholeImageDescriptor;
// cout << "+++\n" << json_obj["DataNodes"][i]["wholeImageDescriptor"] << "\n+++" << endl;
bool status = RawFileIO::read_eigen_vector_fromjson( json_obj["DataNodes"][i]["wholeImageDescriptor"], __wholeImageDescriptor );
if( status == false ) {
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";
exit(1);
}
// cout << "__wholeImageDescriptor=\n" << __wholeImageDescriptor.transpose() << endl;
// exit(1);
n->setWholeImageDescriptor( __wholeImageDescriptor );
n_isWholeImageDescriptorAvailable++;
}
data_map->insert( std::make_pair(stamp, n) );
}
cout << "\t n_isPoseAvailable=" << n_isPoseAvailable << endl;
cout << "\t n_isWholeImageDescriptorAvailable=" << n_isWholeImageDescriptorAvailable << endl;
cout << "\t n_keyframes=" << n_keyframes << endl;
//---
// Misc Variables in class DataManager
//---
// Setup ImageDataManager
// mv folder to /tmp/...
string system_cmd = string("cp -r ")+save_folder_name+"/cerebro_stash "+" /tmp";
// string system_cmd = string("mv ")+save_folder_name+"/cerebro_stash "+" /tmp";
cout << TermColor::YELLOW() << "[DataManager::loadStateFromDisk] " << system_cmd << TermColor::RESET() << endl;
const int mv_dir_err = RawFileIO::exec_cmd( system_cmd );
if ( mv_dir_err == -1 )
{
cout << TermColor::RED() << "[DataManager::loadStateFromDisk] Error moving directory!\n" << TermColor::RESET() << endl;
exit(1);
}
img_data_mgr->initStashDir(false);
img_data_mgr->loadStateFromDisk( json_obj["ImageDataManager"] );
}
================================================
FILE: src/DataManager.h
================================================
#pragma once
/** DataManager - Inmemory database of data from VINS-estimator
This attempts to store all the data from VINS-estimator
in a in-memory database. Note that this has multiple threads.
The data will be maintained as a std::map with key as timestamp,
value as custom class DataNode. Other global data will also be kept.
Callbacks are also handled here.
Author : Manohar Kuse
Created : 3rd Oct, 2017
Major Update : Jun, 2018
Major Update : Oct, 2018
**/
#include
#include
#include
#include
#include
#include
#include
#include