add hfnet cpp ros inference module

hfnet_ros/.gitignore

@@ -0,0 +1,2 @@
+saved_models/
+.vscode/

hfnet_ros/CMakeLists.txt

@@ -0,0 +1,72 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hfnet_ros)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+find_package(CUDA)
+include(FindProtobuf)
+find_package(Protobuf REQUIRED "3.6.0")
+
+if (Protobuf_FOUND)
+    INCLUDE_DIRECTORIES(${PROTOBUF_INCLUDE_DIR})
+else (Protobuf_FOUND)
+    MESSAGE(FATAL_ERROR "PROTOBUF NOT FOUNED")
+endif (Protobuf_FOUND)
+
+find_package(OpenCV REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  image_transport
+  roscpp
+  rospy
+  std_msgs
+)
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES 
+  CATKIN_DEPENDS
+#  DEPENDS system_lib
+)
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${PROJECT_SOURCE_DIR}
+  "/usr/local/include/eigen3"
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+# tensorflow
+include_directories(/usr/local/include/tf)
+include_directories(/usr/local/include/tf/tensorflow)
+#include_directories(/usr/local/include/tf/bazel-tensorflow/external/eigen_archive)
+#include_directories(/usr/local/include/tf/bazel-tensorflow/external/protobuf_archive/src)
+include_directories(/usr/local/include/tf/absl)
+include_directories(/usr/local/include/tf/bazel-genfiles)
+include_directories(/usr/local/include/tf/tensorflow/cc)
+include_directories(/usr/local/include/tf/tensorflow/c)
+include_directories(/usr/local/include/tf/tensorflow/core)
+
+link_directories(/usr/local/lib)
+
+add_executable(hfnet_ros_node src/hfnet_ros_node.cpp)
+find_package(CUDA)
+if(CUDA_FOUND)
+  message("FIND_CUDA")
+  target_link_libraries(hfnet_ros_node 
+                        ${CUDA_LIBRARIES}
+                        ${PROTOBUF_LIBRARY}
+                        ${OpenCV_LIBS}
+                        /usr/local/lib/libtensorflow_cc.so
+                        /usr/local/lib/libtensorflow_framework.so
+			${catkin_LIBRARIES}
+                        )
+
+endif()
+
+

hfnet_ros/README.md

