├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include ├── Kabsch.hpp ├── MeanShift.hpp ├── data.hpp ├── features.hpp ├── forest.hpp ├── node.hpp ├── random.hpp ├── reader.hpp ├── settings.hpp └── tree.hpp └── source ├── MeanShift.cpp └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | Build 3 | .idea 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(relocforests) 3 | 4 | option(BUILD_EXAMPLE "Build example application" ON) 5 | 6 | if(MSVC) 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /W4") 8 | else() 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -std=c++11") 10 | endif() 11 | 12 | find_package(OpenCV REQUIRED) 13 | 14 | include_directories(include/) 15 | include_directories("$ENV{EIGEN_DIR}") # set EIGEN_DIR env variable 16 | 17 | file(GLOB PROJECT_HEADERS include/*.hpp) 18 | file(GLOB PROJECT_SOURCES source/MeanShift.cpp) 19 | 20 | source_group("Headers" FILES ${PROJECT_HEADERS}) 21 | source_group("Sources" FILES ${PROJECT_SOURCES}) 22 | 23 | add_library(${PROJECT_NAME} ${PROJECT_SOURCES} ${PROJECT_HEADERS}) 24 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS}) 25 | 26 | 27 | if(BUILD_EXAMPLE) 28 | add_executable(${PROJECT_NAME}_example source/main.cpp) 29 | target_link_libraries(${PROJECT_NAME}_example ${PROJECT_NAME} ${OpenCV_LIBS}) 30 | endif(BUILD_EXAMPLE) 31 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Conner Brooks 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # relocforests 2 | Scene Coordinate Regression Forests for Camera Relocalization in RGB-D Images. 3 | 4 | ## Usage 5 | ### Dependencies 6 | * Cmake 7 | * Opencv 8 | * Eigen 9 | 10 | ### Building 11 | Set an environment variable for Eigen: `EIGEN_DIR=/path/to/eigen/include`. 12 | 13 | Build like any other cmake project. 14 | 15 | ``` 16 | cd relocforests 17 | mkdir build 18 | cd build 19 | 20 | # UNIX Makefile 21 | cmake .. 22 | 23 | # Mac OSX 24 | cmake -G "Xcode" .. 25 | 26 | # Microsoft Windows 27 | cmake -G "Visual Studio 14" .. 28 | cmake -G "Visual Studio 14 Win64" .. 29 | ... 30 | ``` 31 | 32 | # Usage of Example Application 33 | 34 | The example application takes input frames and poses from the [TUM RGBD datasets](http://vision.in.tum.de/data/datasets/rgbd-dataset), 35 | and requires that your create an [association file](http://vision.in.tum.de/data/datasets/rgbd-dataset/tools) to associate the RGB, Depth, and pose information. 36 | Instructions for this process can be found [here](https://github.com/tum-vision/fastfusion). 37 | 38 | ``` 39 | # Training 40 | # depeding on your setup this could take upwards of 10 minutes to train 41 | $ ./relocforests_example ~/data/rgbd_dataset_freiburg1_desk/ train desk1.rf 42 | 43 | # Testing 44 | # test the forest we created with a different dataset 45 | $ ./relocforests_example ~/data/rgbd_dataset_freiburg1_desk2/ test desk1.rf 46 | ``` 47 | 48 | 49 | ## Citations 50 | ``` 51 | @Inprceedings {export:184826, 52 | author = {Jamie Shotton and Ben Glocker and Christopher Zach and Shahram Izadi and Antonio 53 | Criminisi and Andrew Fitzgibbon}, 54 | booktitle = {Proc. Computer Vision and Pattern Recognition (CVPR)}, 55 | month = {June}, 56 | publisher = {IEEE}, 57 | title = {Scene Coordinate Regression Forests for Camera Relocalization in RGB-D Images}, 58 | url = {http://research.microsoft.com/apps/pubs/default.aspx?id=184826}, 59 | year = {2013}, 60 | } 61 | ``` 62 | 63 | ``` 64 | @software{brooks16, 65 | author = {Conner Brooks}, 66 | title = {C++ Implementation of SCORE Forests for Camera Relocalization}, 67 | howpublished = {\url{https://github.com/isue/relocforests}}, 68 | year = {2016} 69 | } 70 | ``` 71 | 72 | This implementation uses a modified version of [MeanShift++](https://github.com/mattnedrich/MeanShift_cpp), as well as an [implementation of the Kabsch algorithm](https://github.com/oleg-alexandrov/projects/blob/master/eigen/Kabsch.cpp). 73 | 74 | -------------------------------------------------------------------------------- /include/Kabsch.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // This code is released in public domain 3 | 4 | #include 5 | 6 | // Given two sets of 3D points, find the rotation + translation + scale 7 | // which best maps the first set to the second. 8 | // Source: http://en.wikipedia.org/wiki/Kabsch_algorithm 9 | 10 | // The input 3D points are stored as columns. 11 | namespace ISUE 12 | { 13 | namespace RelocForests 14 | { 15 | inline Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) { 16 | 17 | // Default output 18 | Eigen::Affine3d A; 19 | A.linear() = Eigen::Matrix3d::Identity(3, 3); 20 | A.translation() = Eigen::Vector3d::Zero(); 21 | 22 | if (in.cols() != out.cols()) 23 | throw "Find3DAffineTransform(): input data mis-match"; 24 | 25 | // First find the scale, by finding the ratio of sums of some distances, 26 | // then bring the datasets to the same scale. 27 | double dist_in = 0, dist_out = 0; 28 | for (int col = 0; col < in.cols() - 1; col++) { 29 | dist_in += (in.col(col + 1) - in.col(col)).norm(); 30 | dist_out += (out.col(col + 1) - out.col(col)).norm(); 31 | } 32 | if (dist_in <= 0 || dist_out <= 0) 33 | return A; 34 | double scale = dist_out / dist_in; 35 | out /= scale; 36 | 37 | // Find the centroids then shift to the origin 38 | Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero(); 39 | Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero(); 40 | for (int col = 0; col < in.cols(); col++) { 41 | in_ctr += in.col(col); 42 | out_ctr += out.col(col); 43 | } 44 | in_ctr /= in.cols(); 45 | out_ctr /= out.cols(); 46 | for (int col = 0; col < in.cols(); col++) { 47 | in.col(col) -= in_ctr; 48 | out.col(col) -= out_ctr; 49 | } 50 | 51 | // SVD 52 | Eigen::MatrixXd Cov = in * out.transpose(); 53 | Eigen::JacobiSVD svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV); 54 | 55 | // Find the rotation 56 | double d = (svd.matrixV() * svd.matrixU().transpose()).determinant(); 57 | if (d > 0) 58 | d = 1.0; 59 | else 60 | d = -1.0; 61 | Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3); 62 | I(2, 2) = d; 63 | Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose(); 64 | 65 | // The final transform 66 | A.linear() = scale * R; 67 | A.translation() = scale*(out_ctr - R*in_ctr); 68 | 69 | return A; 70 | } 71 | 72 | // A function to test Find3DAffineTransform() 73 | 74 | /* 75 | void TestFind3DAffineTransform(){ 76 | 77 | // Create datasets with known transform 78 | Eigen::Matrix3Xd in(3, 100), out(3, 100); 79 | Eigen::Quaternion Q(1, 3, 5, 2); 80 | Q.normalize(); 81 | Eigen::Matrix3d R = Q.toRotationMatrix(); 82 | double scale = 2.0; 83 | for (int row = 0; row < in.rows(); row++) { 84 | for (int col = 0; col < in.cols(); col++) { 85 | in(row, col) = log(2 * row + 10.0) / sqrt(1.0*col + 4.0) + sqrt(col*1.0) / (row + 1.0); 86 | } 87 | } 88 | Eigen::Vector3d S; 89 | S << -5, 6, -27; 90 | for (int col = 0; col < in.cols(); col++) 91 | out.col(col) = scale*R*in.col(col) + S; 92 | 93 | Eigen::Affine3d A = Find3DAffineTransform(in, out); 94 | 95 | // See if we got the transform we expected 96 | if ((scale*R - A.linear()).cwiseAbs().maxCoeff() > 1e-13 || 97 | (S - A.translation()).cwiseAbs().maxCoeff() > 1e-13) 98 | throw "Could not determine the affine transform accurately enough"; 99 | } 100 | */ 101 | } 102 | } 103 | -------------------------------------------------------------------------------- /include/MeanShift.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | * MeanShift implementation created by Matt Nedrich. 4 | * project: https://github.com/mattnedrich/MeanShift_cpp 5 | * 6 | */ 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | 13 | class MeanShift { 14 | public: 15 | MeanShift() { set_kernel(NULL); } 16 | MeanShift(double (*_kernel_func)(double,double)) { set_kernel(kernel_func); } 17 | vector cluster(vector, double); 18 | private: 19 | double (*kernel_func)(double,double); 20 | void set_kernel(double (*_kernel_func)(double,double)); 21 | Eigen::Vector3d shift_point(const Eigen::Vector3d &, const vector &, double); 22 | }; 23 | -------------------------------------------------------------------------------- /include/data.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace ISUE { 11 | namespace RelocForests { 12 | 13 | /// Labeled data point for training 14 | class LabeledPixel { 15 | public: 16 | LabeledPixel(uint32_t frame, cv::Point2i pos, cv::Point3f label) 17 | : frame_(frame), pos_(pos), label_(label) 18 | {} 19 | uint32_t frame_; 20 | cv::Point2i pos_; 21 | cv::Point3f label_; 22 | }; 23 | 24 | /// struct which holds pose for calculating world coordinates 25 | /// for labeled pixels 26 | struct Pose { 27 | Eigen::Matrix3d rotation; 28 | Eigen::Vector3d position; 29 | 30 | Pose(Eigen::Matrix3d rotation, Eigen::Vector3d position) 31 | :rotation(rotation), position(position) 32 | {} 33 | }; 34 | 35 | class Data { 36 | public: 37 | Data() 38 | { 39 | num_frames_ = 0; 40 | } 41 | 42 | Data(std::string path_to_data) 43 | : data_path_(path_to_data) 44 | { 45 | num_frames_ = 0; 46 | }; 47 | 48 | cv::Mat GetRGBImage(uint32_t frame) 49 | { 50 | if (rgb_images_.count(frame)) { 51 | return rgb_images_.at(frame); 52 | } 53 | else { 54 | cv::Mat img = cv::imread(data_path_ + rgb_names_.at(frame)); 55 | rgb_images_.insert(std::pair(frame, img)); 56 | return img; 57 | } 58 | } 59 | 60 | cv::Mat GetDepthImage(uint32_t frame) 61 | { 62 | if (depth_images_.count(frame)) { 63 | return depth_images_.at(frame); 64 | } 65 | else { 66 | cv::Mat img = cv::imread(data_path_ + depth_names_.at(frame), CV_LOAD_IMAGE_ANYDEPTH); 67 | depth_images_.insert(std::pair(frame, img)); 68 | return img; 69 | } 70 | } 71 | 72 | Pose GetPose(uint32_t frame) 73 | { 74 | return poses_.at(frame); 75 | } 76 | 77 | void AddFrame(cv::Mat rgb_frame, cv::Mat depth_frame, Pose pose) 78 | { 79 | rgb_images_.insert(std::pair(num_frames_, rgb_frame)); 80 | depth_images_.insert(std::pair(num_frames_, depth_frame)); 81 | poses_.push_back(pose); 82 | num_frames_++; 83 | } 84 | 85 | void AddFrame(std::string rgb_frame, std::string depth_frame, Pose pose) 86 | { 87 | poses_.push_back(pose); 88 | depth_names_.push_back(depth_frame); 89 | rgb_names_.push_back(rgb_frame); 90 | num_frames_++; 91 | } 92 | 93 | int GetNumFrames() 94 | { 95 | return num_frames_; 96 | } 97 | 98 | /// Write all frames to a folder. Write all poses to poses.txt. 99 | void Serialize(std::string data_path) 100 | { 101 | std::ofstream o(data_path + "poses.txt"); 102 | o << GetNumFrames() << "\n"; 103 | for (int i = 0; i < GetNumFrames(); i++) { 104 | cv::Mat rgb = GetRGBImage(i); 105 | cv::Mat depth = GetDepthImage(i); 106 | cv::Mat depth_w; 107 | depth.convertTo(depth_w, CV_16U, 5000); // scaled meter values 108 | 109 | cv::imwrite(data_path + std::to_string(i) + "_rgb.png", rgb); 110 | cv::imwrite(data_path + std::to_string(i) + "_d.png", depth_w); 111 | 112 | Pose p = poses_.at(i); 113 | Eigen::Quaterniond q(p.rotation); 114 | o << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " " 115 | << p.position.x() << " " << p.position.y() << " " << p.position.z() << "\n"; 116 | } 117 | } 118 | 119 | void Deserialize(std::string data_path) 120 | { 121 | data_path_ = data_path; 122 | float qx, qy, qz, qw, tx, ty, tz; 123 | 124 | std::ifstream in(data_path + "poses.txt"); 125 | 126 | std::string num(""); 127 | getline(in, num); 128 | std::stringstream num_s(num); 129 | 130 | in >> this->num_frames_; 131 | 132 | for (int i = 1; i <= GetNumFrames(); i++) { 133 | cv::Mat rgb = cv::imread(data_path + std::to_string(i) + "_rgb.png"); 134 | cv::Mat depth = cv::imread(data_path + std::to_string(i) + "_d.png"); 135 | 136 | std::string temp(""); 137 | getline(in, temp); 138 | 139 | std::stringstream stream(temp); 140 | 141 | stream >> qx; stream >> qy; 142 | stream >> qz; stream >> qw; 143 | stream >> tx; stream >> ty; stream >> tz; 144 | 145 | Pose pose(Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix(), Eigen::Vector3d(tx, ty, tz)); 146 | 147 | AddFrame(rgb, depth, pose); 148 | } 149 | } 150 | 151 | private: 152 | std::vector depth_names_; 153 | std::vector rgb_names_; 154 | 155 | // loaded images 156 | std::map rgb_images_; 157 | std::map depth_images_; 158 | std::vector poses_; 159 | 160 | std::string data_path_; 161 | 162 | uint32_t num_frames_; 163 | }; 164 | } 165 | } 166 | -------------------------------------------------------------------------------- /include/features.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "data.hpp" 4 | #include "random.hpp" 5 | #include "settings.hpp" 6 | #include "opencv2/opencv.hpp" 7 | 8 | namespace ISUE { 9 | namespace RelocForests { 10 | 11 | class Feature { 12 | public: 13 | Feature() 14 | { 15 | offset_1_ = cv::Point2i(0,0); 16 | offset_2_ = cv::Point2i(0, 0); 17 | } 18 | 19 | Feature(cv::Point2i offset_1, cv::Point2i offset_2) 20 | : offset_1_(offset_1), offset_2_(offset_2) {}; 21 | 22 | virtual float GetResponse(cv::Mat depth_image, cv::Mat rgb_image, cv::Point2i pos, Settings &settings, bool &valid) = 0; 23 | 24 | protected: 25 | cv::Point2i offset_1_; 26 | cv::Point2i offset_2_; 27 | }; 28 | 29 | /* 30 | * Depth Feature Response Function 31 | */ 32 | 33 | template 34 | class Depth : public Feature { 35 | public: 36 | Depth(cv::Point2i offset_1, cv::Point2i offset_2) 37 | : Feature(offset_1, offset_2) 38 | {}; 39 | 40 | virtual float GetResponse(cv::Mat depth_image, cv::Mat rgb_image, cv::Point2i pos, bool &valid) 41 | { 42 | 43 | D depth_at_pos = depth_image.at(pos); 44 | cv::Point2i depth_inv_1(offset_1_.x / depth_at_pos, offset_2_.y / depth_at_pos); 45 | cv::Point2i depth_inv_2(offset_2_.x / depth_at_pos, offset_2_.y / depth_at_pos); 46 | 47 | if (depth_at_pos == 0) 48 | valid = false; 49 | 50 | D D_1 = depth_image.at(pos + depth_inv_1); 51 | D D_2 = depth_image.at(pos + depth_inv_2); 52 | 53 | return D_1 - D_2; 54 | } 55 | 56 | }; 57 | 58 | /* 59 | * Depth Adaptive RGB Feature Response Function 60 | */ 61 | 62 | template 63 | class DepthAdaptiveRGB : public Feature { 64 | public: 65 | DepthAdaptiveRGB() 66 | { 67 | color_channel_1_ = 0; 68 | color_channel_2_ = 0; 69 | tau_ = 0; 70 | } 71 | 72 | DepthAdaptiveRGB(cv::Point2i offset_1, cv::Point2i offset_2, int color_channel_1, int color_channel_2, float tau) 73 | : Feature(offset_1, offset_2), color_channel_1_(color_channel_1), color_channel_2_(color_channel_2), tau_(tau) 74 | {}; 75 | 76 | static DepthAdaptiveRGB CreateRandom(Random *random) 77 | { 78 | cv::Point2i offset_1(random->Next(-130, 130), random->Next(-130, 130)); // Value from the paper -- +/- 130 pixel meters 79 | cv::Point2i offset_2(random->Next(-130, 130), random->Next(-130, 130)); 80 | int color_channel_1 = random->Next(0, 2); 81 | int color_channel_2 = random->Next(0, 2); 82 | int tau_ = random->Next(-128, 128); 83 | return DepthAdaptiveRGB(offset_1, offset_2, color_channel_1, color_channel_2, tau_); 84 | } 85 | 86 | virtual float GetResponse(cv::Mat depth_img, cv::Mat rgb_img, cv::Point2i pos, Settings &settings, bool &valid) override 87 | { 88 | 89 | D depth_at_pos = depth_img.at(pos); 90 | float depth = (float)depth_at_pos; 91 | 92 | if (depth <= 0) { 93 | valid = false; 94 | return 0.0; 95 | } 96 | else { 97 | depth /= settings.depth_factor_; // scale value 98 | } 99 | 100 | // depth invariance 101 | cv::Point2i depth_inv_1(offset_1_.x / depth, offset_2_.y / depth); 102 | cv::Point2i depth_inv_2(offset_2_.x / depth, offset_2_.y / depth); 103 | 104 | cv::Point2i pos1 = pos + depth_inv_1; 105 | cv::Point2i pos2 = pos + depth_inv_2; 106 | 107 | int width = settings.image_width_; 108 | int height = settings.image_height_; 109 | // check bounds 110 | if (pos1.x >= width || pos1.y >= height || 111 | pos1.x < 0.0 || pos1.y < 0.0 ) { 112 | valid = false; 113 | return 0.0f; 114 | } 115 | if (pos2.x >= width || pos2.y >= height || 116 | pos2.x < 0.0 || pos2.y < 0.0) { 117 | valid = false; 118 | return 0.0f; 119 | } 120 | 121 | float I_1 = rgb_img.at(pos1)[this->color_channel_1_]; 122 | float I_2 = rgb_img.at(pos2)[this->color_channel_2_]; 123 | 124 | return I_1 - I_2; 125 | } 126 | 127 | float GetThreshold() 128 | { 129 | return tau_; 130 | } 131 | 132 | protected: 133 | int color_channel_1_, color_channel_2_; 134 | float tau_; 135 | }; 136 | 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /include/forest.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "data.hpp" 4 | #include "settings.hpp" 5 | #include "tree.hpp" 6 | #include "random.hpp" 7 | #include "Kabsch.hpp" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "opencv2/opencv.hpp" 21 | 22 | namespace ISUE { 23 | namespace RelocForests { 24 | // D depth format, RGB rgb format 25 | template 26 | class Forest { 27 | public: 28 | Forest(Data *data, Settings *settings) 29 | : data_(data), settings_(settings) 30 | { 31 | for (int i = 0; i < settings->num_trees_; ++i) 32 | forest_.push_back(new Tree()); 33 | random_ = new Random(); 34 | }; 35 | 36 | // Init forest from binary file 37 | Forest(Data *data, Settings *settings, const std::string& path) 38 | : data_(data), settings_(settings) 39 | { 40 | random_ = new Random(); 41 | 42 | std::ifstream i(path, std::ios_base::binary); 43 | if (!i.is_open()) { 44 | throw std::runtime_error("Unable to open file"); 45 | } 46 | this->Deserialize(i); 47 | } 48 | 49 | ~Forest() 50 | { 51 | delete random_; 52 | for (auto t : forest_) 53 | delete t; 54 | forest_.clear(); 55 | } 56 | 57 | void Serialize(const std::string& path) 58 | { 59 | std::ofstream o(path, std::ios_base::binary); 60 | Serialize(o); 61 | } 62 | 63 | void Serialize(std::ostream& stream) 64 | { 65 | const int majorVersion = 0, minorVersion = 0; 66 | 67 | stream.write(binaryFileHeader_, strlen(binaryFileHeader_)); 68 | stream.write((const char*)(&majorVersion), sizeof(majorVersion)); 69 | stream.write((const char*)(&minorVersion), sizeof(minorVersion)); 70 | 71 | int treeCount = settings_->num_trees_; 72 | stream.write((const char*)(&treeCount), sizeof(treeCount)); 73 | 74 | for (auto tree : forest_) 75 | tree->Serialize(stream); 76 | 77 | if (stream.bad()) 78 | throw std::runtime_error("Forest serialization failed"); 79 | } 80 | 81 | void Deserialize(std::istream& stream) 82 | { 83 | 84 | std::vector buffer(strlen(binaryFileHeader_) + 1); 85 | stream.read(&buffer[0], strlen(binaryFileHeader_)); 86 | buffer[buffer.size() - 1] = '\0'; 87 | 88 | if (strcmp(&buffer[0], binaryFileHeader_) != 0) 89 | throw std::runtime_error("Unsupported forest format."); 90 | 91 | 92 | const int majorVersion = 0, minorVersion = 0; 93 | stream.read((char*)(&majorVersion), sizeof(majorVersion)); 94 | stream.read((char*)(&minorVersion), sizeof(minorVersion)); 95 | 96 | int treeCount = 0; 97 | stream.read((char*)(&treeCount), sizeof(treeCount)); 98 | 99 | int i = 0; 100 | for (i = 0; i < treeCount; ++i) { 101 | Tree *t = new Tree(); 102 | t->Deserialize(stream); 103 | forest_.push_back(t); 104 | } 105 | } 106 | 107 | bool IsValid() 108 | { 109 | bool ret = true; 110 | for (auto tree : forest_) { 111 | ret = ret && tree->IsValid(); 112 | } 113 | return ret; 114 | } 115 | 116 | // Train time 117 | ///////////// 118 | 119 | std::vector LabelData() 120 | { 121 | std::vector labeled_data; 122 | 123 | // randomly choose frames 124 | for (int i = 0; i < settings_->num_frames_per_tree_; ++i) { 125 | int curr_frame = random_->Next(0, data_->GetNumFrames()); 126 | // randomly sample pixels per frame 127 | for (int j = 0; j < settings_->num_pixels_per_frame_; ++j) { 128 | int row = random_->Next(0, settings_->image_height_); 129 | int col = random_->Next(0, settings_->image_width_); 130 | 131 | // calc label 132 | cv::Mat rgb_image = data_->GetRGBImage(curr_frame); 133 | cv::Mat depth_image = data_->GetDepthImage(curr_frame); 134 | 135 | float Z = (float)depth_image.at(row, col) / (float)settings_->depth_factor_; 136 | float Y = (row - settings_->cy) * Z / settings_->fy; 137 | float X = (col - settings_->cx) * Z / settings_->fx; 138 | 139 | Eigen::Vector3d p_camera(X, Y, Z); 140 | Pose pose = data_->GetPose(curr_frame); 141 | Eigen::Vector3d label_e = pose.rotation * p_camera + pose.position; 142 | 143 | cv::Point3f label(label_e.x(), label_e.y(), label_e.z()); 144 | 145 | // store labeled pixel 146 | LabeledPixel pixel(curr_frame, cv::Point2i(col, row), label); 147 | labeled_data.push_back(pixel); 148 | } 149 | } 150 | return labeled_data; 151 | } 152 | 153 | 154 | void Train() 155 | { 156 | // train each tree individually 157 | int index = 1; 158 | for (auto t : forest_) { 159 | 160 | std::cout << "[Tree " << index << "] " << "Generating Training Data.\n"; 161 | std::vector labeled_data = LabelData(); 162 | 163 | // train tree with set of pixels recursively 164 | std::cout << "[Tree " << index << "] " << "Training.\n"; 165 | 166 | std::clock_t start; 167 | double duration; 168 | start = std::clock(); 169 | 170 | t->Train(data_, labeled_data, random_, settings_); 171 | 172 | duration = (std::clock() - start) / (double)CLOCKS_PER_SEC; 173 | std::cout << "[Tree " << index << "] " 174 | << "Train time: " << duration << " Seconds \n"; 175 | index++; 176 | } 177 | } 178 | 179 | // Test time 180 | //////////// 181 | 182 | class Hypothesis { 183 | public: 184 | Eigen::Affine3d pose; 185 | Eigen::Vector3d camera_space_point; 186 | Eigen::Matrix3Xd input; 187 | Eigen::Matrix3Xd output; 188 | uint32_t energy; 189 | 190 | bool operator < (const Hypothesis& h) const 191 | { 192 | return (energy < h.energy); 193 | } 194 | }; 195 | 196 | std::vector Eval(int row, int col, cv::Mat rgb_image, cv::Mat depth_image) 197 | { 198 | std::vector modes; 199 | 200 | for (auto t : forest_) { 201 | bool valid = true; 202 | Eigen::Vector3d m = t->Eval(row, col, rgb_image, depth_image, valid); 203 | if (valid) 204 | modes.push_back(m); 205 | } 206 | 207 | return modes; 208 | } 209 | 210 | 211 | uint8_t tophat_error(double val) 212 | { 213 | return !(val > 0 && val < 0.1); 214 | } 215 | 216 | 217 | std::vector CreateHypotheses(int K_init, cv::Mat rgb_frame, cv::Mat depth_frame) 218 | { 219 | std::vector hypotheses; 220 | 221 | // sample initial hypotheses 222 | for (uint16_t i = 0; i < K_init; ++i) { 223 | Hypothesis h; 224 | // 3 points 225 | h.input.resize(3, 3); 226 | h.output.resize(3, 3); 227 | 228 | for (uint16_t j = 0; j < 3; ++j) { 229 | int col = 0, row = 0; 230 | double Z = 0.0; 231 | D test = 0; 232 | do { 233 | col = random_->Next(0, settings_->image_width_); 234 | row = random_->Next(0, settings_->image_height_); 235 | test = depth_frame.at(row, col); 236 | Z = (double)test / (double)settings_->depth_factor_; 237 | } while (test == 0); 238 | 239 | double Y = (row - settings_->cy) * Z / settings_->fy; 240 | double X = (col - settings_->cx) * Z / settings_->fx; 241 | 242 | Eigen::Vector3d p_camera(X, Y, Z); 243 | h.camera_space_point = p_camera; 244 | // add to input 245 | h.input.col(j) = p_camera; 246 | 247 | std::vector modes = Eval(row, col, rgb_frame, depth_frame); 248 | if (modes.empty()) { 249 | --j; 250 | } 251 | else { 252 | Eigen::Vector3d point; 253 | if (modes.size() > 1) 254 | point = modes.at(random_->Next(0, modes.size() - 1)); 255 | else 256 | point = modes.front(); 257 | 258 | // add point to output 259 | h.output.col(j) = point; 260 | } 261 | } 262 | 263 | // kabsch algorithm 264 | Eigen::Affine3d transform = Find3DAffineTransform(h.input, h.output); 265 | h.pose = transform; 266 | h.energy = 0; 267 | hypotheses.push_back(h); 268 | } 269 | 270 | return hypotheses; 271 | } 272 | 273 | // Get pose hypothesis from depth and rgb frame 274 | Eigen::Affine3d Test(cv::Mat rgb_frame, cv::Mat depth_frame) 275 | { 276 | int K_init = 1024; 277 | int K = K_init; 278 | 279 | std::vector hypotheses = CreateHypotheses(K_init, rgb_frame, depth_frame); 280 | 281 | while (K > 1) { 282 | 283 | // sample set of B test pixels 284 | std::vector test_pixels; 285 | int batch_size = 500; // todo put in settings 286 | 287 | // sample points in test image 288 | for (int i = 0; i < batch_size; ++i) { 289 | int col = random_->Next(0, settings_->image_width_); 290 | int row = random_->Next(0, settings_->image_height_); 291 | test_pixels.push_back(cv::Point2i(col, row)); 292 | } 293 | 294 | for (auto p : test_pixels) { 295 | // evaluate forest to get modes (union) 296 | auto modes = Eval(p.y, p.x, rgb_frame, depth_frame); 297 | 298 | D test = depth_frame.at(p); 299 | double Z = (double)test / (double)settings_->depth_factor_; 300 | double Y = (p.y - settings_->cy) * Z / settings_->fy; 301 | double X = (p.x - settings_->cx) * Z / settings_->fx; 302 | 303 | Eigen::Vector3d p_camera(X, Y, Z); 304 | 305 | 306 | for (int i = 0; i < K; ++i) { 307 | double e_min = DBL_MAX; 308 | Eigen::Vector3d best_mode; 309 | Eigen::Vector3d best_camera_p; 310 | 311 | for (auto mode : modes) { 312 | Eigen::Vector3d e = mode - hypotheses.at(i).pose * p_camera; 313 | double e_norm = e.norm(); 314 | 315 | if (e_norm < e_min) { 316 | e_min = e_norm; 317 | best_mode = mode; 318 | best_camera_p = p_camera; 319 | } 320 | } 321 | 322 | // update energy 323 | hypotheses.at(i).energy += tophat_error(e_min); 324 | 325 | // inlier 326 | if (tophat_error(e_min) == 0) { 327 | 328 | if (test != 0) { 329 | // add to kabsch matrices 330 | hypotheses.at(i).input.conservativeResize(3, hypotheses.at(i).input.cols() + 1); 331 | hypotheses.at(i).input.col(hypotheses.at(i).input.cols() - 1) = best_camera_p; 332 | 333 | hypotheses.at(i).output.conservativeResize(3, hypotheses.at(i).output.cols() + 1); 334 | hypotheses.at(i).output.col(hypotheses.at(i).output.cols() - 1) = best_mode; 335 | } 336 | } 337 | } 338 | } 339 | 340 | // sort hypotheses 341 | std::sort(hypotheses.begin(), hypotheses.begin() + K); 342 | 343 | K = K / 2; // discard half 344 | 345 | // refine hypotheses (kabsch) 346 | for (int i = 0; i < K; ++i) { 347 | hypotheses.at(i).pose = Find3DAffineTransform(hypotheses.at(i).input, hypotheses.at(i).output); 348 | } 349 | } 350 | // return best pose and energy 351 | return hypotheses.front().pose; 352 | } 353 | 354 | private: 355 | Data *data_; 356 | Settings *settings_; 357 | std::vector*> forest_; 358 | Random *random_; 359 | const char* binaryFileHeader_ = "ISUE.RelocForests.Forest"; 360 | }; 361 | } 362 | 363 | } 364 | -------------------------------------------------------------------------------- /include/node.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "features.hpp" 4 | 5 | namespace ISUE { 6 | namespace RelocForests { 7 | template 8 | void Serialize_(std::ostream& o, const T& t) 9 | { 10 | o.write((const char*)(&t), sizeof(T)); 11 | } 12 | 13 | template 14 | void Deserialize_(std::istream& o, T& t) 15 | { 16 | o.read((char*)(&t), sizeof(T)); 17 | } 18 | 19 | template 20 | class Node { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | Node() 24 | { 25 | left_ = nullptr; 26 | right_ = nullptr; 27 | }; 28 | 29 | ~Node() 30 | { 31 | delete left_; 32 | delete right_; 33 | } 34 | 35 | void Serialize(std::ostream& o) const 36 | { 37 | Serialize_(o, is_leaf_); 38 | Serialize_(o, is_split_); 39 | Serialize_(o, depth_); 40 | Serialize_(o, feature_); 41 | Serialize_(o, mode_); 42 | } 43 | 44 | void Deserialize(std::istream& i) 45 | { 46 | Deserialize_(i, is_leaf_); 47 | Deserialize_(i, is_split_); 48 | Deserialize_(i, depth_); 49 | Deserialize_(i, feature_); 50 | Deserialize_(i, mode_); 51 | } 52 | 53 | bool is_leaf_ = false; 54 | bool is_split_ = false; 55 | uint32_t depth_ = 0; 56 | Node *left_; 57 | Node *right_; 58 | DepthAdaptiveRGB feature_; 59 | Eigen::Vector3d mode_; 60 | }; 61 | } 62 | } -------------------------------------------------------------------------------- /include/random.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | namespace ISUE { 8 | namespace RelocForests{ 9 | class Random 10 | { 11 | public: 12 | /// 13 | /// Creates a 'random number' generator using a seed derived from the system time. 14 | /// 15 | Random() 16 | { 17 | srand((unsigned int)(time(NULL))); 18 | } 19 | 20 | /// 21 | /// Creates a deterministic 'random number' generator using the specified seed. 22 | /// May be useful for debugging. 23 | /// 24 | Random(unsigned int seed) 25 | { 26 | srand(seed); 27 | } 28 | 29 | /// 30 | /// Generate a positive random number. 31 | int Next() 32 | { 33 | return rand(); 34 | } 35 | 36 | /// 37 | /// Generate a random number in the range [0.0, 1.0). 38 | /// 39 | double NextDouble() 40 | { 41 | return (double)(rand()) / RAND_MAX; 42 | } 43 | 44 | /// 45 | /// Generate a random integer within the sepcified range. 46 | /// 47 | /// Inclusive lower bound. 48 | /// Exclusive upper bound. 49 | int Next(int minValue, int maxValue) 50 | { 51 | return minValue + rand() % (maxValue - minValue); 52 | } 53 | 54 | }; 55 | } 56 | } -------------------------------------------------------------------------------- /include/reader.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "data.hpp" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | namespace ISUE { 18 | namespace RelocForests { 19 | 20 | /** Reader for TUM datasets 21 | todo: make generic and add TUM baseclass 22 | */ 23 | class Reader { 24 | public: 25 | Reader() {}; 26 | Reader(std::string path_to_assoc_file) 27 | {}; 28 | 29 | ~Reader() 30 | { 31 | delete data; 32 | } 33 | 34 | Data* GetData() 35 | { 36 | return data; 37 | } 38 | 39 | // Loads data from association file 40 | // returns true if error 41 | bool Load(std::string path_to_data) 42 | { 43 | data = new Data(path_to_data); 44 | std::string path_to_assoc_file_ = path_to_data; 45 | 46 | std::fstream associationFile; 47 | float junkstamp; 48 | std::string depthName; 49 | std::string rgbName; 50 | float qx, qy, qz, qw, tx, ty, tz; 51 | 52 | std::cout << "[Reader] Opening: " << path_to_assoc_file_ + "associate.txt" << "\n"; 53 | 54 | associationFile.open(path_to_assoc_file_ + "associate.txt", std::ios::in); 55 | 56 | if (!associationFile.is_open()) { 57 | std::cout << "Could not open association file.\n"; 58 | return true; 59 | } 60 | else { 61 | while (!associationFile.eof()) { 62 | std::string temp(""); 63 | getline(associationFile, temp); 64 | 65 | std::stringstream stream(temp); 66 | stream >> junkstamp; // throwaway timestamps 67 | 68 | // get pose 69 | stream >> tx; stream >> ty; stream >> tz; 70 | stream >> qx; stream >> qy; 71 | stream >> qz; stream >> qw; 72 | 73 | // rgb and depth 74 | stream >> junkstamp; 75 | stream >> depthName; 76 | 77 | stream >> junkstamp; 78 | stream >> rgbName; 79 | 80 | Pose pose(Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix(), Eigen::Vector3d(tx, ty, tz)); 81 | 82 | data->AddFrame(rgbName, depthName, pose); 83 | } 84 | 85 | std::cout << "[Reader] Read " 86 | << data->GetNumFrames() 87 | << " frames from association file: \n" 88 | << "\t" << path_to_assoc_file_ + "associate.txt" << ".\n"; 89 | } 90 | return false; 91 | } 92 | private: 93 | Data *data; 94 | }; 95 | } 96 | } 97 | -------------------------------------------------------------------------------- /include/settings.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace ISUE { 5 | namespace RelocForests { 6 | class Settings { 7 | public: 8 | // default values for TUM datasets 9 | Settings() 10 | : num_trees_(5), max_tree_depth_(16), 11 | num_frames_per_tree_(500), num_pixels_per_frame_(5000), 12 | image_width_(640), image_height_(480), 13 | depth_factor_(5000), 14 | fx(525.0f), fy(525.0f), cx(319.5f), cy(239.5f) 15 | {} 16 | 17 | Settings(int32_t width, int32_t height, int32_t factor, double fx, double fy, double cx, double cy) 18 | : num_trees_(5), max_tree_depth_(16), 19 | num_frames_per_tree_(500), num_pixels_per_frame_(5000), 20 | image_width_(width), image_height_(height), 21 | depth_factor_(factor), 22 | fx(fx), fy(fy), cx(cx), cy(cy) 23 | {} 24 | 25 | int32_t num_trees_, 26 | max_tree_depth_, 27 | num_frames_per_tree_, 28 | num_pixels_per_frame_, 29 | image_width_, 30 | image_height_, 31 | depth_factor_; // for TUM data sets 32 | 33 | // intrinsics 34 | double fx, fy, cx, cy; 35 | 36 | }; 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /include/tree.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "node.hpp" 4 | #include "random.hpp" 5 | #include "settings.hpp" 6 | #include "data.hpp" 7 | #include "MeanShift.hpp" 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace ISUE { 19 | namespace RelocForests { 20 | class Point3D { 21 | public: 22 | Point3D(double x, double y, double z) : x(x), y(y), z(z) {}; 23 | double x, y, z; 24 | }; 25 | 26 | struct hashFunc{ 27 | size_t operator()(const Point3D &k) const { 28 | size_t h1 = std::hash()(k.x); 29 | size_t h2 = std::hash()(k.y); 30 | size_t h3 = std::hash()(k.z); 31 | return (h1 ^ (h2 << 1)) ^ h3; 32 | } 33 | }; 34 | 35 | struct equalsFunc { 36 | bool operator()(const Point3D &l, const Point3D &r) const{ 37 | return (l.x == r.x) && (l.y == r.y) && (l.z == r.z); 38 | } 39 | }; 40 | 41 | typedef std::unordered_map Point3DMap; 42 | 43 | template 44 | class Tree { 45 | public: 46 | Tree() 47 | { 48 | root_ = new Node(); 49 | root_->depth_ = 0; 50 | }; 51 | 52 | ~Tree() 53 | { 54 | delete root_; 55 | }; 56 | 57 | void WriteTree(std::ostream& o, Node *node) const 58 | { 59 | if (node == nullptr) { 60 | o.write("#", sizeof('#')); 61 | return; 62 | } 63 | node->Serialize(o); 64 | WriteTree(o, node->left_); 65 | WriteTree(o, node->right_); 66 | } 67 | 68 | void Serialize(std::ostream& stream) const 69 | { 70 | const int majorVersion = 0, minorVersion = 0; 71 | 72 | stream.write(binaryFileHeader_, strlen(binaryFileHeader_)); 73 | stream.write((const char*)(&majorVersion), sizeof(majorVersion)); 74 | stream.write((const char*)(&minorVersion), sizeof(minorVersion)); 75 | 76 | //stream.write((const char*)(&settings_->max_tree_depth_), sizeof(settings_->max_tree_depth_)); 77 | 78 | WriteTree(stream, root_); 79 | } 80 | 81 | Node* ReadTree(std::istream& i) 82 | { 83 | int flag = i.peek(); 84 | char val = (char)flag; 85 | if (val == '#') { 86 | i.get(); 87 | return nullptr; 88 | } 89 | Node *tmp = new Node(); 90 | tmp->Deserialize(i); 91 | tmp->left_ = ReadTree(i); 92 | tmp->right_ = ReadTree(i); 93 | return tmp; 94 | } 95 | 96 | Tree* Deserialize(std::istream& stream) 97 | { 98 | settings_ = new Settings(); 99 | 100 | std::vector buffer(strlen(binaryFileHeader_) + 1); 101 | stream.read(&buffer[0], strlen(binaryFileHeader_)); 102 | buffer[buffer.size() - 1] = '\0'; 103 | if (strcmp(&buffer[0], binaryFileHeader_) != 0) 104 | throw std::runtime_error("Unsupported forest format."); 105 | 106 | const int majorVersion = 0, minorVersion = 0; 107 | stream.read((char*)(&majorVersion), sizeof(majorVersion)); 108 | stream.read((char*)(&minorVersion), sizeof(minorVersion)); 109 | 110 | 111 | root_ = ReadTree(stream); 112 | 113 | } 114 | 115 | bool IsValidRecurse(Node *node, bool prevSplit) 116 | { 117 | 118 | if (!node && prevSplit) 119 | return false; 120 | 121 | if (node->is_leaf_) 122 | return true; 123 | 124 | return IsValidRecurse(node->left_, node->is_split_) && IsValidRecurse(node->right_, node->is_split_); 125 | } 126 | 127 | bool IsValid() 128 | { 129 | return IsValidRecurse(root_, true); 130 | } 131 | 132 | // learner output 133 | enum DECISION { LEFT, RIGHT, TRASH }; 134 | 135 | // Evaluates weak learner. Decides whether the point should go left or right. 136 | // Returns DECISION enum value. 137 | DECISION eval_learner(DepthAdaptiveRGB feature, cv::Mat depth_image, cv::Mat rgb_image, cv::Point2i pos) 138 | { 139 | bool valid = true; 140 | float response = feature.GetResponse(depth_image, rgb_image, pos, *settings_, valid); 141 | 142 | if (!valid) // no depth or out of bounds 143 | return DECISION::TRASH; 144 | 145 | return (DECISION)(response >= feature.GetThreshold()); 146 | } 147 | 148 | // V(S) 149 | double variance(std::vector labeled_data) 150 | { 151 | if (labeled_data.size() == 0) 152 | return 0.0; 153 | double V = (1.0f / (double)labeled_data.size()); 154 | double sum = 0.0f; 155 | 156 | // calculate mean of S 157 | cv::Point3f tmp; 158 | for (auto p : labeled_data) 159 | tmp += p.label_; 160 | uint32_t size = labeled_data.size(); 161 | cv::Point3f mean(tmp.x / size, tmp.y / size, tmp.z / size); 162 | 163 | for (auto p : labeled_data) { 164 | cv::Point3f val = (p.label_ - mean); 165 | sum += val.x * val.x + val.y * val.y + val.z * val.z; 166 | } 167 | 168 | return V * sum; 169 | } 170 | 171 | 172 | // Q(S_n, \theta) 173 | double objective_function(std::vector data, std::vector left, std::vector right) 174 | { 175 | double var = variance(data); 176 | double left_val = ((double)left.size() / (double)data.size()) * variance(left); 177 | double right_val = ((double)right.size() / (double)data.size()) * variance(right); 178 | 179 | return var - (left_val + right_val); 180 | } 181 | 182 | Eigen::Vector3d GetLeafMode(std::vector S) 183 | { 184 | std::vector data; 185 | 186 | // calc mode for leaf, sub-sample N_SS = 500 187 | for (uint16_t i = 0; i < (S.size() < 500 ? S.size() : 500); i++) { 188 | auto p = S.at(i); 189 | Eigen::Vector3d point{ p.label_.x, p.label_.y, p.label_.z }; 190 | data.push_back(point); 191 | } 192 | 193 | // cluster 194 | MeanShift ms = MeanShift(nullptr); 195 | double kernel_bandwidth = 0.01f; // gaussian 196 | std::vector cluster = ms.cluster(data, kernel_bandwidth); 197 | 198 | // find mode 199 | std::vector clustered_points; 200 | for (auto c : cluster) 201 | clustered_points.push_back(Point3D(floor(c[0] * 10000) / 10000, 202 | floor(c[1] * 10000) / 10000, 203 | floor(c[2] * 10000) / 10000)); 204 | 205 | Point3DMap cluster_map; 206 | 207 | for (auto p : clustered_points) 208 | cluster_map[p]++; 209 | 210 | std::pair mode(Point3D(0.0, 0.0, 0.0), 0); 211 | 212 | for (auto p : cluster_map) 213 | if (p.second > mode.second) 214 | mode = p; 215 | 216 | return Eigen::Vector3d(mode.first.x, mode.first.y, mode.first.z); 217 | } 218 | 219 | 220 | void train_recurse(Node *node, std::vector S) 221 | { 222 | uint16_t height = node->depth_; 223 | if (S.size() == 1 || ((height == settings_->max_tree_depth_ - 1) && S.size() >= 1)) { 224 | 225 | node->mode_ = GetLeafMode(S); 226 | node->is_leaf_ = true; 227 | node->left_ = nullptr; 228 | node->right_ = nullptr; 229 | return; 230 | } 231 | 232 | uint32_t num_candidates = 5, 233 | feature = 0; 234 | double minimum_objective = DBL_MAX; 235 | 236 | std::vector > candidate_params; 237 | std::vector left_final, right_final; 238 | 239 | 240 | for (uint32_t i = 0; i < num_candidates; ++i) { 241 | 242 | // add candidate 243 | candidate_params.push_back(DepthAdaptiveRGB::CreateRandom(random_)); 244 | 245 | // partition data with candidate 246 | std::vector left_data, right_data; 247 | 248 | for (uint32_t j = 0; j < S.size(); ++j) { 249 | 250 | LabeledPixel p = S.at(j); 251 | DECISION val = eval_learner(candidate_params.at(i), data_->GetDepthImage(p.frame_), data_->GetRGBImage(p.frame_), p.pos_); 252 | 253 | switch (val) { 254 | case LEFT: 255 | left_data.push_back(S.at(j)); 256 | break; 257 | case RIGHT: 258 | right_data.push_back(S.at(j)); 259 | break; 260 | case TRASH: 261 | // do nothing 262 | break; 263 | } 264 | 265 | } 266 | 267 | // eval tree training objective function and take best 268 | // todo: ensure objective function is correct 269 | double objective = objective_function(S, left_data, right_data); 270 | 271 | if (objective < minimum_objective) { 272 | feature = i; 273 | minimum_objective = objective; 274 | left_final = left_data; 275 | right_final = right_data; 276 | } 277 | } 278 | 279 | // split went only one way 280 | if (left_final.empty()) { 281 | node->mode_ = GetLeafMode(right_final); 282 | node->is_leaf_ = true; 283 | node->left_ = nullptr; 284 | node->right_ = nullptr; 285 | return; 286 | } 287 | if (right_final.empty()) { 288 | node->mode_ = GetLeafMode(left_final); 289 | node->is_leaf_ = true; 290 | node->left_ = nullptr; 291 | node->right_ = nullptr; 292 | return; 293 | } 294 | 295 | // set feature 296 | node->is_split_ = true; 297 | node->is_leaf_ = false; 298 | node->feature_ = candidate_params.at(feature); 299 | node->left_ = new Node(); 300 | node->right_ = new Node(); 301 | node->left_->depth_ = node->right_->depth_ = node->depth_ + 1; 302 | 303 | train_recurse(node->left_, left_final); 304 | train_recurse(node->right_, right_final); 305 | } 306 | 307 | void Train(Data *data, std::vector labeled_data, Random *random, Settings *settings) 308 | { 309 | data_ = data; 310 | random_ = random; 311 | settings_ = settings; 312 | train_recurse(root_, labeled_data); 313 | } 314 | 315 | Eigen::Vector3d eval_recursive(Node **node, int row, int col, cv::Mat rgb_image, cv::Mat depth_image, bool &valid) 316 | { 317 | if ((*node)->is_leaf_) { 318 | return (*node)->mode_; 319 | } 320 | 321 | DECISION val = eval_learner((*node)->feature_, depth_image, rgb_image, cv::Point2i(col, row)); 322 | 323 | switch (val) { 324 | case LEFT: 325 | return eval_recursive(&(*node)->left_, row, col, rgb_image, depth_image, valid); 326 | break; 327 | case RIGHT: 328 | return eval_recursive(&(*node)->right_, row, col, rgb_image, depth_image, valid); 329 | break; 330 | case TRASH: 331 | valid = false; 332 | break; 333 | } 334 | } 335 | 336 | // Evaluate tree at a pixel 337 | Eigen::Vector3d Eval(int row, int col, cv::Mat rgb_image, cv::Mat depth_image, bool &valid) 338 | { 339 | auto m = eval_recursive(&root_, row, col, rgb_image, depth_image, valid); 340 | return m; 341 | } 342 | 343 | 344 | private: 345 | Node *root_; 346 | Data *data_; 347 | Random *random_; 348 | Settings *settings_; 349 | const char* binaryFileHeader_ = "ISUE.RelocForests.Tree"; 350 | }; 351 | } 352 | } 353 | -------------------------------------------------------------------------------- /source/MeanShift.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "../include/MeanShift.hpp" 4 | 5 | 6 | //#define EPSILON 0.0000001 7 | #define EPSILON 0.0001 8 | 9 | 10 | double euclidean_distance(const Eigen::Vector3d &point_a, const Eigen::Vector3d &point_b){ 11 | double total = 0; 12 | Eigen::Vector3d tmp = point_a - point_b; 13 | 14 | /* 15 | for(int i=0; i &points, double kernel_bandwidth) { 36 | Eigen::Vector3d shifted_point = point; 37 | for(int dim = 0; dim MeanShift::cluster(vector points, double kernel_bandwidth){ 58 | vector stop_moving(points.size(), false); 59 | stop_moving.reserve(points.size()); 60 | vector shifted_points = points; 61 | double max_shift_distance; 62 | do { 63 | max_shift_distance = 0; 64 | for(int i=0; i max_shift_distance){ 69 | max_shift_distance = shift_distance; 70 | } 71 | if(shift_distance <= EPSILON) { 72 | stop_moving[i] = true; 73 | } 74 | shifted_points[i] = point_new; 75 | } 76 | } 77 | //printf("max_shift_distance: %f\n", max_shift_distance); 78 | } while (max_shift_distance > EPSILON); 79 | return shifted_points; 80 | } 81 | -------------------------------------------------------------------------------- /source/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "reader.hpp" 4 | #include "settings.hpp" 5 | #include "forest.hpp" 6 | 7 | using namespace std; 8 | using namespace ISUE::RelocForests; 9 | 10 | int main(int argc, char *argv[]) { 11 | if (argc < 2) { 12 | cout << "Usage: ./relocforests_example (train|test) \n"; 13 | return 1; 14 | } 15 | 16 | // get path 17 | string data_path(argv[1]); 18 | 19 | Reader *reader = new Reader(); 20 | bool err = reader->Load(data_path); 21 | if (err) { 22 | return 1; 23 | } 24 | 25 | // train or test time 26 | string shouldTrain(argv[2]); 27 | bool train = (shouldTrain == "train"); 28 | 29 | // Get data from reader 30 | Data *data = reader->GetData(); 31 | 32 | // settings for forest 33 | Settings *settings = new Settings(640, 480, 5000, 525.0f, 525.0f, 319.5f, 239.5f); 34 | 35 | Forest *forest = nullptr; 36 | 37 | string forest_file_name; 38 | if (argv[3]) 39 | forest_file_name = string(argv[3]); 40 | else 41 | cout << "Train and Test time requires ouput/input forest file name.\n"; 42 | 43 | if (train) { 44 | forest = new Forest(data, settings); 45 | forest->Train(); 46 | forest->Serialize(forest_file_name); 47 | 48 | cout << "Is forest valid:" << forest->IsValid() << endl; 49 | } 50 | else { 51 | 52 | // load forest 53 | forest = new Forest(data, settings, forest_file_name); 54 | 55 | // check if valid 56 | if (forest->IsValid()) { 57 | cout << "Forest is valid." << endl; 58 | } 59 | else { 60 | cout << "Forest is not valid." << endl; 61 | return 1; 62 | } 63 | 64 | // Todo: test each image in test dataset and provide relevent statistics about accuracy 65 | 66 | // eval forest at frame 67 | std::clock_t start; 68 | double duration; 69 | start = std::clock(); 70 | 71 | // test a single frame 72 | Eigen::Affine3d pose = forest->Test(data->GetRGBImage(200), data->GetDepthImage(200)); 73 | 74 | duration = (std::clock() - start) / (double)CLOCKS_PER_SEC; 75 | cout << "Test time: " << duration << " Seconds \n"; 76 | 77 | // compare pose to groundtruth value 78 | auto ground_truth = data->GetPose(200); 79 | 80 | cout << "Evaluated Pose:" << endl; 81 | cout << pose.rotation() << endl << endl; 82 | cout << pose.rotation().eulerAngles(0, 1, 2) * 180 / M_PI << endl; 83 | cout << pose.translation() << endl; 84 | 85 | cout << "Ground Truth:" << endl; 86 | cout << ground_truth.rotation << endl; 87 | cout << ground_truth.rotation.eulerAngles(0, 1, 2) * 180 / M_PI << endl; 88 | cout << ground_truth.position << endl; 89 | } 90 | 91 | delete forest; 92 | delete reader; 93 | delete settings; 94 | 95 | 96 | return 0; 97 | } 98 | --------------------------------------------------------------------------------