@@ -0,0 +1,72 @@
+## **A Cpp Version of HF-NET**
+
+![demo](data/hfnet-1660s-demo.gif)
+
+## **9ms in GeForce GTX 2080 Ti**
+
+## **15-20ms in GeForce GTX 1660s 6G**
+
+Since the origin HF-NET based on Python3, May be it is a little difficult integrate the code with your Cpp projects(some SLAM system). To run the `hfnet_ros` package, you need install the tensorflow from the source. For convience, you can using the docker image here to run this package.
+
+This package allows to:
+
+* Extract feature detectors and descriptors on standard image sequence
+* Integrate other slam system which written in cpp.
+* Combine the origin [hloc-cpp](https://github.com/ethz-asl/hfnet/tree/master/hloc-cpp) back-end and make a full cpp projects.
+* It takes 15-20ms in GeForce GTX 1660s 6G.
+
+### Setup
+
+* python 2.7
+* Ubuntu 16.04
+* ROS Kinetic
+* CUDA = 9.0
+* Cudnn = 7.0
+* Tensorflow cpp gpu == 1.12 (you need build and install tf1.12 from source, and copy the libs to /usr/local/lib)
+* opencv > 3.0
+
+### Docker
+
+Build tensorflow from source is annoying, So if you wanna test the code , you can using docker image.
+
+```bash
+sudo docker pull xinliangzhong/1604-tf1.2cc-cuda9.0-cudnn7.0-ros:latest
+```
+
+```bash
+sudo docker run --gpus all --name="hfnet-ros" --net=host -it -v /YOUR/PATH:/root/data xinliangzhong/1604-tf1.2cc-cuda9.0-cudnn7.0-ros:latest /bin/bash
+```
+
+
+
+### How to run
+
+
+```bash
+cd /root/data
+mkdir -p catkin_ws/src
+cd catkin_ws/src
+git clone https://github.com/TurtleZhong/hfnet_ros.git
+
+# fetch model
+cd hfnet_ros/model/
+chmod +x download.sh
+./download.sh
+
+catkin_make -DCMAKE_BUILD_TYPE=Release
+```
+
+```bash
+cd /root/data/catkin_ws/
+source devel/setup.bash
+roslaunch hfnet_ros hfnet_ros_node.launch
+```
+Then you will get this in terminal.
+```bash
+...
+detected: 817 pts
+IMAGE[480 x 752] cost: 18 ms
+...
+```
+
+The results image is publish as ros msg. if your start docker container using `--net=host`, you can using rqt to view the results.

hfnet_ros/include/hfnet_ros/tensorflow_net.h

@@ -0,0 +1,152 @@
+#ifndef HFNET_TENSORFLOW_NET_H_
+#define HFNET_TENSORFLOW_NET_H_
+
+#include <vector>
+#include <memory>
+#include <iostream>
+
+
+#include <tensorflow/core/platform/env.h>
+#include <tensorflow/core/public/session.h>
+#include <tensorflow/core/graph/graph_def_builder.h>
+#include <tensorflow/core/framework/tensor.h>
+#include <tensorflow/core/framework/graph.pb.h>
+#include <tensorflow/cc/saved_model/loader.h>
+#include <tensorflow/cc/saved_model/tag_constants.h>
+
+#include <tensorflow/c/c_api.h>
+
+#include <opencv2/opencv.hpp>
+
+#include <Eigen/Core>
+
+
+using tensorflow::Status;
+using tensorflow::Tensor;
+using namespace tensorflow;
+
+class TensorflowNet {
+  public:
+    typedef Eigen::Matrix<float, Eigen::Dynamic, 1> GlobalDescType;
+    typedef Eigen::Matrix<float, Eigen::Dynamic, 2, Eigen::RowMajor> KeyPointType;
+    typedef Eigen::Matrix<float, Eigen::Dynamic, 256, Eigen::RowMajor> LocalDescType;
+
+    TensorflowNet(const std::string model_path ,
+                  const unsigned int topk_num = 2000,
+                  const unsigned int nms_radius = 4):
+                topk_kpts_(topk_num), nms_radius_(nms_radius) {
+        CHECK(tensorflow::MaybeSavedModelDirectory(model_path));
+        input_channels_ = 1;
+        descriptor_size_ = 4096;
+        std::string resample_path = "/usr/local/lib/_resampler_ops.so";
+        TF_Status* tf_status = TF_NewStatus();
+
+
+        TF_LoadLibrary(resample_path.c_str(),tf_status);
+
+        std::cout << "TF_GetCode(tf_status): " << TF_GetCode(tf_status) << std::endl;
+
+        LOG(INFO) << "HERE";
+
+        if (TF_GetCode(tf_status) !=TF_OK) {
+            std::cerr << "TF_LoadLibrary  _resampler_ops.so ERROR, Load resampler.so failed, Please check.\n";
+        }
+
+        // Load model
+        tensorflow::SessionOptions session_options;
+        // tensorflow::graph::SetDefaultDevice("/gpu:0", &graph_def);
+        session_options.config.mutable_gpu_options()->set_allow_growth(true);
+        session_options.config.mutable_gpu_options()->set_per_process_gpu_memory_fraction(0.5);
+        Status status = tensorflow::LoadSavedModel(
+                session_options, tensorflow::RunOptions(),
+                model_path, {tensorflow::kSavedModelTagServe}, &bundle_);
+        if (!status.ok())
+            LOG(FATAL) << status.ToString();
+
+        // Check input and output shapes
+        tensorflow::GraphDef graph_def = bundle_.meta_graph_def.graph_def();
+        bool found_input = false, found_output = false;
+        for (auto& node : graph_def.node()) {
+                // check graph here
+            if(node.name() == "global_descriptor")
+                std::cout << "global_desc: " << node.name() << std::endl;
+            // std::cout << node.name() << std::endl;
+        }
+
+        std::cout << "load suscessfully" << std::endl;
+    }
+
+    void PerformInference(const cv::Mat& image, int &kpts_num, GlobalDescType* global_desc, KeyPointType* key_points, LocalDescType* local_desc) {
+        CHECK(image.data);
+        CHECK(image.isContinuous());
+
+        unsigned height = image.size().height, width = image.size().width;
+        // TODO: cleaner way to avoid copying when the image is already float
+        // or combine with tensor creation
+        cv::Mat *float_image_ptr, tmp;
+        if(image.type() != CV_32F) {
+            image.convertTo(tmp, CV_32F);
+            float_image_ptr = &tmp;
+        } else {
+            float_image_ptr = &const_cast<cv::Mat&>(image);
+        }
+
+        // Prepare input tensor
+        Tensor input_tensor(
+                tensorflow::DT_FLOAT,
+                tensorflow::TensorShape({1, height, width, input_channels_}));
+        Tensor keypoints_num(tensorflow::DT_INT32,TensorShape());
+        Tensor radius(tensorflow::DT_INT32,TensorShape());
+        keypoints_num.scalar<int>()() = 2000;
+        radius.scalar<int>()() = 4;
+        // TODO: avoid copy if possible
+        tensorflow::StringPiece tmp_data = input_tensor.tensor_data();
+        std::memcpy(const_cast<char*>(tmp_data.data()), float_image_ptr->data,
+                    height * width * input_channels_ * sizeof(float));
+
+
+        // Run inference here average 15-20ms in 1660 
+        std::vector<Tensor> outputs;
+        Status status = bundle_.session->Run({{"image:0", input_tensor},{"pred/simple_nms/radius", radius},{"pred/top_k_keypoints/k",keypoints_num}},
+                                             {"global_descriptor", "keypoints", "local_descriptors"}, {}, &outputs);
+
+        if (!status.ok())
+            LOG(FATAL) << status.ToString();
+        // std::cout << "inference done" << std::endl;
+        // Copy result
+        float *descriptor_ptr = outputs[0].flat<float>().data();
+        Eigen::Map<GlobalDescType> descriptor_map(descriptor_ptr, descriptor_size_);
+        Eigen::Matrix<float, Eigen::Dynamic, 1> desc =
+                        Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>>(descriptor_ptr, 4096, 1);
+        int kp_num = outputs[1].shape().dim_size(1);
+        kpts_num = kp_num;
+        std::cout << "detected: " << kp_num << " pts" << std::endl;
+        *global_desc = descriptor_map;  // Copy
+        int *kpts_ptr = outputs[1].flat<int>().data();
+        Eigen::Matrix<int, Eigen::Dynamic, 2, Eigen::RowMajor> kpts_int =
+                        Eigen::Map<Eigen::Matrix<int, Eigen::Dynamic, 2, Eigen::RowMajor>>(kpts_ptr, kp_num, 2);
+        Eigen::MatrixXf kpts = kpts_int.cast<float>();
+        *key_points = kpts;  // Copy
+
+
+        float *local_desc_ptr = outputs[2].flat<float>().data();
+        static const int dim = 256;
+        Eigen::Matrix<float, Eigen::Dynamic, dim, Eigen::RowMajor> kpts_desc =
+                        Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, dim, Eigen::RowMajor>>(local_desc_ptr, kp_num, dim);
+        *local_desc = kpts_desc;  // Copy
+    }
+
+    unsigned descriptor_size() {
+        return descriptor_size_;
+    }
+
+  private:
+    tensorflow::SavedModelBundle bundle_;
+    unsigned descriptor_size_;
+    unsigned input_channels_;
+    unsigned int topk_kpts_;
+    unsigned int nms_radius_;
+};
+
+#endif  // HFNET_TENSORFLOW_NET_H_
+

hfnet_ros/launch/hfnet_ros_node.launch

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<launch>
+    <arg name="bag_file" default="$(find hfnet_ros)/data/sample.bag"/>
+    <node pkg="hfnet_ros" type="hfnet_ros_node" name="hfnet_ros_node" output="screen">
+        <param name="model_dir" 	type="str" value="$(find hfnet_ros)/model/saved_models/hfnet" />
+        <!-- <param name="model_dir" 	type="str"   value="/root/data/hfnet/hfnet_tf/saved_models/hfnet" /> -->
+	<param name="test_image"        value="$(find hfnet_ros)/data/1.png"/>
+	<param name="input_img_topic"	value="/cam0/image_raw"/>
+        <param name="topk_kpts_num"     type="int"   value="2000"/>
+        <param name="nms_radius"        type="int"   value="4"/>
+        <param name="opencv_show_image" type="bool"  value="false"/>
+    </node>
+
+    <node pkg="rosbag" type="play" name="rosbag" args="--clock $(arg bag_file) -l" />
+</launch>

hfnet_ros/model/download.sh

@@ -0,0 +1,10 @@
+#!/bin/sh
+echo "Download hfnet model..."
+wget http://robotics.ethz.ch/~asl-datasets/2019_CVPR_hierarchical_localization/hfnet_tf.tar.gz
+echo "Download suscess! start extract..."
+tar -xavf hfnet_tf.tar.gz
+
+rm -rf hfnet
+rm *.gz
+
+echo "Done!"