MaYixiao/GeoYolo-SLAM
0
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

删除文件夹

Browse Source
This commit is contained in:
miao 2025-04-17 15:02:54 +08:00
parent aed223518c
commit 836d4112e4
1637 changed files with 0 additions and 8178991 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
SLAM/.gitignore vendored
View File

@ -1,47 +0,0 @@
.vscode
CMakeLists.txt.user
CMakeLists_modified.txt
Examples/RGB-D/rgbd_tum
Examples/Monocular/mono_euroc
Examples/Monocular/mono_kitti
Examples/Monocular/mono_tum
Examples/Monocular/mono_tum_vi
Examples/Stereo/stereo_euroc
Examples/Stereo/stereo_kitti
Examples/Stereo/stereo_tum_vi
Examples/Monocular-Inertial/mono_inertial_euroc
Examples/Monocular-Inertial/mono_inertial_tum_vi
Examples/Stereo-Inertial/stereo_inertial_euroc
Examples/Stereo-Inertial/stereo_inertial_tum_vi
Examples/ROS/ORB_SLAM3/Mono
Examples/ROS/ORB_SLAM3/MonoAR
Examples/ROS/ORB_SLAM3/RGBD
Examples/ROS/ORB_SLAM3/Stereo
Examples/ROS/ORB_SLAM3/CMakeLists.txt.user
Examples/ROS/ORB_SLAM3/build/
KeyFrameTrajectory.txt
CameraTrajectory.txt
kf_*.txt
f_*.txt
Thirdparty/DBoW2/build/
Thirdparty/DBoW2/lib/
Thirdparty/g2o/build/
Thirdparty/g2o/config.h
Thirdparty/g2o/lib/
Vocabulary/ORBvoc.txt
build/
lib/
cmake-build-debug/
*.pyc

296
SLAM/CMakeLists.txt
View File

@ -1,296 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(rds_slam)
# enable this if you want to debug using gdb
# SET(CMAKE_BUILD_TYPE Debug)
# IF(NOT CMAKE_BUILD_TYPE)
# SET(CMAKE_BUILD_TYPE Release)
# ENDIF()
SET(CMAKE_BUILD_TYPE Release)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
# set optimization level to -O1 if you want to debug using gdb
# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O1 -march=native ")
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O1 -march=native")
# Check C++11 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS "Using flag -std=c++0x.")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
cv_bridge
image_transport
tf
sensor_msgs
dynamic_reconfigure
message_generation
maskrcnn_ros
segnet_ros
ultralytics_ros
)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
find_package(OpenCV 3.3.1)
find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
find_package(Glog REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## 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 exec_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 exec_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
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## 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 your 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 rds_slam
CATKIN_DEPENDS roscpp rospy std_msgs cv_bridge image_transport tf sensor_msgs dynamic_reconfigure message_generation segnet_ros maskrcnn_ros ultralytics_ros
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# message("Project Source Directory: ${PROJECT_SOURCE_DIR}")
# message("${catkin_INCLUDE_DIRS}: ${catkin_INCLUDE_DIRS}")
include_directories(
include
${catkin_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${PROJECT_SOURCE_DIR}/include/CameraModels
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${GLOG_INCLUDE_DIR}
# TODO not use absolute path
/root/catkin_ws/devel/include
)
aux_source_directory(src DIR_LIB_SRCS)
add_subdirectory(Thirdparty/g2o)
add_subdirectory(Thirdparty/DBoW2)
# Declare a C++ library
add_library(${PROJECT_NAME}
${DIR_LIB_SRCS}
src/CameraModels/Pinhole.cpp
src/CameraModels/KannalaBrandt8.cpp
include/CameraModels/GeometricCamera.h
include/CameraModels/Pinhole.h
include/CameraModels/KannalaBrandt8.h
)
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
${catkin_LIBRARIES}
${Boost_LIBRARIES}
${GLOG_LIBRARY}
-lboost_serialization
-lcrypto
# aislamwrapper
)
add_executable(Dynamic_RGBD Examples/ros_tum_rgbd.cpp)
add_dependencies(Dynamic_RGBD ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(Dynamic_RGBD
${PROJECT_NAME}
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/orbslam_ros_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## 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 for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_orbslam_ros.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)

197
SLAM/Examples/ros_tum_rgbd.cpp
View File

@ -1,197 +0,0 @@
/*
* Copyright (C) 2021, Yubao Liu, AISL, TOYOHASHI UNIVERSITY of TECHNOLOGY
* Email: yubao.liu.ra@tut.jp
*
*/
#include "Semantic.h"
#include "System.h"
#include <algorithm>
#include <chrono>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ros/ros.h>
using namespace std;
using namespace ORB_SLAM3;
void spin_thread()
{
ros::spinOnce();
usleep(1);
}
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]);
ros::init(argc, argv, "tum_rgbd_raw");
ros::start();
ros::NodeHandle nh("~");
if (argc < 5) {
cerr << endl
<< "Usage: EXE ./Dynamic_RGBD path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
ros::shutdown();
return 1;
}
std::string rgb_image_topic, depth_image_topic, bagfile_name;
std::cout << "===========================" << std::endl;
std::cout << "argv[1]: " << argv[1] << std::endl;
std::cout << "argv[2]: " << argv[2] << std::endl;
std::cout << "argv[3]: " << argv[3] << std::endl;
std::cout << "argv[4]: " << argv[4] << std::endl;
std::cout << "argv[5] result path: " << argv[5] << std::endl;
LOG(INFO) << "---------Parameters---------------";
LOG(INFO) << "argv[1]: " << argv[1];
LOG(INFO) << "argv[2]: " << argv[2];
LOG(INFO) << "argv[3]: " << argv[3];
LOG(INFO) << "argv[4]: " << argv[4];
LOG(INFO) << "argv[5] result path: " << argv[5];
LOG(INFO) << "----------------------------------";
std::cout << "===========================" << std::endl;
// save result
if (argc == 6) {
Config::GetInstance()->IsSaveResult(true);
Config::GetInstance()->createSavePath(std::string(argv[5]));
} else {
Config::GetInstance()->IsSaveResult(false);
}
std::thread(spin_thread);
// Retrieve paths to images
std::vector<std::string> vstrImageFilenamesRGB;
std::vector<std::string> vstrImageFilenamesD;
std::vector<double> vTimestamps;
std::string strAssociationFilename = std::string(argv[4]);
Config::GetInstance()->LoadTUMDataset(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);
// Check consistency in the number of images and depthmaps
int nImages = vstrImageFilenamesRGB.size();
if (vstrImageFilenamesRGB.empty()) {
cerr << endl
<< "No images found in provided path." << endl;
return 1;
} else if (vstrImageFilenamesD.size() != vstrImageFilenamesRGB.size()) {
cerr << endl
<< "Different number of images for rgb and depth." << endl;
return 1;
}
std::string cnn_method = "yolov8"; // myx maskrcnn segnet
// SegNet case do not need set the "init_delay" and "frame_delay"
// delay for the initial few frames
float init_delay = 0.05; //usec
int init_frames = 2; //usec
// delay for each frame
float frame_delay = 0;
nh.getParam("cnn_method", cnn_method);
// control the framerate
nh.getParam("init_delay", init_delay);
nh.getParam("frame_delay", frame_delay);
nh.getParam("init_frames", init_frames);
LOG(INFO) << "Delay for the initial few frames: " << init_delay;
Semantic::GetInstance()->SetSemanticMethod(cnn_method);
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::RGBD, true);
Semantic::GetInstance()->Run();
std::vector<float> vTimesTrack;
vTimesTrack.resize(nImages);
cv::Mat imRGB, imD;
for (int ni = 0; ni < nImages; ni++) {
// Read image and depthmap from file
imRGB = cv::imread(std::string(argv[3]) + "/" + vstrImageFilenamesRGB[ni], cv::IMREAD_UNCHANGED);
imD = cv::imread(std::string(argv[3]) + "/" + vstrImageFilenamesD[ni], cv::IMREAD_UNCHANGED);
double tframe = vTimestamps[ni];
if (imRGB.empty()) {
cerr << endl
<< "Failed to load image at: "
<< std::string(argv[3]) << "/" << vstrImageFilenamesRGB[ni] << endl;
return 1;
}
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif
// Pass the image to the SLAM system
SLAM.TrackRGBD(imRGB, imD, tframe);
// Manually add delay to evaluate TUM, because TUM dataset is very short
if (ni < init_frames) {
usleep(init_delay);
} else {
usleep(frame_delay);
}
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
double ttrack = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni] = ttrack;
// Wait to load the next frame
double T = 0;
if (ni < nImages - 1)
T = vTimestamps[ni + 1] - tframe;
else if (ni > 0)
T = tframe - vTimestamps[ni - 1];
if (ttrack < T)
usleep((T - ttrack) * 1e6);
}
LOG(INFO) << "===============Tracking Finished============";
std::cout << "===============Tracking Finished============" << std::endl;
// ros::spin();
LOG(INFO) << "===============Final Stage============";
std::cout << "===============Final Stage============" << std::endl;
// Stop semantic thread
// Semantic::GetInstance()->RequestFinish();
// Stop all threads
SLAM.Shutdown();
// Tracking time statistics
std::sort(vTimesTrack.begin(), vTimesTrack.end());
float totaltime = 0;
for (int ni = 0; ni < nImages; ni++) {
totaltime += vTimesTrack[ni];
}
cout << "-------" << endl;
cout << "median tracking time: " << vTimesTrack[nImages / 2] << endl;
LOG(INFO) << "median tracking time: " << vTimesTrack[nImages / 2] * 1000 << "ms";
cout << "mean tracking time: " << totaltime / nImages << endl;
LOG(INFO) << "mean tracking time: " << totaltime / nImages * 1000 << "ms";
// Save camera trajectory
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
ros::shutdown();
return 0;
}

31
SLAM/README.md
View File

@ -1,31 +0,0 @@
# Manually Build Probject
```sh
cd ~/catkin_ws/src/SLAM/
./build_thirdparty.sh
cd ~/catkin_ws/src
catkin_make
```
# Run SLAM client
## SegNet Version
```sh
roslaunch rds_slam tum_segnet_walk_xyz.launch
```
## MaskRCNN Version
```sh
roslaunch rds_slam tum_maskrcnn_walk_xyz.launch
```
# How to control the framerate
Due to the performance is somehow related to the GPU configuration, you can trade off the tracking performance and real-time performance by adjusting these parameters.
- init_delay: the delay of first few frames
- frame_delay: the delay of each frame other than the first few frames
- init_frames: wait some time for the first few frames

40
SLAM/Thirdparty/DBoW2/CMakeLists.txt vendored
View File

@ -1,40 +0,0 @@
cmake_minimum_required(VERSION 2.8)
project(DBoW2)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
set(HDRS_DBOW2
DBoW2/BowVector.h
DBoW2/FORB.h
DBoW2/FClass.h
DBoW2/FeatureVector.h
DBoW2/ScoringObject.h
DBoW2/TemplatedVocabulary.h)
set(SRCS_DBOW2
DBoW2/BowVector.cpp
DBoW2/FORB.cpp
DBoW2/FeatureVector.cpp
DBoW2/ScoringObject.cpp)
set(HDRS_DUTILS
DUtils/Random.h
DUtils/Timestamp.h)
set(SRCS_DUTILS
DUtils/Random.cpp
DUtils/Timestamp.cpp)
find_package(OpenCV 3.0 QUIET)
if(NOT OpenCV_FOUND)
find_package(OpenCV 2.4.3 QUIET)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
endif()
endif()
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
include_directories(${OpenCV_INCLUDE_DIRS})
add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS})
target_link_libraries(DBoW2 ${OpenCV_LIBS})

130
SLAM/Thirdparty/DBoW2/DBoW2/BowVector.cpp vendored
View File

@ -1,130 +0,0 @@
/**
* File: BowVector.cpp
* Date: March 2011
* Author: Dorian Galvez-Lopez
* Description: bag of words vector
* License: see the LICENSE.txt file
*
*/
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <cmath>
#include "BowVector.h"
namespace DBoW2 {
// --------------------------------------------------------------------------
BowVector::BowVector(void)
{
}
// --------------------------------------------------------------------------
BowVector::~BowVector(void)
{
}
// --------------------------------------------------------------------------
void BowVector::addWeight(WordId id, WordValue v)
{
BowVector::iterator vit = this->lower_bound(id);
if(vit != this->end() && !(this->key_comp()(id, vit->first)))
{
vit->second += v;
}
else
{
this->insert(vit, BowVector::value_type(id, v));
}
}
// --------------------------------------------------------------------------
void BowVector::addIfNotExist(WordId id, WordValue v)
{
BowVector::iterator vit = this->lower_bound(id);
if(vit == this->end() || (this->key_comp()(id, vit->first)))
{
this->insert(vit, BowVector::value_type(id, v));
}
}
// --------------------------------------------------------------------------
void BowVector::normalize(LNorm norm_type)
{
double norm = 0.0;
BowVector::iterator it;
if(norm_type == DBoW2::L1)
{
for(it = begin(); it != end(); ++it)
norm += fabs(it->second);
}
else
{
for(it = begin(); it != end(); ++it)
norm += it->second * it->second;
norm = sqrt(norm);
}
if(norm > 0.0)
{
for(it = begin(); it != end(); ++it)
it->second /= norm;
}
}
// --------------------------------------------------------------------------
std::ostream& operator<< (std::ostream &out, const BowVector &v)
{
BowVector::const_iterator vit;
std::vector<unsigned int>::const_iterator iit;
unsigned int i = 0;
const unsigned int N = v.size();
for(vit = v.begin(); vit != v.end(); ++vit, ++i)
{
out << "<" << vit->first << ", " << vit->second << ">";
if(i < N-1) out << ", ";
}
return out;
}
// --------------------------------------------------------------------------
void BowVector::saveM(const std::string &filename, size_t W) const
{
std::fstream f(filename.c_str(), std::ios::out);
WordId last = 0;
BowVector::const_iterator bit;
for(bit = this->begin(); bit != this->end(); ++bit)
{
for(; last < bit->first; ++last)
{
f << "0 ";
}
f << bit->second << " ";
last = bit->first + 1;
}
for(; last < (WordId)W; ++last)
f << "0 ";
f.close();
}
// --------------------------------------------------------------------------
} // namespace DBoW2

119
SLAM/Thirdparty/DBoW2/DBoW2/BowVector.h vendored
View File

@ -1,119 +0,0 @@
/**
* File: BowVector.h
* Date: March 2011
* Author: Dorian Galvez-Lopez
* Description: bag of words vector
* License: see the LICENSE.txt file
*
*/
#ifndef __D_T_BOW_VECTOR__
#define __D_T_BOW_VECTOR__
#include <iostream>
#include <map>
#include <vector>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/map.hpp>
namespace DBoW2 {
/// Id of words
typedef unsigned int WordId;
/// Value of a word
typedef double WordValue;
/// Id of nodes in the vocabulary treee
typedef unsigned int NodeId;
/// L-norms for normalization
enum LNorm
{
L1,
L2
};
/// Weighting type
enum WeightingType
{
TF_IDF,
TF,
IDF,
BINARY
};
/// Scoring type
enum ScoringType
{
L1_NORM,
L2_NORM,
CHI_SQUARE,
KL,
BHATTACHARYYA,
DOT_PRODUCT,
};
/// Vector of words to represent images
class BowVector:
public std::map<WordId, WordValue>
{
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const int version)
{
ar & boost::serialization::base_object<std::map<WordId, WordValue> >(*this);
}
public:
/**
* Constructor
*/
BowVector(void);
/**
* Destructor
*/
~BowVector(void);
/**
* Adds a value to a word value existing in the vector, or creates a new
* word with the given value
* @param id word id to look for
* @param v value to create the word with, or to add to existing word
*/
void addWeight(WordId id, WordValue v);
/**
* Adds a word with a value to the vector only if this does not exist yet
* @param id word id to look for
* @param v value to give to the word if this does not exist
*/
void addIfNotExist(WordId id, WordValue v);
/**
* L1-Normalizes the values in the vector
* @param norm_type norm used
*/
void normalize(LNorm norm_type);
/**
* Prints the content of the bow vector
* @param out stream
* @param v
*/
friend std::ostream& operator<<(std::ostream &out, const BowVector &v);
/**
* Saves the bow vector as a vector in a matlab file
* @param filename
* @param W number of words in the vocabulary
*/
void saveM(const std::string &filename, size_t W) const;
};
} // namespace DBoW2
#endif

71
SLAM/Thirdparty/DBoW2/DBoW2/FClass.h vendored
View File

@ -1,71 +0,0 @@
/**
* File: FClass.h
* Date: November 2011
* Author: Dorian Galvez-Lopez
* Description: generic FClass to instantiate templated classes
* License: see the LICENSE.txt file
*
*/
#ifndef __D_T_FCLASS__
#define __D_T_FCLASS__
#include <opencv2/core/core.hpp>
#include <vector>
#include <string>
namespace DBoW2 {
/// Generic class to encapsulate functions to manage descriptors.
/**
* This class must be inherited. Derived classes can be used as the
* parameter F when creating Templated structures
* (TemplatedVocabulary, TemplatedDatabase, ...)
*/
class FClass
{
class TDescriptor;
typedef const TDescriptor *pDescriptor;
/**
* Calculates the mean value of a set of descriptors
* @param descriptors
* @param mean mean descriptor
*/
virtual void meanValue(const std::vector<pDescriptor> &descriptors,
TDescriptor &mean) = 0;
/**
* Calculates the distance between two descriptors
* @param a
* @param b
* @return distance
*/
static double distance(const TDescriptor &a, const TDescriptor &b);
/**
* Returns a string version of the descriptor
* @param a descriptor
* @return string version
*/
static std::string toString(const TDescriptor &a);
/**
* Returns a descriptor from a string
* @param a descriptor
* @param s string version
*/
static void fromString(TDescriptor &a, const std::string &s);
/**
* Returns a mat with the descriptors in float format
* @param descriptors
* @param mat (out) NxL 32F matrix
*/
static void toMat32F(const std::vector<TDescriptor> &descriptors,
cv::Mat &mat);
};
} // namespace DBoW2
#endif

193
SLAM/Thirdparty/DBoW2/DBoW2/FORB.cpp vendored
View File

@ -1,193 +0,0 @@
/**
* File: FORB.cpp
* Date: June 2012
* Author: Dorian Galvez-Lopez
* Description: functions for ORB descriptors
* License: see the LICENSE.txt file
*
* Distance function has been modified
*
*/
#include <vector>
#include <string>
#include <sstream>
#include <stdint-gcc.h>
#include "FORB.h"
using namespace std;
namespace DBoW2 {
// --------------------------------------------------------------------------
const int FORB::L=32;
void FORB::meanValue(const std::vector<FORB::pDescriptor> &descriptors,
FORB::TDescriptor &mean)
{
if(descriptors.empty())
{
mean.release();
return;
}
else if(descriptors.size() == 1)
{
mean = descriptors[0]->clone();
}
else
{
vector<int> sum(FORB::L * 8, 0);
for(size_t i = 0; i < descriptors.size(); ++i)
{
const cv::Mat &d = *descriptors[i];
const unsigned char *p = d.ptr<unsigned char>();
for(int j = 0; j < d.cols; ++j, ++p)
{
if(*p & (1 << 7)) ++sum[ j*8 ];
if(*p & (1 << 6)) ++sum[ j*8 + 1 ];
if(*p & (1 << 5)) ++sum[ j*8 + 2 ];
if(*p & (1 << 4)) ++sum[ j*8 + 3 ];
if(*p & (1 << 3)) ++sum[ j*8 + 4 ];
if(*p & (1 << 2)) ++sum[ j*8 + 5 ];
if(*p & (1 << 1)) ++sum[ j*8 + 6 ];
if(*p & (1)) ++sum[ j*8 + 7 ];
}
}
mean = cv::Mat::zeros(1, FORB::L, CV_8U);
unsigned char *p = mean.ptr<unsigned char>();
const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2;
for(size_t i = 0; i < sum.size(); ++i)
{
if(sum[i] >= N2)
{
// set bit
*p |= 1 << (7 - (i % 8));
}
if(i % 8 == 7) ++p;
}
}
}
// --------------------------------------------------------------------------
int FORB::distance(const FORB::TDescriptor &a,
const FORB::TDescriptor &b)
{
// Bit set count operation from
// http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
const int *pa = a.ptr<int32_t>();
const int *pb = b.ptr<int32_t>();
int dist=0;
for(int i=0; i<8; i++, pa++, pb++)
{
unsigned int v = *pa ^ *pb;
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
}
return dist;
}
// --------------------------------------------------------------------------
std::string FORB::toString(const FORB::TDescriptor &a)
{
stringstream ss;
const unsigned char *p = a.ptr<unsigned char>();
for(int i = 0; i < a.cols; ++i, ++p)
{
ss << (int)*p << " ";
}
return ss.str();
}
// --------------------------------------------------------------------------
void FORB::fromString(FORB::TDescriptor &a, const std::string &s)
{
a.create(1, FORB::L, CV_8U);
unsigned char *p = a.ptr<unsigned char>();
stringstream ss(s);
for(int i = 0; i < FORB::L; ++i, ++p)
{
int n;
ss >> n;
if(!ss.fail())
*p = (unsigned char)n;
}
}
// --------------------------------------------------------------------------
void FORB::toMat32F(const std::vector<TDescriptor> &descriptors,
cv::Mat &mat)
{
if(descriptors.empty())
{
mat.release();
return;
}
const size_t N = descriptors.size();
mat.create(N, FORB::L*8, CV_32F);
float *p = mat.ptr<float>();
for(size_t i = 0; i < N; ++i)
{
const int C = descriptors[i].cols;
const unsigned char *desc = descriptors[i].ptr<unsigned char>();
for(int j = 0; j < C; ++j, p += 8)
{
p[0] = (desc[j] & (1 << 7) ? 1 : 0);
p[1] = (desc[j] & (1 << 6) ? 1 : 0);
p[2] = (desc[j] & (1 << 5) ? 1 : 0);
p[3] = (desc[j] & (1 << 4) ? 1 : 0);
p[4] = (desc[j] & (1 << 3) ? 1 : 0);
p[5] = (desc[j] & (1 << 2) ? 1 : 0);
p[6] = (desc[j] & (1 << 1) ? 1 : 0);
p[7] = desc[j] & (1);
}
}
}
// --------------------------------------------------------------------------
void FORB::toMat8U(const std::vector<TDescriptor> &descriptors,
cv::Mat &mat)
{
mat.create(descriptors.size(), 32, CV_8U);
unsigned char *p = mat.ptr<unsigned char>();
for(size_t i = 0; i < descriptors.size(); ++i, p += 32)
{
const unsigned char *d = descriptors[i].ptr<unsigned char>();
std::copy(d, d+32, p);
}
}
// --------------------------------------------------------------------------
} // namespace DBoW2

79
SLAM/Thirdparty/DBoW2/DBoW2/FORB.h vendored
View File

@ -1,79 +0,0 @@
/**
* File: FORB.h
* Date: June 2012
* Author: Dorian Galvez-Lopez
* Description: functions for ORB descriptors
* License: see the LICENSE.txt file
*
*/
#ifndef __D_T_F_ORB__
#define __D_T_F_ORB__
#include <opencv2/core/core.hpp>
#include <vector>
#include <string>
#include "FClass.h"
namespace DBoW2 {
/// Functions to manipulate ORB descriptors
class FORB: protected FClass
{
public:
/// Descriptor type
typedef cv::Mat TDescriptor; // CV_8U
/// Pointer to a single descriptor
typedef const TDescriptor *pDescriptor;
/// Descriptor length (in bytes)
static const int L;
/**
* Calculates the mean value of a set of descriptors
* @param descriptors
* @param mean mean descriptor
*/
static void meanValue(const std::vector<pDescriptor> &descriptors,
TDescriptor &mean);
/**
* Calculates the distance between two descriptors
* @param a
* @param b
* @return distance
*/
static int distance(const TDescriptor &a, const TDescriptor &b);
/**
* Returns a string version of the descriptor
* @param a descriptor
* @return string version
*/
static std::string toString(const TDescriptor &a);
/**
* Returns a descriptor from a string
* @param a descriptor
* @param s string version
*/
static void fromString(TDescriptor &a, const std::string &s);
/**
* Returns a mat with the descriptors in float format
* @param descriptors
* @param mat (out) NxL 32F matrix
*/
static void toMat32F(const std::vector<TDescriptor> &descriptors,
cv::Mat &mat);
static void toMat8U(const std::vector<TDescriptor> &descriptors,
cv::Mat &mat);
};
} // namespace DBoW2
#endif

85
SLAM/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp vendored
View File

@ -1,85 +0,0 @@
/**
* File: FeatureVector.cpp
* Date: November 2011
* Author: Dorian Galvez-Lopez
* Description: feature vector
* License: see the LICENSE.txt file
*
*/
#include "FeatureVector.h"
#include <map>
#include <vector>
#include <iostream>
namespace DBoW2 {
// ---------------------------------------------------------------------------
FeatureVector::FeatureVector(void)
{
}
// ---------------------------------------------------------------------------
FeatureVector::~FeatureVector(void)
{
}
// ---------------------------------------------------------------------------
void FeatureVector::addFeature(NodeId id, unsigned int i_feature)
{
FeatureVector::iterator vit = this->lower_bound(id);
if(vit != this->end() && vit->first == id)
{
vit->second.push_back(i_feature);
}
else
{
vit = this->insert(vit, FeatureVector::value_type(id,
std::vector<unsigned int>() ));
vit->second.push_back(i_feature);
}
}
// ---------------------------------------------------------------------------
std::ostream& operator<<(std::ostream &out,
const FeatureVector &v)
{
if(!v.empty())
{
FeatureVector::const_iterator vit = v.begin();
const std::vector<unsigned int>* f = &vit->second;
out << "<" << vit->first << ": [";
if(!f->empty()) out << (*f)[0];
for(unsigned int i = 1; i < f->size(); ++i)
{
out << ", " << (*f)[i];
}
out << "]>";
for(++vit; vit != v.end(); ++vit)
{
f = &vit->second;
out << ", <" << vit->first << ": [";
if(!f->empty()) out << (*f)[0];
for(unsigned int i = 1; i < f->size(); ++i)
{
out << ", " << (*f)[i];
}
out << "]>";
}
}
return out;
}
// ---------------------------------------------------------------------------
} // namespace DBoW2

66
SLAM/Thirdparty/DBoW2/DBoW2/FeatureVector.h vendored
View File

@ -1,66 +0,0 @@
/**
* File: FeatureVector.h
* Date: November 2011
* Author: Dorian Galvez-Lopez
* Description: feature vector
* License: see the LICENSE.txt file
*
*/
#ifndef __D_T_FEATURE_VECTOR__
#define __D_T_FEATURE_VECTOR__
#include "BowVector.h"
#include <map>
#include <vector>
#include <iostream>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/map.hpp>
namespace DBoW2 {
/// Vector of nodes with indexes of local features
class FeatureVector:
public std::map<NodeId, std::vector<unsigned int> >
{
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const int version)
{
ar & boost::serialization::base_object<std::map<NodeId, std::vector<unsigned int> > >(*this);
}
public:
/**
* Constructor
*/
FeatureVector(void);
/**
* Destructor
*/
~FeatureVector(void);
/**
* Adds a feature to an existing node, or adds a new node with an initial
* feature
* @param id node id to add or to modify
* @param i_feature index of feature to add to the given node
*/
void addFeature(NodeId id, unsigned int i_feature);
/**
* Sends a string versions of the feature vector through the stream
* @param out stream
* @param v feature vector
*/
friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v);
};
} // namespace DBoW2
#endif

315
SLAM/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp vendored
View File

@ -1,315 +0,0 @@
/**
* File: ScoringObject.cpp
* Date: November 2011
* Author: Dorian Galvez-Lopez
* Description: functions to compute bow scores
* License: see the LICENSE.txt file
*
*/
#include <cfloat>
#include "TemplatedVocabulary.h"
#include "BowVector.h"
using namespace DBoW2;
// If you change the type of WordValue, make sure you change also the
// epsilon value (this is needed by the KL method)
const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------
double L1Scoring::score(const BowVector &v1, const BowVector &v2) const
{
BowVector::const_iterator v1_it, v2_it;
const BowVector::const_iterator v1_end = v1.end();
const BowVector::const_iterator v2_end = v2.end();
v1_it = v1.begin();
v2_it = v2.begin();
double score = 0;
while(v1_it != v1_end && v2_it != v2_end)
{
const WordValue& vi = v1_it->second;
const WordValue& wi = v2_it->second;
if(v1_it->first == v2_it->first)
{
score += fabs(vi - wi) - fabs(vi) - fabs(wi);
// move v1 and v2 forward
++v1_it;
++v2_it;
}
else if(v1_it->first < v2_it->first)
{
// move v1 forward
v1_it = v1.lower_bound(v2_it->first);
// v1_it = (first element >= v2_it.id)
}
else
{
// move v2 forward
v2_it = v2.lower_bound(v1_it->first);
// v2_it = (first element >= v1_it.id)
}
}
// ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|)
// for all i | v_i != 0 and w_i != 0
// (Nister, 2006)
// scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}
score = -score/2.0;
return score; // [0..1]
}
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------
double L2Scoring::score(const BowVector &v1, const BowVector &v2) const
{
BowVector::const_iterator v1_it, v2_it;
const BowVector::const_iterator v1_end = v1.end();
const BowVector::const_iterator v2_end = v2.end();
v1_it = v1.begin();
v2_it = v2.begin();
double score = 0;
while(v1_it != v1_end && v2_it != v2_end)
{
const WordValue& vi = v1_it->second;
const WordValue& wi = v2_it->second;
if(v1_it->first == v2_it->first)
{
score += vi * wi;
// move v1 and v2 forward
++v1_it;
++v2_it;
}
else if(v1_it->first < v2_it->first)
{
// move v1 forward
v1_it = v1.lower_bound(v2_it->first);
// v1_it = (first element >= v2_it.id)
}
else
{
// move v2 forward
v2_it = v2.lower_bound(v1_it->first);
// v2_it = (first element >= v1_it.id)
}
}
// ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) )
// for all i | v_i != 0 and w_i != 0 )
// (Nister, 2006)
if(score >= 1) // rounding errors
score = 1.0;
else
score = 1.0 - sqrt(1.0 - score); // [0..1]
return score;
}
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------
double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2)
const
{
BowVector::const_iterator v1_it, v2_it;
const BowVector::const_iterator v1_end = v1.end();
const BowVector::const_iterator v2_end = v2.end();
v1_it = v1.begin();
v2_it = v2.begin();
double score = 0;
// all the items are taken into account
while(v1_it != v1_end && v2_it != v2_end)
{
const WordValue& vi = v1_it->second;
const WordValue& wi = v2_it->second;
if(v1_it->first == v2_it->first)
{
// (v-w)^2/(v+w) - v - w = -4 vw/(v+w)
// we move the -4 out
if(vi + wi != 0.0) score += vi * wi / (vi + wi);
// move v1 and v2 forward
++v1_it;
++v2_it;
}
else if(v1_it->first < v2_it->first)
{
// move v1 forward
v1_it = v1.lower_bound(v2_it->first);
}
else
{
// move v2 forward
v2_it = v2.lower_bound(v1_it->first);
}
}
// this takes the -4 into account
score = 2. * score; // [0..1]
return score;
}
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------
double KLScoring::score(const BowVector &v1, const BowVector &v2) const
{
BowVector::const_iterator v1_it, v2_it;
const BowVector::const_iterator v1_end = v1.end();
const BowVector::const_iterator v2_end = v2.end();
v1_it = v1.begin();
v2_it = v2.begin();
double score = 0;
// all the items or v are taken into account
while(v1_it != v1_end && v2_it != v2_end)
{
const WordValue& vi = v1_it->second;
const WordValue& wi = v2_it->second;
if(v1_it->first == v2_it->first)
{
if(vi != 0 && wi != 0) score += vi * log(vi/wi);
// move v1 and v2 forward
++v1_it;
++v2_it;
}
else if(v1_it->first < v2_it->first)
{
// move v1 forward
score += vi * (log(vi) - LOG_EPS);
++v1_it;
}
else
{
// move v2_it forward, do not add any score
v2_it = v2.lower_bound(v1_it->first);
// v2_it = (first element >= v1_it.id)
}
}
// sum rest of items of v
for(; v1_it != v1_end; ++v1_it)
if(v1_it->second != 0)
score += v1_it->second * (log(v1_it->second) - LOG_EPS);
return score; // cannot be scaled
}
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------
double BhattacharyyaScoring::score(const BowVector &v1,
const BowVector &v2) const
{
BowVector::const_iterator v1_it, v2_it;
const BowVector::const_iterator v1_end = v1.end();
const BowVector::const_iterator v2_end = v2.end();
v1_it = v1.begin();
v2_it = v2.begin();
double score = 0;
while(v1_it != v1_end && v2_it != v2_end)
{
const WordValue& vi = v1_it->second;
const WordValue& wi = v2_it->second;
if(v1_it->first == v2_it->first)
{
score += sqrt(vi * wi);
// move v1 and v2 forward
++v1_it;
++v2_it;
}
else if(v1_it->first < v2_it->first)
{
// move v1 forward
v1_it = v1.lower_bound(v2_it->first);
// v1_it = (first element >= v2_it.id)
}
else
{
// move v2 forward
v2_it = v2.lower_bound(v1_it->first);
// v2_it = (first element >= v1_it.id)
}
}
return score; // already scaled
}
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------
double DotProductScoring::score(const BowVector &v1,
const BowVector &v2) const
{
BowVector::const_iterator v1_it, v2_it;
const BowVector::const_iterator v1_end = v1.end();
const BowVector::const_iterator v2_end = v2.end();
v1_it = v1.begin();
v2_it = v2.begin();
double score = 0;
while(v1_it != v1_end && v2_it != v2_end)
{
const WordValue& vi = v1_it->second;
const WordValue& wi = v2_it->second;
if(v1_it->first == v2_it->first)
{
score += vi * wi;
// move v1 and v2 forward
++v1_it;
++v2_it;
}
else if(v1_it->first < v2_it->first)
{
// move v1 forward
v1_it = v1.lower_bound(v2_it->first);
// v1_it = (first element >= v2_it.id)
}
else
{
// move v2 forward
v2_it = v2.lower_bound(v1_it->first);
// v2_it = (first element >= v1_it.id)
}
}
return score; // cannot scale
}
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------

96
SLAM/Thirdparty/DBoW2/DBoW2/ScoringObject.h vendored
View File

@ -1,96 +0,0 @@
/**
* File: ScoringObject.h
* Date: November 2011
* Author: Dorian Galvez-Lopez
* Description: functions to compute bow scores
* License: see the LICENSE.txt file
*
*/
#ifndef __D_T_SCORING_OBJECT__
#define __D_T_SCORING_OBJECT__
#include "BowVector.h"
namespace DBoW2 {
/// Base class of scoring functions
class GeneralScoring
{
public:
/**
* Computes the score between two vectors. Vectors must be sorted and
* normalized if necessary
* @param v (in/out)
* @param w (in/out)
* @return score
*/
virtual double score(const BowVector &v, const BowVector &w) const = 0;
/**
* Returns whether a vector must be normalized before scoring according
* to the scoring scheme
* @param norm norm to use
* @return true iff must normalize
*/
virtual bool mustNormalize(LNorm &norm) const = 0;
/// Log of epsilon
static const double LOG_EPS;
// If you change the type of WordValue, make sure you change also the
// epsilon value (this is needed by the KL method)
virtual ~GeneralScoring() {} //!< Required for virtual base classes
};
/**
* Macro for defining Scoring classes
* @param NAME name of class
* @param MUSTNORMALIZE if vectors must be normalized to compute the score
* @param NORM type of norm to use when MUSTNORMALIZE
*/
#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \
NAME: public GeneralScoring \
{ public: \
/** \
* Computes score between two vectors \
* @param v \
* @param w \
* @return score between v and w \
*/ \
virtual double score(const BowVector &v, const BowVector &w) const; \
\
/** \
* Says if a vector must be normalized according to the scoring function \
* @param norm (out) if true, norm to use
* @return true iff vectors must be normalized \
*/ \
virtual inline bool mustNormalize(LNorm &norm) const \
{ norm = NORM; return MUSTNORMALIZE; } \
}
/// L1 Scoring object
class __SCORING_CLASS(L1Scoring, true, L1);
/// L2 Scoring object
class __SCORING_CLASS(L2Scoring, true, L2);
/// Chi square Scoring object
class __SCORING_CLASS(ChiSquareScoring, true, L1);
/// KL divergence Scoring object
class __SCORING_CLASS(KLScoring, true, L1);
/// Bhattacharyya Scoring object
class __SCORING_CLASS(BhattacharyyaScoring, true, L1);
/// Dot product Scoring object
class __SCORING_CLASS(DotProductScoring, false, L1);
#undef __SCORING_CLASS
} // namespace DBoW2
#endif

1665
SLAM/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h vendored
View File

File diff suppressed because it is too large Load Diff

129
SLAM/Thirdparty/DBoW2/DUtils/Random.cpp vendored
View File

@ -1,129 +0,0 @@
/*
* File: Random.cpp
* Project: DUtils library
* Author: Dorian Galvez-Lopez
* Date: April 2010
* Description: manages pseudo-random numbers
* License: see the LICENSE.txt file
*
*/
#include "Random.h"
#include "Timestamp.h"
#include <cstdlib>
using namespace std;
bool DUtils::Random::m_already_seeded = false;
void DUtils::Random::SeedRand(){
Timestamp time;
time.setToCurrentTime();
srand((unsigned)time.getFloatTime());
}
void DUtils::Random::SeedRandOnce()
{
if(!m_already_seeded)
{
DUtils::Random::SeedRand();
m_already_seeded = true;
}
}
void DUtils::Random::SeedRand(int seed)
{
srand(seed);
}
void DUtils::Random::SeedRandOnce(int seed)
{
if(!m_already_seeded)
{
DUtils::Random::SeedRand(seed);
m_already_seeded = true;
}
}
int DUtils::Random::RandomInt(int min, int max){
int d = max - min + 1;
return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min;
}
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------
DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max)
{
if(min <= max)
{
m_min = min;
m_max = max;
}
else
{
m_min = max;
m_max = min;
}
createValues();
}
// ---------------------------------------------------------------------------
DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer
(const DUtils::Random::UnrepeatedRandomizer& rnd)
{
*this = rnd;
}
// ---------------------------------------------------------------------------
int DUtils::Random::UnrepeatedRandomizer::get()
{
if(empty()) createValues();
DUtils::Random::SeedRandOnce();
int k = DUtils::Random::RandomInt(0, m_values.size()-1);
int ret = m_values[k];
m_values[k] = m_values.back();
m_values.pop_back();
return ret;
}
// ---------------------------------------------------------------------------
void DUtils::Random::UnrepeatedRandomizer::createValues()
{
int n = m_max - m_min + 1;
m_values.resize(n);
for(int i = 0; i < n; ++i) m_values[i] = m_min + i;
}
// ---------------------------------------------------------------------------
void DUtils::Random::UnrepeatedRandomizer::reset()
{
if((int)m_values.size() != m_max - m_min + 1) createValues();
}
// ---------------------------------------------------------------------------
DUtils::Random::UnrepeatedRandomizer&
DUtils::Random::UnrepeatedRandomizer::operator=
(const DUtils::Random::UnrepeatedRandomizer& rnd)
{
if(this != &rnd)
{
this->m_min = rnd.m_min;
this->m_max = rnd.m_max;
this->m_values = rnd.m_values;
}
return *this;
}
// ---------------------------------------------------------------------------

184
SLAM/Thirdparty/DBoW2/DUtils/Random.h vendored
View File

@ -1,184 +0,0 @@
/*
* File: Random.h
* Project: DUtils library
* Author: Dorian Galvez-Lopez
* Date: April 2010, November 2011
* Description: manages pseudo-random numbers
* License: see the LICENSE.txt file
*
*/
#pragma once
#ifndef __D_RANDOM__
#define __D_RANDOM__
#include <cstdlib>
#include <vector>
namespace DUtils {
/// Functions to generate pseudo-random numbers
class Random
{
public:
class UnrepeatedRandomizer;
public:
/**
* Sets the random number seed to the current time
*/
static void SeedRand();
/**
* Sets the random number seed to the current time only the first
* time this function is called
*/
static void SeedRandOnce();
/**
* Sets the given random number seed
* @param seed
*/
static void SeedRand(int seed);
/**
* Sets the given random number seed only the first time this function
* is called
* @param seed
*/
static void SeedRandOnce(int seed);
/**
* Returns a random number in the range [0..1]
* @return random T number in [0..1]
*/
template <class T>
static T RandomValue(){
return (T)rand()/(T)RAND_MAX;
}
/**
* Returns a random number in the range [min..max]
* @param min
* @param max
* @return random T number in [min..max]
*/
template <class T>
static T RandomValue(T min, T max){
return Random::RandomValue<T>() * (max - min) + min;
}
/**
* Returns a random int in the range [min..max]
* @param min
* @param max
* @return random int in [min..max]
*/
static int RandomInt(int min, int max);
/**
* Returns a random number from a gaussian distribution
* @param mean
* @param sigma standard deviation
*/
template <class T>
static T RandomGaussianValue(T mean, T sigma)
{
// Box-Muller transformation
T x1, x2, w, y1;
do {
x1 = (T)2. * RandomValue<T>() - (T)1.;
x2 = (T)2. * RandomValue<T>() - (T)1.;
w = x1 * x1 + x2 * x2;
} while ( w >= (T)1. || w == (T)0. );
w = sqrt( ((T)-2.0 * log( w ) ) / w );
y1 = x1 * w;
return( mean + y1 * sigma );
}
private:
/// If SeedRandOnce() or SeedRandOnce(int) have already been called
static bool m_already_seeded;
};
// ---------------------------------------------------------------------------
/// Provides pseudo-random numbers with no repetitions
class Random::UnrepeatedRandomizer
{
public:
/**
* Creates a randomizer that returns numbers in the range [min, max]
* @param min
* @param max
*/
UnrepeatedRandomizer(int min, int max);
~UnrepeatedRandomizer(){}
/**
* Copies a randomizer
* @param rnd
*/
UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd);
/**
* Copies a randomizer
* @param rnd
*/
UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd);
/**
* Returns a random number not given before. If all the possible values
* were already given, the process starts again
* @return unrepeated random number
*/
int get();
/**
* Returns whether all the possible values between min and max were
* already given. If get() is called when empty() is true, the behaviour
* is the same than after creating the randomizer
* @return true iff all the values were returned
*/
inline bool empty() const { return m_values.empty(); }
/**
* Returns the number of values still to be returned
* @return amount of values to return
*/
inline unsigned int left() const { return m_values.size(); }
/**
* Resets the randomizer as it were just created
*/
void reset();
protected:
/**
* Creates the vector with available values
*/
void createValues();
protected:
/// Min of range of values
int m_min;
/// Max of range of values
int m_max;
/// Available values
std::vector<int> m_values;
};
}
#endif

246
SLAM/Thirdparty/DBoW2/DUtils/Timestamp.cpp vendored
View File

@ -1,246 +0,0 @@
/*
* File: Timestamp.cpp
* Author: Dorian Galvez-Lopez
* Date: March 2009
* Description: timestamping functions
*
* Note: in windows, this class has a 1ms resolution
*
* License: see the LICENSE.txt file
*
*/
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <sstream>
#include <iomanip>
#ifdef _WIN32
#ifndef WIN32
#define WIN32
#endif
#endif
#ifdef WIN32
#include <sys/timeb.h>
#define sprintf sprintf_s
#else
#include <sys/time.h>
#endif
#include "Timestamp.h"
using namespace std;
using namespace DUtils;
Timestamp::Timestamp(Timestamp::tOptions option)
{
if(option & CURRENT_TIME)
setToCurrentTime();
else if(option & ZERO)
setTime(0.);
}
Timestamp::~Timestamp(void)
{
}
bool Timestamp::empty() const
{
return m_secs == 0 && m_usecs == 0;
}
void Timestamp::setToCurrentTime(){
#ifdef WIN32
struct __timeb32 timebuffer;
_ftime32_s( &timebuffer ); // C4996
// Note: _ftime is deprecated; consider using _ftime_s instead
m_secs = timebuffer.time;
m_usecs = timebuffer.millitm * 1000;
#else
struct timeval now;
gettimeofday(&now, NULL);
m_secs = now.tv_sec;
m_usecs = now.tv_usec;
#endif
}
void Timestamp::setTime(const string &stime){
string::size_type p = stime.find('.');
if(p == string::npos){
m_secs = atol(stime.c_str());
m_usecs = 0;
}else{
m_secs = atol(stime.substr(0, p).c_str());
string s_usecs = stime.substr(p+1, 6);
m_usecs = atol(stime.substr(p+1).c_str());
m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length()));
}
}
void Timestamp::setTime(double s)
{
m_secs = (unsigned long)s;
m_usecs = (s - (double)m_secs) * 1e6;
}
double Timestamp::getFloatTime() const {
return double(m_secs) + double(m_usecs)/1000000.0;
}
string Timestamp::getStringTime() const {
char buf[32];
sprintf(buf, "%.6lf", this->getFloatTime());
return string(buf);
}
double Timestamp::operator- (const Timestamp &t) const {
return this->getFloatTime() - t.getFloatTime();
}
Timestamp& Timestamp::operator+= (double s)
{
*this = *this + s;
return *this;
}
Timestamp& Timestamp::operator-= (double s)
{
*this = *this - s;
return *this;
}
Timestamp Timestamp::operator+ (double s) const
{
unsigned long secs = (long)floor(s);
unsigned long usecs = (long)((s - (double)secs) * 1e6);
return this->plus(secs, usecs);
}
Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const
{
Timestamp t;
const unsigned long max = 1000000ul;
if(m_usecs + usecs >= max)
t.setTime(m_secs + secs + 1, m_usecs + usecs - max);
else
t.setTime(m_secs + secs, m_usecs + usecs);
return t;
}
Timestamp Timestamp::operator- (double s) const
{
unsigned long secs = (long)floor(s);
unsigned long usecs = (long)((s - (double)secs) * 1e6);
return this->minus(secs, usecs);
}
Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const
{
Timestamp t;
const unsigned long max = 1000000ul;
if(m_usecs < usecs)
t.setTime(m_secs - secs - 1, max - (usecs - m_usecs));
else
t.setTime(m_secs - secs, m_usecs - usecs);
return t;
}
bool Timestamp::operator> (const Timestamp &t) const
{
if(m_secs > t.m_secs) return true;
else if(m_secs == t.m_secs) return m_usecs > t.m_usecs;
else return false;
}
bool Timestamp::operator>= (const Timestamp &t) const
{
if(m_secs > t.m_secs) return true;
else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs;
else return false;
}
bool Timestamp::operator< (const Timestamp &t) const
{
if(m_secs < t.m_secs) return true;
else if(m_secs == t.m_secs) return m_usecs < t.m_usecs;
else return false;
}
bool Timestamp::operator<= (const Timestamp &t) const
{
if(m_secs < t.m_secs) return true;
else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs;
else return false;
}
bool Timestamp::operator== (const Timestamp &t) const
{
return(m_secs == t.m_secs && m_usecs == t.m_usecs);
}
string Timestamp::Format(bool machine_friendly) const
{
struct tm tm_time;
time_t t = (time_t)getFloatTime();
#ifdef WIN32
localtime_s(&tm_time, &t);
#else
localtime_r(&t, &tm_time);
#endif
char buffer[128];
if(machine_friendly)
{
strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time);
}
else
{
strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001
}
return string(buffer);
}
string Timestamp::Format(double s) {
int days = int(s / (24. * 3600.0));
s -= days * (24. * 3600.0);
int hours = int(s / 3600.0);
s -= hours * 3600;
int minutes = int(s / 60.0);
s -= minutes * 60;
int seconds = int(s);
int ms = int((s - seconds)*1e6);
stringstream ss;
ss.fill('0');
bool b;
if((b = (days > 0))) ss << days << "d ";
if((b = (b || hours > 0))) ss << setw(2) << hours << ":";
if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":";
if(b) ss << setw(2);
ss << seconds;
if(!b) ss << "." << setw(6) << ms;
return ss.str();
}

204
SLAM/Thirdparty/DBoW2/DUtils/Timestamp.h vendored
View File

@ -1,204 +0,0 @@
/*
* File: Timestamp.h
* Author: Dorian Galvez-Lopez
* Date: March 2009
* Description: timestamping functions
* License: see the LICENSE.txt file
*
*/
#ifndef __D_TIMESTAMP__
#define __D_TIMESTAMP__
#include <iostream>
using namespace std;
namespace DUtils {
/// Timestamp
class Timestamp
{
public:
/// Options to initiate a timestamp
enum tOptions
{
NONE = 0,
CURRENT_TIME = 0x1,
ZERO = 0x2
};
public:
/**
* Creates a timestamp
* @param option option to set the initial time stamp
*/
Timestamp(Timestamp::tOptions option = NONE);
/**
* Destructor
*/
virtual ~Timestamp(void);
/**
* Says if the timestamp is "empty": seconds and usecs are both 0, as
* when initiated with the ZERO flag
* @return true iif secs == usecs == 0
*/
bool empty() const;
/**
* Sets this instance to the current time
*/
void setToCurrentTime();
/**
* Sets the timestamp from seconds and microseconds
* @param secs: seconds
* @param usecs: microseconds
*/
inline void setTime(unsigned long secs, unsigned long usecs){
m_secs = secs;
m_usecs = usecs;
}
/**
* Returns the timestamp in seconds and microseconds
* @param secs seconds
* @param usecs microseconds
*/
inline void getTime(unsigned long &secs, unsigned long &usecs) const
{
secs = m_secs;
usecs = m_usecs;
}
/**
* Sets the timestamp from a string with the time in seconds
* @param stime: string such as "1235603336.036609"
*/
void setTime(const string &stime);
/**
* Sets the timestamp from a number of seconds from the epoch
* @param s seconds from the epoch
*/
void setTime(double s);
/**
* Returns this timestamp as the number of seconds in (long) float format
*/
double getFloatTime() const;
/**
* Returns this timestamp as the number of seconds in fixed length string format
*/
string getStringTime() const;
/**
* Returns the difference in seconds between this timestamp (greater) and t (smaller)
* If the order is swapped, a negative number is returned
* @param t: timestamp to subtract from this timestamp
* @return difference in seconds
*/
double operator- (const Timestamp &t) const;
/**
* Returns a copy of this timestamp + s seconds + us microseconds
* @param s seconds
* @param us microseconds
*/
Timestamp plus(unsigned long s, unsigned long us) const;
/**
* Returns a copy of this timestamp - s seconds - us microseconds
* @param s seconds
* @param us microseconds
*/
Timestamp minus(unsigned long s, unsigned long us) const;
/**
* Adds s seconds to this timestamp and returns a reference to itself
* @param s seconds
* @return reference to this timestamp
*/
Timestamp& operator+= (double s);
/**
* Substracts s seconds to this timestamp and returns a reference to itself
* @param s seconds
* @return reference to this timestamp
*/
Timestamp& operator-= (double s);
/**
* Returns a copy of this timestamp + s seconds
* @param s: seconds
*/
Timestamp operator+ (double s) const;
/**
* Returns a copy of this timestamp - s seconds
* @param s: seconds
*/
Timestamp operator- (double s) const;
/**
* Returns whether this timestamp is at the future of t
* @param t
*/
bool operator> (const Timestamp &t) const;
/**
* Returns whether this timestamp is at the future of (or is the same as) t
* @param t
*/
bool operator>= (const Timestamp &t) const;
/**
* Returns whether this timestamp and t represent the same instant
* @param t
*/
bool operator== (const Timestamp &t) const;
/**
* Returns whether this timestamp is at the past of t
* @param t
*/
bool operator< (const Timestamp &t) const;
/**
* Returns whether this timestamp is at the past of (or is the same as) t
* @param t
*/
bool operator<= (const Timestamp &t) const;
/**
* Returns the timestamp in a human-readable string
* @param machine_friendly if true, the returned string is formatted
* to yyyymmdd_hhmmss, without weekday or spaces
* @note This has not been tested under Windows
* @note The timestamp is truncated to seconds
*/
string Format(bool machine_friendly = false) const;
/**
* Returns a string version of the elapsed time in seconds, with the format
* xd hh:mm:ss, hh:mm:ss, mm:ss or s.us
* @param s: elapsed seconds (given by getFloatTime) to format
*/
static string Format(double s);
protected:
/// Seconds
unsigned long m_secs; // seconds
/// Microseconds
unsigned long m_usecs; // microseconds
};
}
#endif

7
SLAM/Thirdparty/DBoW2/README.txt vendored
View File

@ -1,7 +0,0 @@
You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2).
See the original DBoW2 library at: https://github.com/dorian3d/DBoW2
All files included in this version are BSD, see LICENSE.txt
We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils.
See the original DLib library at: https://github.com/dorian3d/DLib
All files included in this version are BSD, see LICENSE.txt

178
SLAM/Thirdparty/g2o/CMakeLists.txt vendored
View File

@ -1,178 +0,0 @@
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
SET(CMAKE_LEGACY_CYGWIN_WIN32 0)
PROJECT(g2o)
SET(g2o_C_FLAGS)
SET(g2o_CXX_FLAGS)
# default built type
IF(NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE Release)
ENDIF()
MESSAGE(STATUS "BUILD TYPE:" ${CMAKE_BUILD_TYPE})
SET (G2O_LIB_TYPE SHARED)
# There seems to be an issue with MSVC8
# see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=83
if(MSVC90)
add_definitions(-DEIGEN_DONT_ALIGN_STATICALLY=1)
message(STATUS "Disabling memory alignment for MSVC8")
endif(MSVC90)
# Set the output directory for the build executables and libraries
IF(WIN32)
SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/bin CACHE PATH "Target for the libraries")
ELSE(WIN32)
SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/lib CACHE PATH "Target for the libraries")
ENDIF(WIN32)
SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${g2o_RUNTIME_OUTPUT_DIRECTORY})
# Set search directory for looking for our custom CMake scripts to
# look for SuiteSparse, QGLViewer, and Eigen3.
LIST(APPEND CMAKE_MODULE_PATH ${g2o_SOURCE_DIR}/cmake_modules)
# Detect OS and define macros appropriately
IF(UNIX)
ADD_DEFINITIONS(-DUNIX)
MESSAGE(STATUS "Compiling on Unix")
ENDIF(UNIX)
# Eigen library parallelise itself, though, presumably due to performance issues
# OPENMP is experimental. We experienced some slowdown with it
FIND_PACKAGE(OpenMP)
SET(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)")
IF(OPENMP_FOUND AND G2O_USE_OPENMP)
SET (G2O_OPENMP 1)
SET(g2o_C_FLAGS "${g2o_C_FLAGS} ${OpenMP_C_FLAGS}")
SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}")
MESSAGE(STATUS "Compiling with OpenMP support")
ENDIF(OPENMP_FOUND AND G2O_USE_OPENMP)
# Compiler specific options for gcc
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native")
SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native")
# SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3")
# SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3")
# activate warnings !!!
SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W")
SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall -W")
# specifying compiler flags
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${g2o_CXX_FLAGS}")
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}")
# Find Eigen3
SET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE})
FIND_PACKAGE(Eigen3 3.1.0 REQUIRED)
IF(EIGEN3_FOUND)
SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH "Directory of Eigen3")
ELSE(EIGEN3_FOUND)
SET(G2O_EIGEN3_INCLUDE "" CACHE PATH "Directory of Eigen3")
ENDIF(EIGEN3_FOUND)
# Generate config.h
SET(G2O_CXX_COMPILER "${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}")
configure_file(config.h.in ${g2o_SOURCE_DIR}/config.h)
# Set up the top-level include directories
INCLUDE_DIRECTORIES(
${g2o_SOURCE_DIR}/core
${g2o_SOURCE_DIR}/types
${g2o_SOURCE_DIR}/stuff
${G2O_EIGEN3_INCLUDE})
# Include the subdirectories
ADD_LIBRARY(g2o ${G2O_LIB_TYPE}
#types
g2o/types/types_sba.h
g2o/types/types_six_dof_expmap.h
g2o/types/types_sba.cpp
g2o/types/types_six_dof_expmap.cpp
g2o/types/types_seven_dof_expmap.cpp
g2o/types/types_seven_dof_expmap.h
g2o/types/se3quat.h
g2o/types/se3_ops.h
g2o/types/se3_ops.hpp
#core
g2o/core/base_edge.h
g2o/core/base_binary_edge.h
g2o/core/hyper_graph_action.cpp
g2o/core/base_binary_edge.hpp
g2o/core/hyper_graph_action.h
g2o/core/base_multi_edge.h
g2o/core/hyper_graph.cpp
g2o/core/base_multi_edge.hpp
g2o/core/hyper_graph.h
g2o/core/base_unary_edge.h
g2o/core/linear_solver.h
g2o/core/base_unary_edge.hpp
g2o/core/marginal_covariance_cholesky.cpp
g2o/core/base_vertex.h
g2o/core/marginal_covariance_cholesky.h
g2o/core/base_vertex.hpp
g2o/core/matrix_structure.cpp
g2o/core/batch_stats.cpp
g2o/core/matrix_structure.h
g2o/core/batch_stats.h
g2o/core/openmp_mutex.h
g2o/core/block_solver.h
g2o/core/block_solver.hpp
g2o/core/parameter.cpp
g2o/core/parameter.h
g2o/core/cache.cpp
g2o/core/cache.h
g2o/core/optimizable_graph.cpp
g2o/core/optimizable_graph.h
g2o/core/solver.cpp
g2o/core/solver.h
g2o/core/creators.h
g2o/core/optimization_algorithm_factory.cpp
g2o/core/estimate_propagator.cpp
g2o/core/optimization_algorithm_factory.h
g2o/core/estimate_propagator.h
g2o/core/factory.cpp
g2o/core/optimization_algorithm_property.h
g2o/core/factory.h
g2o/core/sparse_block_matrix.h
g2o/core/sparse_optimizer.cpp
g2o/core/sparse_block_matrix.hpp
g2o/core/sparse_optimizer.h
g2o/core/hyper_dijkstra.cpp
g2o/core/hyper_dijkstra.h
g2o/core/parameter_container.cpp
g2o/core/parameter_container.h
g2o/core/optimization_algorithm.cpp
g2o/core/optimization_algorithm.h
g2o/core/optimization_algorithm_with_hessian.cpp
g2o/core/optimization_algorithm_with_hessian.h
g2o/core/optimization_algorithm_levenberg.cpp
g2o/core/optimization_algorithm_levenberg.h
g2o/core/optimization_algorithm_gauss_newton.cpp
g2o/core/optimization_algorithm_gauss_newton.h
g2o/core/jacobian_workspace.cpp
g2o/core/jacobian_workspace.h
g2o/core/robust_kernel.cpp
g2o/core/robust_kernel.h
g2o/core/robust_kernel_factory.cpp
g2o/core/robust_kernel_factory.h
g2o/core/robust_kernel_impl.cpp
g2o/core/robust_kernel_impl.h
#stuff
g2o/stuff/string_tools.h
g2o/stuff/color_macros.h
g2o/stuff/macros.h
g2o/stuff/timeutil.cpp
g2o/stuff/misc.h
g2o/stuff/timeutil.h
g2o/stuff/os_specific.c
g2o/stuff/os_specific.h
g2o/stuff/string_tools.cpp
g2o/stuff/property.cpp
g2o/stuff/property.h
)

3
SLAM/Thirdparty/g2o/README.txt vendored
View File

@ -1,3 +0,0 @@
You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2).
See the original g2o library at: https://github.com/RainerKuemmerle/g2o
All files included in this g2o version are BSD, see license-bsd.txt

13
SLAM/Thirdparty/g2o/config.h.in vendored
View File

@ -1,13 +0,0 @@
#ifndef G2O_CONFIG_H
#define G2O_CONFIG_H
#cmakedefine G2O_OPENMP 1
#cmakedefine G2O_SHARED_LIBS 1
// give a warning if Eigen defaults to row-major matrices.
// We internally assume column-major matrices throughout the code.
#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
# error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)"
#endif
#endif

119
SLAM/Thirdparty/g2o/g2o/core/base_binary_edge.h vendored
View File

@ -1,119 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_BASE_BINARY_EDGE_H
#define G2O_BASE_BINARY_EDGE_H
#include <iostream>
#include <limits>
#include "base_edge.h"
#include "robust_kernel.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseBinaryEdge : public BaseEdge<D, E>
{
public:
typedef VertexXi VertexXiType;
typedef VertexXj VertexXjType;
static const int Di = VertexXiType::Dimension;
static const int Dj = VertexXjType::Dimension;
static const int Dimension = BaseEdge<D, E>::Dimension;
typedef typename BaseEdge<D,E>::Measurement Measurement;
typedef typename Matrix<double, D, Di>::AlignedMapType JacobianXiOplusType;
typedef typename Matrix<double, D, Dj>::AlignedMapType JacobianXjOplusType;
typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
typedef typename BaseEdge<D,E>::InformationType InformationType;
typedef Eigen::Map<Matrix<double, Di, Dj>, Matrix<double, Di, Dj>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
typedef Eigen::Map<Matrix<double, Dj, Di>, Matrix<double, Dj, Di>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType;
BaseBinaryEdge() : BaseEdge<D,E>(),
_hessianRowMajor(false),
_hessian(0, VertexXiType::Dimension, VertexXjType::Dimension), // HACK we map to the null pointer for initializing the Maps
_hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension),
_jacobianOplusXi(0, D, Di), _jacobianOplusXj(0, D, Dj)
{
_vertices.resize(2);
}
virtual OptimizableGraph::Vertex* createFrom();
virtual OptimizableGraph::Vertex* createTo();
virtual void resize(size_t size);
virtual bool allVerticesFixed() const;
virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
/**
* Linearizes the oplus operator in the vertex, and stores
* the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
*/
virtual void linearizeOplus();
//! returns the result of the linearization in the manifold space for the node xi
const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}
//! returns the result of the linearization in the manifold space for the node xj
const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;}
virtual void constructQuadraticForm() ;
virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);
using BaseEdge<D,E>::resize;
using BaseEdge<D,E>::computeError;
protected:
using BaseEdge<D,E>::_measurement;
using BaseEdge<D,E>::_information;
using BaseEdge<D,E>::_error;
using BaseEdge<D,E>::_vertices;
using BaseEdge<D,E>::_dimension;
bool _hessianRowMajor;
HessianBlockType _hessian;
HessianBlockTransposedType _hessianTransposed;
JacobianXiOplusType _jacobianOplusXi;
JacobianXjOplusType _jacobianOplusXj;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#include "base_binary_edge.hpp"
} // end namespace g2o
#endif

2
SLAM/Thirdparty/g2o/g2o/core/base_binary_edge.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_binary_edge.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_binary_edge.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

218
SLAM/Thirdparty/g2o/g2o/core/base_binary_edge.hpp vendored
View File

@ -1,218 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
template <int D, typename E, typename VertexXiType, typename VertexXjType>
OptimizableGraph::Vertex* BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::createFrom(){
return new VertexXiType();
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
OptimizableGraph::Vertex* BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::createTo(){
return new VertexXjType();
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::resize(size_t size)
{
if (size != 2) {
std::cerr << "WARNING, attempting to resize binary edge " << BaseEdge<D, E>::id() << " to " << size << std::endl;
}
BaseEdge<D, E>::resize(size);
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
bool BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::allVerticesFixed() const
{
return (static_cast<const VertexXiType*> (_vertices[0])->fixed() &&
static_cast<const VertexXjType*> (_vertices[1])->fixed());
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::constructQuadraticForm()
{
VertexXiType* from = static_cast<VertexXiType*>(_vertices[0]);
VertexXjType* to = static_cast<VertexXjType*>(_vertices[1]);
// get the Jacobian of the nodes in the manifold domain
const JacobianXiOplusType& A = jacobianOplusXi();
const JacobianXjOplusType& B = jacobianOplusXj();
bool fromNotFixed = !(from->fixed());
bool toNotFixed = !(to->fixed());
if (fromNotFixed || toNotFixed) {
#ifdef G2O_OPENMP
from->lockQuadraticForm();
to->lockQuadraticForm();
#endif
const InformationType& omega = _information;
Matrix<double, D, 1> omega_r = - omega * _error;
if (this->robustKernel() == 0) {
if (fromNotFixed) {
Matrix<double, VertexXiType::Dimension, D> AtO = A.transpose() * omega;
from->b().noalias() += A.transpose() * omega_r;
from->A().noalias() += AtO*A;
if (toNotFixed ) {
if (_hessianRowMajor) // we have to write to the block as transposed
_hessianTransposed.noalias() += B.transpose() * AtO.transpose();
else
_hessian.noalias() += AtO * B;
}
}
if (toNotFixed) {
to->b().noalias() += B.transpose() * omega_r;
to->A().noalias() += B.transpose() * omega * B;
}
} else { // robust (weighted) error according to some kernel
double error = this->chi2();
Eigen::Vector3d rho;
this->robustKernel()->robustify(error, rho);
InformationType weightedOmega = this->robustInformation(rho);
//std::cout << PVAR(rho.transpose()) << std::endl;
//std::cout << PVAR(weightedOmega) << std::endl;
omega_r *= rho[1];
if (fromNotFixed) {
from->b().noalias() += A.transpose() * omega_r;
from->A().noalias() += A.transpose() * weightedOmega * A;
if (toNotFixed ) {
if (_hessianRowMajor) // we have to write to the block as transposed
_hessianTransposed.noalias() += B.transpose() * weightedOmega * A;
else
_hessian.noalias() += A.transpose() * weightedOmega * B;
}
}
if (toNotFixed) {
to->b().noalias() += B.transpose() * omega_r;
to->A().noalias() += B.transpose() * weightedOmega * B;
}
}
#ifdef G2O_OPENMP
to->unlockQuadraticForm();
from->unlockQuadraticForm();
#endif
}
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
{
new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di);
new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj);
linearizeOplus();
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus()
{
VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
VertexXjType* vj = static_cast<VertexXjType*>(_vertices[1]);
bool iNotFixed = !(vi->fixed());
bool jNotFixed = !(vj->fixed());
if (!iNotFixed && !jNotFixed)
return;
#ifdef G2O_OPENMP
vi->lockQuadraticForm();
vj->lockQuadraticForm();
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector errorBak;
ErrorVector errorBeforeNumeric = _error;
if (iNotFixed) {
//Xi - estimate the jacobian numerically
double add_vi[VertexXiType::Dimension];
std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);
// add small step along the unit vector in each dimension
for (int d = 0; d < VertexXiType::Dimension; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
errorBak = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
errorBak -= _error;
vi->pop();
add_vi[d] = 0.0;
_jacobianOplusXi.col(d) = scalar * errorBak;
} // end dimension
}
if (jNotFixed) {
//Xj - estimate the jacobian numerically
double add_vj[VertexXjType::Dimension];
std::fill(add_vj, add_vj + VertexXjType::Dimension, 0.0);
// add small step along the unit vector in each dimension
for (int d = 0; d < VertexXjType::Dimension; ++d) {
vj->push();
add_vj[d] = delta;
vj->oplus(add_vj);
computeError();
errorBak = _error;
vj->pop();
vj->push();
add_vj[d] = -delta;
vj->oplus(add_vj);
computeError();
errorBak -= _error;
vj->pop();
add_vj[d] = 0.0;
_jacobianOplusXj.col(d) = scalar * errorBak;
}
} // end dimension
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
vj->unlockQuadraticForm();
vi->unlockQuadraticForm();
#endif
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
{
(void) i; (void) j;
//assert(i == 0 && j == 1);
if (rowMajor) {
new (&_hessianTransposed) HessianBlockTransposedType(d, VertexXjType::Dimension, VertexXiType::Dimension);
} else {
new (&_hessian) HessianBlockType(d, VertexXiType::Dimension, VertexXjType::Dimension);
}
_hessianRowMajor = rowMajor;
}

2
SLAM/Thirdparty/g2o/g2o/core/base_binary_edge.hpp:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_binary_edge.hpp:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_binary_edge.hpp:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

110
SLAM/Thirdparty/g2o/g2o/core/base_edge.h vendored
View File

@ -1,110 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_BASE_EDGE_H
#define G2O_BASE_EDGE_H
#include <iostream>
#include <limits>
#include <Eigen/Core>
#include "optimizable_graph.h"
namespace g2o {
using namespace Eigen;
template <int D, typename E>
class BaseEdge : public OptimizableGraph::Edge
{
public:
static const int Dimension = D;
typedef E Measurement;
typedef Matrix<double, D, 1> ErrorVector;
typedef Matrix<double, D, D> InformationType;
BaseEdge() : OptimizableGraph::Edge()
{
_dimension = D;
}
virtual ~BaseEdge() {}
virtual double chi2() const
{
return _error.dot(information()*_error);
}
virtual const double* errorData() const { return _error.data();}
virtual double* errorData() { return _error.data();}
const ErrorVector& error() const { return _error;}
ErrorVector& error() { return _error;}
//! information matrix of the constraint
const InformationType& information() const { return _information;}
InformationType& information() { return _information;}
void setInformation(const InformationType& information) { _information = information;}
virtual const double* informationData() const { return _information.data();}
virtual double* informationData() { return _information.data();}
//! accessor functions for the measurement represented by the edge
const Measurement& measurement() const { return _measurement;}
virtual void setMeasurement(const Measurement& m) { _measurement = m;}
virtual int rank() const {return _dimension;}
virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)
{
std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl;
}
protected:
Measurement _measurement;
InformationType _information;
ErrorVector _error;
/**
* calculate the robust information matrix by updating the information matrix of the error
*/
InformationType robustInformation(const Eigen::Vector3d& rho)
{
InformationType result = rho[1] * _information;
//ErrorVector weightedErrror = _information * _error;
//result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose());
return result;
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace g2o
#endif

2
SLAM/Thirdparty/g2o/g2o/core/base_edge.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_edge.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_edge.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

113
SLAM/Thirdparty/g2o/g2o/core/base_multi_edge.h vendored
View File

@ -1,113 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_BASE_MULTI_EDGE_H
#define G2O_BASE_MULTI_EDGE_H
#include <iostream>
#include <iomanip>
#include <limits>
#include <Eigen/StdVector>
#include "base_edge.h"
#include "robust_kernel.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
/**
* \brief base class to represent an edge connecting an arbitrary number of nodes
*
* D - Dimension of the measurement
* E - type to represent the measurement
*/
template <int D, typename E>
class BaseMultiEdge : public BaseEdge<D,E>
{
public:
/**
* \brief helper for mapping the Hessian memory of the upper triangular block
*/
struct HessianHelper {
Eigen::Map<MatrixXd> matrix; ///< the mapped memory
bool transposed; ///< the block has to be transposed
HessianHelper() : matrix(0, 0, 0), transposed(false) {}
};
public:
static const int Dimension = BaseEdge<D,E>::Dimension;
typedef typename BaseEdge<D,E>::Measurement Measurement;
typedef MatrixXd::MapType JacobianType;
typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
typedef typename BaseEdge<D,E>::InformationType InformationType;
typedef Eigen::Map<MatrixXd, MatrixXd::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
BaseMultiEdge() : BaseEdge<D,E>()
{
}
virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
/**
* Linearizes the oplus operator in the vertex, and stores
* the result in temporary variable vector _jacobianOplus
*/
virtual void linearizeOplus();
virtual void resize(size_t size);
virtual bool allVerticesFixed() const;
virtual void constructQuadraticForm() ;
virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);
using BaseEdge<D,E>::computeError;
protected:
using BaseEdge<D,E>::_measurement;
using BaseEdge<D,E>::_information;
using BaseEdge<D,E>::_error;
using BaseEdge<D,E>::_vertices;
using BaseEdge<D,E>::_dimension;
std::vector<HessianHelper> _hessian;
std::vector<JacobianType, aligned_allocator<JacobianType> > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus)
void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#include "base_multi_edge.hpp"
} // end namespace g2o
#endif

2
SLAM/Thirdparty/g2o/g2o/core/base_multi_edge.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_multi_edge.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_multi_edge.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

222
SLAM/Thirdparty/g2o/g2o/core/base_multi_edge.hpp vendored
View File

@ -1,222 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace internal {
inline int computeUpperTriangleIndex(int i, int j)
{
int elemsUpToCol = ((j-1) * j) / 2;
return elemsUpToCol + i;
}
}
template <int D, typename E>
void BaseMultiEdge<D, E>::constructQuadraticForm()
{
if (this->robustKernel()) {
double error = this->chi2();
Eigen::Vector3d rho;
this->robustKernel()->robustify(error, rho);
Matrix<double, D, 1> omega_r = - _information * _error;
omega_r *= rho[1];
computeQuadraticForm(this->robustInformation(rho), omega_r);
} else {
computeQuadraticForm(_information, - _information * _error);
}
}
template <int D, typename E>
void BaseMultiEdge<D, E>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
{
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
assert(v->dimension() >= 0);
new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension());
}
linearizeOplus();
}
template <int D, typename E>
void BaseMultiEdge<D, E>::linearizeOplus()
{
#ifdef G2O_OPENMP
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->lockQuadraticForm();
}
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector errorBak;
ErrorVector errorBeforeNumeric = _error;
for (size_t i = 0; i < _vertices.size(); ++i) {
//Xi - estimate the jacobian numerically
OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
if (vi->fixed())
continue;
const int vi_dim = vi->dimension();
assert(vi_dim >= 0);
#ifdef _MSC_VER
double* add_vi = new double[vi_dim];
#else
double add_vi[vi_dim];
#endif
std::fill(add_vi, add_vi + vi_dim, 0.0);
assert(_dimension >= 0);
assert(_jacobianOplus[i].rows() == _dimension && _jacobianOplus[i].cols() == vi_dim && "jacobian cache dimension does not match");
_jacobianOplus[i].resize(_dimension, vi_dim);
// add small step along the unit vector in each dimension
for (int d = 0; d < vi_dim; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
errorBak = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
errorBak -= _error;
vi->pop();
add_vi[d] = 0.0;
_jacobianOplus[i].col(d) = scalar * errorBak;
} // end dimension
#ifdef _MSC_VER
delete[] add_vi;
#endif
}
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->unlockQuadraticForm();
}
#endif
}
template <int D, typename E>
void BaseMultiEdge<D, E>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
{
int idx = internal::computeUpperTriangleIndex(i, j);
assert(idx < (int)_hessian.size());
OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(i));
OptimizableGraph::Vertex* vj = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(j));
assert(vi->dimension() >= 0);
assert(vj->dimension() >= 0);
HessianHelper& h = _hessian[idx];
if (rowMajor) {
if (h.matrix.data() != d || h.transposed != rowMajor)
new (&h.matrix) HessianBlockType(d, vj->dimension(), vi->dimension());
} else {
if (h.matrix.data() != d || h.transposed != rowMajor)
new (&h.matrix) HessianBlockType(d, vi->dimension(), vj->dimension());
}
h.transposed = rowMajor;
}
template <int D, typename E>
void BaseMultiEdge<D, E>::resize(size_t size)
{
BaseEdge<D,E>::resize(size);
int n = (int)_vertices.size();
int maxIdx = (n * (n-1))/2;
assert(maxIdx >= 0);
_hessian.resize(maxIdx);
_jacobianOplus.resize(size, JacobianType(0,0,0));
}
template <int D, typename E>
bool BaseMultiEdge<D, E>::allVerticesFixed() const
{
for (size_t i = 0; i < _vertices.size(); ++i) {
if (!static_cast<const OptimizableGraph::Vertex*> (_vertices[i])->fixed()) {
return false;
}
}
return true;
}
template <int D, typename E>
void BaseMultiEdge<D, E>::computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError)
{
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
bool istatus = !(from->fixed());
if (istatus) {
const MatrixXd& A = _jacobianOplus[i];
MatrixXd AtO = A.transpose() * omega;
int fromDim = from->dimension();
assert(fromDim >= 0);
Eigen::Map<MatrixXd> fromMap(from->hessianData(), fromDim, fromDim);
Eigen::Map<VectorXd> fromB(from->bData(), fromDim);
// ii block in the hessian
#ifdef G2O_OPENMP
from->lockQuadraticForm();
#endif
fromMap.noalias() += AtO * A;
fromB.noalias() += A.transpose() * weightedError;
// compute the off-diagonal blocks ij for all j
for (size_t j = i+1; j < _vertices.size(); ++j) {
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(_vertices[j]);
#ifdef G2O_OPENMP
to->lockQuadraticForm();
#endif
bool jstatus = !(to->fixed());
if (jstatus) {
const MatrixXd& B = _jacobianOplus[j];
int idx = internal::computeUpperTriangleIndex(i, j);
assert(idx < (int)_hessian.size());
HessianHelper& hhelper = _hessian[idx];
if (hhelper.transposed) { // we have to write to the block as transposed
hhelper.matrix.noalias() += B.transpose() * AtO.transpose();
} else {
hhelper.matrix.noalias() += AtO * B;
}
}
#ifdef G2O_OPENMP
to->unlockQuadraticForm();
#endif
}
#ifdef G2O_OPENMP
from->unlockQuadraticForm();
#endif
}
}
}

2
SLAM/Thirdparty/g2o/g2o/core/base_multi_edge.hpp:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_multi_edge.hpp:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_multi_edge.hpp:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

100
SLAM/Thirdparty/g2o/g2o/core/base_unary_edge.h vendored
View File

@ -1,100 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_BASE_UNARY_EDGE_H
#define G2O_BASE_UNARY_EDGE_H
#include <iostream>
#include <cassert>
#include <limits>
#include "base_edge.h"
#include "robust_kernel.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
template <int D, typename E, typename VertexXi>
class BaseUnaryEdge : public BaseEdge<D,E>
{
public:
static const int Dimension = BaseEdge<D, E>::Dimension;
typedef typename BaseEdge<D,E>::Measurement Measurement;
typedef VertexXi VertexXiType;
typedef typename Matrix<double, D, VertexXiType::Dimension>::AlignedMapType JacobianXiOplusType;
typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
typedef typename BaseEdge<D,E>::InformationType InformationType;
BaseUnaryEdge() : BaseEdge<D,E>(),
_jacobianOplusXi(0, D, VertexXiType::Dimension)
{
_vertices.resize(1);
}
virtual void resize(size_t size);
virtual bool allVerticesFixed() const;
virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
/**
* Linearizes the oplus operator in the vertex, and stores
* the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
*/
virtual void linearizeOplus();
//! returns the result of the linearization in the manifold space for the node xi
const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}
virtual void constructQuadraticForm();
virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to);
virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");}
using BaseEdge<D,E>::resize;
using BaseEdge<D,E>::computeError;
protected:
using BaseEdge<D,E>::_measurement;
using BaseEdge<D,E>::_information;
using BaseEdge<D,E>::_error;
using BaseEdge<D,E>::_vertices;
using BaseEdge<D,E>::_dimension;
JacobianXiOplusType _jacobianOplusXi;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#include "base_unary_edge.hpp"
} // end namespace g2o
#endif

2
SLAM/Thirdparty/g2o/g2o/core/base_unary_edge.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_unary_edge.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_unary_edge.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

129
SLAM/Thirdparty/g2o/g2o/core/base_unary_edge.hpp vendored
View File

@ -1,129 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::resize(size_t size)
{
if (size != 1) {
std::cerr << "WARNING, attempting to resize unary edge " << BaseEdge<D, E>::id() << " to " << size << std::endl;
}
BaseEdge<D, E>::resize(size);
}
template <int D, typename E, typename VertexXiType>
bool BaseUnaryEdge<D, E, VertexXiType>::allVerticesFixed() const
{
return static_cast<const VertexXiType*> (_vertices[0])->fixed();
}
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::constructQuadraticForm()
{
VertexXiType* from=static_cast<VertexXiType*>(_vertices[0]);
// chain rule to get the Jacobian of the nodes in the manifold domain
const JacobianXiOplusType& A = jacobianOplusXi();
const InformationType& omega = _information;
bool istatus = !from->fixed();
if (istatus) {
#ifdef G2O_OPENMP
from->lockQuadraticForm();
#endif
if (this->robustKernel()) {
double error = this->chi2();
Eigen::Vector3d rho;
this->robustKernel()->robustify(error, rho);
InformationType weightedOmega = this->robustInformation(rho);
from->b().noalias() -= rho[1] * A.transpose() * omega * _error;
from->A().noalias() += A.transpose() * weightedOmega * A;
} else {
from->b().noalias() -= A.transpose() * omega * _error;
from->A().noalias() += A.transpose() * omega * A;
}
#ifdef G2O_OPENMP
from->unlockQuadraticForm();
#endif
}
}
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
{
new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension);
linearizeOplus();
}
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::linearizeOplus()
{
//Xi - estimate the jacobian numerically
VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
if (vi->fixed())
return;
#ifdef G2O_OPENMP
vi->lockQuadraticForm();
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector error1;
ErrorVector errorBeforeNumeric = _error;
double add_vi[VertexXiType::Dimension];
std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);
// add small step along the unit vector in each dimension
for (int d = 0; d < VertexXiType::Dimension; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
error1 = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
vi->pop();
add_vi[d] = 0.0;
_jacobianOplusXi.col(d) = scalar * (error1 - _error);
} // end dimension
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
vi->unlockQuadraticForm();
#endif
}
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)
{
std::cerr << __PRETTY_FUNCTION__ << " is not implemented, please give implementation in your derived class" << std::endl;
}

2
SLAM/Thirdparty/g2o/g2o/core/base_unary_edge.hpp:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_unary_edge.hpp:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_unary_edge.hpp:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

120
SLAM/Thirdparty/g2o/g2o/core/base_vertex.h vendored
View File

@ -1,120 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_BASE_VERTEX_H
#define G2O_BASE_VERTEX_H
#include "optimizable_graph.h"
#include "creators.h"
#include "../stuff/macros.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/StdVector>
#include <stack>
namespace g2o {
using namespace Eigen;
/**
* \brief Templatized BaseVertex
*
* Templatized BaseVertex
* D : minimal dimension of the vertex, e.g., 3 for rotation in 3D
* T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
*/
template <int D, typename T>
class BaseVertex : public OptimizableGraph::Vertex {
public:
typedef T EstimateType;
typedef std::stack<EstimateType,
std::vector<EstimateType, Eigen::aligned_allocator<EstimateType> > >
BackupStackType;
static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space
typedef Eigen::Map<Matrix<double, D, D>, Matrix<double,D,D>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
public:
BaseVertex();
virtual const double& hessian(int i, int j) const { assert(i<D && j<D); return _hessian(i,j);}
virtual double& hessian(int i, int j) { assert(i<D && j<D); return _hessian(i,j);}
virtual double hessianDeterminant() const {return _hessian.determinant();}
virtual double* hessianData() { return const_cast<double*>(_hessian.data());}
virtual void mapHessianMemory(double* d);
virtual int copyB(double* b_) const {
memcpy(b_, _b.data(), Dimension * sizeof(double));
return Dimension;
}
virtual const double& b(int i) const { assert(i < D); return _b(i);}
virtual double& b(int i) { assert(i < D); return _b(i);}
virtual double* bData() { return _b.data();}
virtual void clearQuadraticForm();
//! updates the current vertex with the direct solution x += H_ii\b_ii
//! @returns the determinant of the inverted hessian
virtual double solveDirect(double lambda=0);
//! return right hand side b of the constructed linear system
Matrix<double, D, 1>& b() { return _b;}
const Matrix<double, D, 1>& b() const { return _b;}
//! return the hessian block associated with the vertex
HessianBlockType& A() { return _hessian;}
const HessianBlockType& A() const { return _hessian;}
virtual void push() { _backup.push(_estimate);}
virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}
virtual void discardTop() { assert(!_backup.empty()); _backup.pop();}
virtual int stackSize() const {return _backup.size();}
//! return the current estimate of the vertex
const EstimateType& estimate() const { return _estimate;}
//! set the estimate for the vertex also calls updateCache()
void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}
protected:
HessianBlockType _hessian;
Matrix<double, D, 1> _b;
EstimateType _estimate;
BackupStackType _backup;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#include "base_vertex.hpp"
} // end namespace g2o
#endif

2
SLAM/Thirdparty/g2o/g2o/core/base_vertex.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_vertex.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_vertex.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

55
SLAM/Thirdparty/g2o/g2o/core/base_vertex.hpp vendored
View File

@ -1,55 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
template <int D, typename T>
BaseVertex<D, T>::BaseVertex() :
OptimizableGraph::Vertex(),
_hessian(0, D, D)
{
_dimension = D;
}
template <int D, typename T>
double BaseVertex<D, T>::solveDirect(double lambda) {
Matrix <double, D, D> tempA=_hessian + Matrix <double, D, D>::Identity()*lambda;
double det=tempA.determinant();
if (g2o_isnan(det) || det < std::numeric_limits<double>::epsilon())
return det;
Matrix <double, D, 1> dx=tempA.llt().solve(_b);
oplus(&dx[0]);
return det;
}
template <int D, typename T>
void BaseVertex<D, T>::clearQuadraticForm() {
_b.setZero();
}
template <int D, typename T>
void BaseVertex<D, T>::mapHessianMemory(double* d)
{
new (&_hessian) HessianBlockType(d, D, D);
}

2
SLAM/Thirdparty/g2o/g2o/core/base_vertex.hpp:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_vertex.hpp:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/base_vertex.hpp:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

90
SLAM/Thirdparty/g2o/g2o/core/batch_stats.cpp vendored
View File

@ -1,90 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "batch_stats.h"
#include <cstring>
namespace g2o {
using namespace std;
G2OBatchStatistics* G2OBatchStatistics::_globalStats=0;
#ifndef PTHING
#define PTHING(s) \
#s << "= " << (st.s) << "\t "
#endif
G2OBatchStatistics::G2OBatchStatistics(){
// zero all.
memset (this, 0, sizeof(G2OBatchStatistics));
// set the iteration to -1 to show that it isn't valid
iteration = -1;
}
std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st)
{
os << PTHING(iteration);
os << PTHING( numVertices ); // how many vertices are involved
os << PTHING( numEdges ); // hoe many edges
os << PTHING( chi2 ); // total chi2
/** timings **/
// nonlinear part
os << PTHING( timeResiduals );
os << PTHING( timeLinearize ); // jacobians
os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph
// block_solver (constructs Ax=b, plus maybe schur);
os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done);
// linear solver (computes Ax=b); );
os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done);
os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done);
os << PTHING( timeLinearSolution ); // total time for solving Ax=b
os << PTHING( iterationsLinearSolver ); // iterations of PCG
os << PTHING( timeUpdate ); // oplus
os << PTHING( timeIteration ); // total time );
os << PTHING( levenbergIterations );
os << PTHING( timeLinearSolver);
os << PTHING(hessianDimension);
os << PTHING(hessianPoseDimension);
os << PTHING(hessianLandmarkDimension);
os << PTHING(choleskyNNZ);
os << PTHING(timeMarginals);
return os;
};
void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b)
{
_globalStats = b;
}
} // end namespace

2
SLAM/Thirdparty/g2o/g2o/core/batch_stats.cpp:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/batch_stats.cpp:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/batch_stats.cpp:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

83
SLAM/Thirdparty/g2o/g2o/core/batch_stats.h vendored
View File

@ -1,83 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_BATCH_STATS_H_
#define G2O_BATCH_STATS_H_
#include <iostream>
#include <vector>
namespace g2o {
/**
* \brief statistics about the optimization
*/
struct G2OBatchStatistics {
G2OBatchStatistics();
int iteration; ///< which iteration
int numVertices; ///< how many vertices are involved
int numEdges; ///< how many edges
double chi2; ///< total chi2
/** timings **/
// nonlinear part
double timeResiduals; ///< residuals
double timeLinearize; ///< jacobians
double timeQuadraticForm; ///< construct the quadratic form in the graph
int levenbergIterations; ///< number of iterations performed by LM
// block_solver (constructs Ax=b, plus maybe schur)
double timeSchurComplement; ///< compute schur complement (0 if not done)
// linear solver (computes Ax=b);
double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done)
double timeNumericDecomposition; ///< numeric decomposition (0 if not done)
double timeLinearSolution; ///< total time for solving Ax=b (including detup for schur)
double timeLinearSolver; ///< time for solving, excluding Schur setup
int iterationsLinearSolver; ///< iterations of PCG, (0 if not used, i.e., Cholesky)
double timeUpdate; ///< time to apply the update
double timeIteration; ///< total time;
double timeMarginals; ///< computing the inverse elements (solve blocks) and thus the marginal covariances
// information about the Hessian matrix
size_t hessianDimension; ///< rows / cols of the Hessian
size_t hessianPoseDimension; ///< dimension of the pose matrix in Schur
size_t hessianLandmarkDimension; ///< dimension of the landmark matrix in Schur
size_t choleskyNNZ; ///< number of non-zeros in the cholesky factor
static G2OBatchStatistics* globalStats() {return _globalStats;}
static void setGlobalStats(G2OBatchStatistics* b);
protected:
static G2OBatchStatistics* _globalStats;
};
std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&);
typedef std::vector<G2OBatchStatistics> BatchStatisticsContainer;
}
#endif

2
SLAM/Thirdparty/g2o/g2o/core/batch_stats.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/batch_stats.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/batch_stats.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

193
SLAM/Thirdparty/g2o/g2o/core/block_solver.h vendored
View File

@ -1,193 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_BLOCK_SOLVER_H
#define G2O_BLOCK_SOLVER_H
#include <Eigen/Core>
#include "solver.h"
#include "linear_solver.h"
#include "sparse_block_matrix.h"
#include "sparse_block_matrix_diagonal.h"
#include "openmp_mutex.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
/**
* \brief traits to summarize the properties of the fixed size optimization problem
*/
template <int _PoseDim, int _LandmarkDim>
struct BlockSolverTraits
{
static const int PoseDim = _PoseDim;
static const int LandmarkDim = _LandmarkDim;
typedef Matrix<double, PoseDim, PoseDim> PoseMatrixType;
typedef Matrix<double, LandmarkDim, LandmarkDim> LandmarkMatrixType;
typedef Matrix<double, PoseDim, LandmarkDim> PoseLandmarkMatrixType;
typedef Matrix<double, PoseDim, 1> PoseVectorType;
typedef Matrix<double, LandmarkDim, 1> LandmarkVectorType;
typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
typedef LinearSolver<PoseMatrixType> LinearSolverType;
};
/**
* \brief traits to summarize the properties of the dynamic size optimization problem
*/
template <>
struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>
{
static const int PoseDim = Eigen::Dynamic;
static const int LandmarkDim = Eigen::Dynamic;
typedef MatrixXd PoseMatrixType;
typedef MatrixXd LandmarkMatrixType;
typedef MatrixXd PoseLandmarkMatrixType;
typedef VectorXd PoseVectorType;
typedef VectorXd LandmarkVectorType;
typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
typedef LinearSolver<PoseMatrixType> LinearSolverType;
};
/**
* \brief base for the block solvers with some basic function interfaces
*/
class BlockSolverBase : public Solver
{
public:
virtual ~BlockSolverBase() {}
/**
* compute dest = H * src
*/
virtual void multiplyHessian(double* dest, const double* src) const = 0;
};
/**
* \brief Implementation of a solver operating on the blocks of the Hessian
*/
template <typename Traits>
class BlockSolver: public BlockSolverBase {
public:
static const int PoseDim = Traits::PoseDim;
static const int LandmarkDim = Traits::LandmarkDim;
typedef typename Traits::PoseMatrixType PoseMatrixType;
typedef typename Traits::LandmarkMatrixType LandmarkMatrixType;
typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
typedef typename Traits::PoseVectorType PoseVectorType;
typedef typename Traits::LandmarkVectorType LandmarkVectorType;
typedef typename Traits::PoseHessianType PoseHessianType;
typedef typename Traits::LandmarkHessianType LandmarkHessianType;
typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
typedef typename Traits::LinearSolverType LinearSolverType;
public:
/**
* allocate a block solver ontop of the underlying linear solver.
* NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer
* in its destructor.
*/
BlockSolver(LinearSolverType* linearSolver);
~BlockSolver();
virtual bool init(SparseOptimizer* optmizer, bool online = false);
virtual bool buildStructure(bool zeroBlocks = false);
virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
virtual bool buildSystem();
virtual bool solve();
virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
virtual bool setLambda(double lambda, bool backup = false);
virtual void restoreDiagonal();
virtual bool supportsSchur() {return true;}
virtual bool schur() { return _doSchur;}
virtual void setSchur(bool s) { _doSchur = s;}
LinearSolver<PoseMatrixType>* linearSolver() const { return _linearSolver;}
virtual void setWriteDebug(bool writeDebug);
virtual bool writeDebug() const {return _linearSolver->writeDebug();}
virtual bool saveHessian(const std::string& fileName) const;
virtual void multiplyHessian(double* dest, const double* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}
protected:
void resize(int* blockPoseIndices, int numPoseBlocks,
int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);
void deallocate();
SparseBlockMatrix<PoseMatrixType>* _Hpp;
SparseBlockMatrix<LandmarkMatrixType>* _Hll;
SparseBlockMatrix<PoseLandmarkMatrixType>* _Hpl;
SparseBlockMatrix<PoseMatrixType>* _Hschur;
SparseBlockMatrixDiagonal<LandmarkMatrixType>* _DInvSchur;
SparseBlockMatrixCCS<PoseLandmarkMatrixType>* _HplCCS;
SparseBlockMatrixCCS<PoseMatrixType>* _HschurTransposedCCS;
LinearSolver<PoseMatrixType>* _linearSolver;
std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;
std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;
# ifdef G2O_OPENMP
std::vector<OpenMPMutex> _coefficientsMutex;
# endif
bool _doSchur;
double* _coefficients;
double* _bschur;
int _numPoses, _numLandmarks;
int _sizePoses, _sizeLandmarks;
};
//variable size solver
typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > BlockSolverX;
// solver for BA/3D SLAM
typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;
// solver fo BA with scale
typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;
// 2Dof landmarks 3Dof poses
typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;
} // end namespace
#include "block_solver.hpp"
#endif

2
SLAM/Thirdparty/g2o/g2o/core/block_solver.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/block_solver.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/block_solver.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

634
SLAM/Thirdparty/g2o/g2o/core/block_solver.hpp vendored
View File

@ -1,634 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "sparse_optimizer.h"
#include <Eigen/LU>
#include <fstream>
#include <iomanip>
#include "../stuff/timeutil.h"
#include "../stuff/macros.h"
#include "../stuff/misc.h"
namespace g2o {
using namespace std;
using namespace Eigen;
template <typename Traits>
BlockSolver<Traits>::BlockSolver(LinearSolverType* linearSolver) :
BlockSolverBase(),
_linearSolver(linearSolver)
{
// workspace
_Hpp=0;
_Hll=0;
_Hpl=0;
_HplCCS = 0;
_HschurTransposedCCS = 0;
_Hschur=0;
_DInvSchur=0;
_coefficients=0;
_bschur = 0;
_xSize=0;
_numPoses=0;
_numLandmarks=0;
_sizePoses=0;
_sizeLandmarks=0;
_doSchur=true;
}
template <typename Traits>
void BlockSolver<Traits>::resize(int* blockPoseIndices, int numPoseBlocks,
int* blockLandmarkIndices, int numLandmarkBlocks,
int s)
{
deallocate();
resizeVector(s);
if (_doSchur) {
// the following two are only used in schur
assert(_sizePoses > 0 && "allocating with wrong size");
_coefficients = new double [s];
_bschur = new double[_sizePoses];
}
_Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
if (_doSchur) {
_Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
_Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);
_DInvSchur = new SparseBlockMatrixDiagonal<LandmarkMatrixType>(_Hll->colBlockIndices());
_Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);
_HplCCS = new SparseBlockMatrixCCS<PoseLandmarkMatrixType>(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices());
_HschurTransposedCCS = new SparseBlockMatrixCCS<PoseMatrixType>(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices());
#ifdef G2O_OPENMP
_coefficientsMutex.resize(numPoseBlocks);
#endif
}
}
template <typename Traits>
void BlockSolver<Traits>::deallocate()
{
if (_Hpp){
delete _Hpp;
_Hpp=0;
}
if (_Hll){
delete _Hll;
_Hll=0;
}
if (_Hpl){
delete _Hpl;
_Hpl = 0;
}
if (_Hschur){
delete _Hschur;
_Hschur=0;
}
if (_DInvSchur){
delete _DInvSchur;
_DInvSchur=0;
}
if (_coefficients) {
delete[] _coefficients;
_coefficients = 0;
}
if (_bschur) {
delete[] _bschur;
_bschur = 0;
}
if (_HplCCS) {
delete _HplCCS;
_HplCCS = 0;
}
if (_HschurTransposedCCS) {
delete _HschurTransposedCCS;
_HschurTransposedCCS = 0;
}
}
template <typename Traits>
BlockSolver<Traits>::~BlockSolver()
{
delete _linearSolver;
deallocate();
}
template <typename Traits>
bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
{
assert(_optimizer);
size_t sparseDim = 0;
_numPoses=0;
_numLandmarks=0;
_sizePoses=0;
_sizeLandmarks=0;
int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
int dim = v->dimension();
if (! v->marginalized()){
v->setColInHessian(_sizePoses);
_sizePoses+=dim;
blockPoseIndices[_numPoses]=_sizePoses;
++_numPoses;
} else {
v->setColInHessian(_sizeLandmarks);
_sizeLandmarks+=dim;
blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
++_numLandmarks;
}
sparseDim += dim;
}
resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
delete[] blockLandmarkIndices;
delete[] blockPoseIndices;
// allocate the diagonal on Hpp and Hll
int poseIdx = 0;
int landmarkIdx = 0;
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
if (! v->marginalized()){
//assert(poseIdx == v->hessianIndex());
PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
if (zeroBlocks)
m->setZero();
v->mapHessianMemory(m->data());
++poseIdx;
} else {
LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
if (zeroBlocks)
m->setZero();
v->mapHessianMemory(m->data());
++landmarkIdx;
}
}
assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);
// temporary structures for building the pattern of the Schur complement
SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
if (_doSchur) {
schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
}
// here we assume that the landmark indices start after the pose ones
// create the structure in Hpp, Hll and in Hpl
for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
OptimizableGraph::Edge* e = *it;
for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
int ind1 = v1->hessianIndex();
if (ind1 == -1)
continue;
int indexV1Bak = ind1;
for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
int ind2 = v2->hessianIndex();
if (ind2 == -1)
continue;
ind1 = indexV1Bak;
bool transposedBlock = ind1 > ind2;
if (transposedBlock){ // make sure, we allocate the upper triangle block
swap(ind1, ind2);
}
if (! v1->marginalized() && !v2->marginalized()){
PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
if (_Hschur) {// assume this is only needed in case we solve with the schur complement
schurMatrixLookup->addBlock(ind1, ind2);
}
} else if (v1->marginalized() && v2->marginalized()){
// RAINER hmm.... should we ever reach this here????
LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
} else {
if (v1->marginalized()){
PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
} else {
PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
}
}
}
}
}
if (! _doSchur)
return true;
_DInvSchur->diagonal().resize(landmarkIdx);
_Hpl->fillSparseBlockMatrixCCS(*_HplCCS);
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
if (v->marginalized()){
const HyperGraph::EdgeSet& vedges=v->edges();
for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
for (size_t i=0; i<(*it1)->vertices().size(); ++i)
{
OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
if (v1->hessianIndex()==-1 || v1==v)
continue;
for (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
for (size_t j=0; j<(*it2)->vertices().size(); ++j)
{
OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
if (v2->hessianIndex()==-1 || v2==v)
continue;
int i1=v1->hessianIndex();
int i2=v2->hessianIndex();
if (i1<=i2) {
schurMatrixLookup->addBlock(i1, i2);
}
}
}
}
}
}
}
_Hschur->takePatternFromHash(*schurMatrixLookup);
delete schurMatrixLookup;
_Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);
return true;
}
template <typename Traits>
bool BlockSolver<Traits>::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
{
for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
int dim = v->dimension();
if (! v->marginalized()){
v->setColInHessian(_sizePoses);
_sizePoses+=dim;
_Hpp->rowBlockIndices().push_back(_sizePoses);
_Hpp->colBlockIndices().push_back(_sizePoses);
_Hpp->blockCols().push_back(typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
++_numPoses;
int ind = v->hessianIndex();
PoseMatrixType* m = _Hpp->block(ind, ind, true);
v->mapHessianMemory(m->data());
} else {
std::cerr << "updateStructure(): Schur not supported" << std::endl;
abort();
}
}
resizeVector(_sizePoses + _sizeLandmarks);
for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
int ind1 = v1->hessianIndex();
int indexV1Bak = ind1;
if (ind1 == -1)
continue;
for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
int ind2 = v2->hessianIndex();
if (ind2 == -1)
continue;
ind1 = indexV1Bak;
bool transposedBlock = ind1 > ind2;
if (transposedBlock) // make sure, we allocate the upper triangular block
swap(ind1, ind2);
if (! v1->marginalized() && !v2->marginalized()) {
PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
} else {
std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
}
}
}
}
return true;
}
template <typename Traits>
bool BlockSolver<Traits>::solve(){
//cerr << __PRETTY_FUNCTION__ << endl;
if (! _doSchur){
double t=get_monotonic_time();
bool ok = _linearSolver->solve(*_Hpp, _x, _b);
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
}
return ok;
}
// schur thing
// backup the coefficient matrix
double t=get_monotonic_time();
// _Hschur = _Hpp, but keeping the pattern of _Hschur
_Hschur->clear();
_Hpp->add(_Hschur);
//_DInvSchur->clear();
memset (_coefficients, 0, _sizePoses*sizeof(double));
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");
// calculate inverse block for the landmark
const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
assert (D && D->rows()==D->cols() && "Error in landmark matrix");
LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
Dinv = D->inverse();
LandmarkVectorType db(D->rows());
for (int j=0; j<D->rows(); ++j) {
db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
}
db=Dinv*db;
assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
it_outer != landmarkColumn.end(); ++it_outer) {
int i1 = it_outer->row;
const PoseLandmarkMatrixType* Bi = it_outer->block;
assert(Bi);
PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
# ifdef G2O_OPENMP
ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
# endif
Bb.noalias() += (*Bi)*db;
assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
for (; it_inner != landmarkColumn.end(); ++it_inner) {
int i2 = it_inner->row;
const PoseLandmarkMatrixType* Bj = it_inner->block;
assert(Bj);
while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
++targetColumnIt;
assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
assert(Hi1i2);
(*Hi1i2).noalias() -= BDinv*Bj->transpose();
}
}
}
//cerr << "Solve [marginalize] = " << get_monotonic_time()-t << endl;
// _bschur = _b for calling solver, and not touching _b
memcpy(_bschur, _b, _sizePoses * sizeof(double));
for (int i=0; i<_sizePoses; ++i){
_bschur[i]-=_coefficients[i];
}
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats){
globalStats->timeSchurComplement = get_monotonic_time() - t;
}
t=get_monotonic_time();
bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianPoseDimension = _Hpp->cols();
globalStats->hessianLandmarkDimension = _Hll->cols();
globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
}
//cerr << "Solve [decompose and solve] = " << get_monotonic_time()-t << endl;
if (! solvedPoses)
return false;
// _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
// solution;
double* xp = _x;
double* cp = _coefficients;
double* xl=_x+_sizePoses;
double* cl=_coefficients + _sizePoses;
double* bl=_b+_sizePoses;
// cp = -xp
for (int i=0; i<_sizePoses; ++i)
cp[i]=-xp[i];
// cl = bl
memcpy(cl,bl,_sizeLandmarks*sizeof(double));
// cl = bl - Bt * xp
//Bt->multiply(cl, cp);
_HplCCS->rightMultiply(cl, cp);
// xl = Dinv * cl
memset(xl,0, _sizeLandmarks*sizeof(double));
_DInvSchur->multiply(xl,cl);
//_DInvSchur->rightMultiply(xl,cl);
//cerr << "Solve [landmark delta] = " << get_monotonic_time()-t << endl;
return true;
}
template <typename Traits>
bool BlockSolver<Traits>::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
{
double t = get_monotonic_time();
bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeMarginals = get_monotonic_time() - t;
}
return ok;
}
template <typename Traits>
bool BlockSolver<Traits>::buildSystem()
{
// clear b vector
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
assert(v);
v->clearQuadraticForm();
}
_Hpp->clear();
if (_doSchur) {
_Hll->clear();
_Hpl->clear();
}
// resetting the terms for the pairwise constraints
// built up the current system by storing the Hessian blocks in the edges and vertices
# ifndef G2O_OPENMP
// no threading, we do not need to copy the workspace
JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
# else
// if running with threads need to produce copies of the workspace for each thread
JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
# endif
for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k) {
OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
e->constructQuadraticForm();
# ifndef NDEBUG
for (size_t i = 0; i < e->vertices().size(); ++i) {
const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
if (! v->fixed()) {
bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
if (hasANan) {
cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << endl;
break;
}
}
}
# endif
}
// flush the current system in a sparse block matrix
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
int iBase = v->colInHessian();
if (v->marginalized())
iBase+=_sizePoses;
v->copyB(_b+iBase);
}
return 0;
}
template <typename Traits>
bool BlockSolver<Traits>::setLambda(double lambda, bool backup)
{
if (backup) {
_diagonalBackupPose.resize(_numPoses);
_diagonalBackupLandmark.resize(_numLandmarks);
}
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_numPoses > 100)
# endif
for (int i = 0; i < _numPoses; ++i) {
PoseMatrixType *b=_Hpp->block(i,i);
if (backup)
_diagonalBackupPose[i] = b->diagonal();
b->diagonal().array() += lambda;
}
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_numLandmarks > 100)
# endif
for (int i = 0; i < _numLandmarks; ++i) {
LandmarkMatrixType *b=_Hll->block(i,i);
if (backup)
_diagonalBackupLandmark[i] = b->diagonal();
b->diagonal().array() += lambda;
}
return true;
}
template <typename Traits>
void BlockSolver<Traits>::restoreDiagonal()
{
assert((int) _diagonalBackupPose.size() == _numPoses && "Mismatch in dimensions");
assert((int) _diagonalBackupLandmark.size() == _numLandmarks && "Mismatch in dimensions");
for (int i = 0; i < _numPoses; ++i) {
PoseMatrixType *b=_Hpp->block(i,i);
b->diagonal() = _diagonalBackupPose[i];
}
for (int i = 0; i < _numLandmarks; ++i) {
LandmarkMatrixType *b=_Hll->block(i,i);
b->diagonal() = _diagonalBackupLandmark[i];
}
}
template <typename Traits>
bool BlockSolver<Traits>::init(SparseOptimizer* optimizer, bool online)
{
_optimizer = optimizer;
if (! online) {
if (_Hpp)
_Hpp->clear();
if (_Hpl)
_Hpl->clear();
if (_Hll)
_Hll->clear();
}
_linearSolver->init();
return true;
}
template <typename Traits>
void BlockSolver<Traits>::setWriteDebug(bool writeDebug)
{
_linearSolver->setWriteDebug(writeDebug);
}
template <typename Traits>
bool BlockSolver<Traits>::saveHessian(const std::string& fileName) const
{
return _Hpp->writeOctave(fileName.c_str(), true);
}
} // end namespace

2
SLAM/Thirdparty/g2o/g2o/core/block_solver.hpp:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/block_solver.hpp:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/block_solver.hpp:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

183
SLAM/Thirdparty/g2o/g2o/core/cache.cpp vendored
View File

@ -1,183 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "cache.h"
#include "optimizable_graph.h"
#include "factory.h"
#include <iostream>
namespace g2o {
using namespace std;
Cache::CacheKey::CacheKey() :
_type(), _parameters()
{
}
Cache::CacheKey::CacheKey(const std::string& type_, const ParameterVector& parameters_) :
_type(type_), _parameters(parameters_)
{
}
Cache::Cache(CacheContainer* container_, const ParameterVector& parameters_) :
_updateNeeded(true), _parameters(parameters_), _container(container_)
{
}
bool Cache::CacheKey::operator<(const Cache::CacheKey& c) const{
if (_type < c._type)
return true;
return std::lexicographical_compare (_parameters.begin( ), _parameters.end( ),
c._parameters.begin( ), c._parameters.end( ) );
}
OptimizableGraph::Vertex* Cache::vertex() {
if (container() )
return container()->vertex();
return 0;
}
OptimizableGraph* Cache::graph() {
if (container())
return container()->graph();
return 0;
}
CacheContainer* Cache::container() {
return _container;
}
ParameterVector& Cache::parameters() {
return _parameters;
}
Cache::CacheKey Cache::key() const {
Factory* factory=Factory::instance();
return CacheKey(factory->tag(this), _parameters);
};
void Cache::update(){
if (! _updateNeeded)
return;
for(std::vector<Cache*>::iterator it=_parentCaches.begin(); it!=_parentCaches.end(); it++){
(*it)->update();
}
updateImpl();
_updateNeeded=false;
}
Cache* Cache::installDependency(const std::string& type_, const std::vector<int>& parameterIndices){
ParameterVector pv(parameterIndices.size());
for (size_t i=0; i<parameterIndices.size(); i++){
if (parameterIndices[i]<0 || parameterIndices[i] >=(int)_parameters.size())
return 0;
pv[i]=_parameters[ parameterIndices[i] ];
}
CacheKey k(type_, pv);
if (!container())
return 0;
Cache* c=container()->findCache(k);
if (!c) {
c = container()->createCache(k);
}
if (c)
_parentCaches.push_back(c);
return c;
}
bool Cache::resolveDependancies(){
return true;
}
CacheContainer::CacheContainer(OptimizableGraph::Vertex* vertex_) {
_vertex = vertex_;
}
Cache* CacheContainer::findCache(const Cache::CacheKey& key) {
iterator it=find(key);
if (it==end())
return 0;
return it->second;
}
Cache* CacheContainer::createCache(const Cache::CacheKey& key){
Factory* f = Factory::instance();
HyperGraph::HyperGraphElement* e = f->construct(key.type());
if (!e) {
cerr << __PRETTY_FUNCTION__ << endl;
cerr << "fatal error in creating cache of type " << key.type() << endl;
return 0;
}
Cache* c = dynamic_cast<Cache*>(e);
if (! c){
cerr << __PRETTY_FUNCTION__ << endl;
cerr << "fatal error in creating cache of type " << key.type() << endl;
return 0;
}
c->_container = this;
c->_parameters = key._parameters;
if (c->resolveDependancies()){
insert(make_pair(key,c));
c->update();
return c;
}
return 0;
}
OptimizableGraph::Vertex* CacheContainer::vertex() {
return _vertex;
}
OptimizableGraph* CacheContainer::graph(){
if (_vertex)
return _vertex->graph();
return 0;
}
void CacheContainer::update() {
for (iterator it=begin(); it!=end(); it++){
(it->second)->update();
}
_updateNeeded=false;
}
void CacheContainer::setUpdateNeeded(bool needUpdate) {
_updateNeeded=needUpdate;
for (iterator it=begin(); it!=end(); ++it){
(it->second)->_updateNeeded = needUpdate;
}
}
CacheContainer::~CacheContainer(){
for (iterator it=begin(); it!=end(); ++it){
delete (it->second);
}
}
} // end namespace

2
SLAM/Thirdparty/g2o/g2o/core/cache.cpp:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/cache.cpp:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/cache.cpp:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

140
SLAM/Thirdparty/g2o/g2o/core/cache.h vendored
View File

@ -1,140 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_CACHE_HH_
#define G2O_CACHE_HH_
#include <map>
#include "optimizable_graph.h"
namespace g2o {
class CacheContainer;
class Cache: public HyperGraph::HyperGraphElement
{
public:
friend class CacheContainer;
class CacheKey
{
public:
friend class CacheContainer;
CacheKey();
CacheKey(const std::string& type_, const ParameterVector& parameters_);
bool operator<(const CacheKey& c) const;
const std::string& type() const { return _type;}
const ParameterVector& parameters() const { return _parameters;}
protected:
std::string _type;
ParameterVector _parameters;
};
Cache(CacheContainer* container_ = 0, const ParameterVector& parameters_ = ParameterVector());
CacheKey key() const;
OptimizableGraph::Vertex* vertex();
OptimizableGraph* graph();
CacheContainer* container();
ParameterVector& parameters();
void update();
virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_CACHE;}
protected:
//! redefine this to do the update
virtual void updateImpl() = 0;
/**
* this function installs and satisfies a cache
* @param type_: the typename of the dependency
* @param parameterIndices: a vector containing the indices if the parameters
* in _parameters that will be used to assemble the Key of the cache being created
* For example if I have a cache of type C2, having parameters "A, B, and C",
* and it depends on a cache of type C1 that depends on the parameters A and C,
* the parameterIndices should contain "0, 2", since they are the positions in the
* parameter vector of C2 of the parameters needed to construct C1.
* @returns the newly created cache
*/
Cache* installDependency(const std::string& type_, const std::vector<int>& parameterIndices);
/**
* Function to be called from a cache that has dependencies. It just invokes a
* sequence of installDependency().
* Although the caches returned are stored in the _parentCache vector,
* it is better that you redefine your own cache member variables, for better readability
*/
virtual bool resolveDependancies();
bool _updateNeeded;
ParameterVector _parameters;
std::vector<Cache*> _parentCaches;
CacheContainer* _container;
};
class CacheContainer: public std::map<Cache::CacheKey, Cache*>
{
public:
CacheContainer(OptimizableGraph::Vertex* vertex_);
virtual ~CacheContainer();
OptimizableGraph::Vertex* vertex();
OptimizableGraph* graph();
Cache* findCache(const Cache::CacheKey& key);
Cache* createCache(const Cache::CacheKey& key);
void setUpdateNeeded(bool needUpdate=true);
void update();
protected:
OptimizableGraph::Vertex* _vertex;
bool _updateNeeded;
};
template <typename CacheType>
void OptimizableGraph::Edge::resolveCache(CacheType*& cache,
OptimizableGraph::Vertex* v,
const std::string& type_,
const ParameterVector& parameters_)
{
cache = 0;
CacheContainer* container= v->cacheContainer();
Cache::CacheKey key(type_, parameters_);
Cache* c = container->findCache(key);
if (!c) {
c = container->createCache(key);
}
if (c) {
cache = dynamic_cast<CacheType*>(c);
}
}
} // end namespace
#endif

2
SLAM/Thirdparty/g2o/g2o/core/cache.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/cache.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/cache.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

75
SLAM/Thirdparty/g2o/g2o/core/creators.h vendored
View File

@ -1,75 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_CREATORS_H
#define G2O_CREATORS_H
#include "hyper_graph.h"
#include <string>
#include <typeinfo>
namespace g2o
{
/**
* \brief Abstract interface for allocating HyperGraphElement
*/
class AbstractHyperGraphElementCreator
{
public:
/**
* create a hyper graph element. Has to implemented in derived class.
*/
virtual HyperGraph::HyperGraphElement* construct() = 0;
/**
* name of the class to be created. Has to implemented in derived class.
*/
virtual const std::string& name() const = 0;
virtual ~AbstractHyperGraphElementCreator() { }
};
/**
* \brief templatized creator class which creates graph elements
*/
template <typename T>
class HyperGraphElementCreator : public AbstractHyperGraphElementCreator
{
public:
HyperGraphElementCreator() : _name(typeid(T).name()) {}
#if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC
__attribute__((force_align_arg_pointer))
#endif
HyperGraph::HyperGraphElement* construct() { return new T;}
virtual const std::string& name() const { return _name;}
protected:
std::string _name;
};
} // end namespace
#endif

2
SLAM/Thirdparty/g2o/g2o/core/creators.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/creators.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/creators.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

73
SLAM/Thirdparty/g2o/g2o/core/eigen_types.h vendored
View File

@ -1,73 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_EIGEN_TYPES_H
#define G2O_EIGEN_TYPES_H
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace g2o {
typedef Eigen::Matrix<int,2,1,Eigen::ColMajor> Vector2I;
typedef Eigen::Matrix<int,3,1,Eigen::ColMajor> Vector3I;
typedef Eigen::Matrix<int,4,1,Eigen::ColMajor> Vector4I;
typedef Eigen::Matrix<int,Eigen::Dynamic,1,Eigen::ColMajor> VectorXI;
typedef Eigen::Matrix<float,2,1,Eigen::ColMajor> Vector2F;
typedef Eigen::Matrix<float,3,1,Eigen::ColMajor> Vector3F;
typedef Eigen::Matrix<float,4,1,Eigen::ColMajor> Vector4F;
typedef Eigen::Matrix<float,Eigen::Dynamic,1,Eigen::ColMajor> VectorXF;
typedef Eigen::Matrix<double,2,1,Eigen::ColMajor> Vector2D;
typedef Eigen::Matrix<double,3,1,Eigen::ColMajor> Vector3D;
typedef Eigen::Matrix<double,4,1,Eigen::ColMajor> Vector4D;
typedef Eigen::Matrix<double,Eigen::Dynamic,1,Eigen::ColMajor> VectorXD;
typedef Eigen::Matrix<int,2,2,Eigen::ColMajor> Matrix2I;
typedef Eigen::Matrix<int,3,3,Eigen::ColMajor> Matrix3I;
typedef Eigen::Matrix<int,4,4,Eigen::ColMajor> Matrix4I;
typedef Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXI;
typedef Eigen::Matrix<float,2,2,Eigen::ColMajor> Matrix2F;
typedef Eigen::Matrix<float,3,3,Eigen::ColMajor> Matrix3F;
typedef Eigen::Matrix<float,4,4,Eigen::ColMajor> Matrix4F;
typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXF;
typedef Eigen::Matrix<double,2,2,Eigen::ColMajor> Matrix2D;
typedef Eigen::Matrix<double,3,3,Eigen::ColMajor> Matrix3D;
typedef Eigen::Matrix<double,4,4,Eigen::ColMajor> Matrix4D;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXD;
typedef Eigen::Transform<double,2,Eigen::Isometry,Eigen::ColMajor> Isometry2D;
typedef Eigen::Transform<double,3,Eigen::Isometry,Eigen::ColMajor> Isometry3D;
typedef Eigen::Transform<double,2,Eigen::Affine,Eigen::ColMajor> Affine2D;
typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Affine3D;
} // end namespace g2o
#endif

2
SLAM/Thirdparty/g2o/g2o/core/eigen_types.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/eigen_types.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/eigen_types.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

267
SLAM/Thirdparty/g2o/g2o/core/estimate_propagator.cpp vendored
View File

@ -1,267 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "estimate_propagator.h"
#include <queue>
#include <vector>
#include <cassert>
#include <iostream>
#include <algorithm>
#include <fstream>
//#define DEBUG_ESTIMATE_PROPAGATOR
using namespace std;
namespace g2o {
# ifdef DEBUG_ESTIMATE_PROPAGATOR
struct FrontierLevelCmp {
bool operator()(EstimatePropagator::AdjacencyMapEntry* e1, EstimatePropagator::AdjacencyMapEntry* e2) const
{
return e1->frontierLevel() < e2->frontierLevel();
}
};
# endif
EstimatePropagator::AdjacencyMapEntry::AdjacencyMapEntry()
{
reset();
}
void EstimatePropagator::AdjacencyMapEntry::reset()
{
_child = 0;
_parent.clear();
_edge = 0;
_distance = numeric_limits<double>::max();
_frontierLevel = -1;
inQueue = false;
}
EstimatePropagator::EstimatePropagator(OptimizableGraph* g): _graph(g)
{
for (OptimizableGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it){
AdjacencyMapEntry entry;
entry._child = static_cast<OptimizableGraph::Vertex*>(it->second);
_adjacencyMap.insert(make_pair(entry.child(), entry));
}
}
void EstimatePropagator::reset()
{
for (OptimizableGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); ++it){
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
AdjacencyMap::iterator at = _adjacencyMap.find(v);
assert(at != _adjacencyMap.end());
at->second.reset();
}
_visited.clear();
}
void EstimatePropagator::propagate(OptimizableGraph::Vertex* v,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action,
double maxDistance,
double maxEdgeCost)
{
OptimizableGraph::VertexSet vset;
vset.insert(v);
propagate(vset, cost, action, maxDistance, maxEdgeCost);
}
void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action,
double maxDistance,
double maxEdgeCost)
{
reset();
PriorityQueue frontier;
for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
AdjacencyMap::iterator it = _adjacencyMap.find(v);
assert(it != _adjacencyMap.end());
it->second._distance = 0.;
it->second._parent.clear();
it->second._frontierLevel = 0;
frontier.push(&it->second);
}
while(! frontier.empty()){
AdjacencyMapEntry* entry = frontier.pop();
OptimizableGraph::Vertex* u = entry->child();
double uDistance = entry->distance();
//cerr << "uDistance " << uDistance << endl;
// initialize the vertex
if (entry->_frontierLevel > 0) {
action(entry->edge(), entry->parent(), u);
}
/* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
while (et != u->edges().end()){
OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
++et;
int maxFrontier = -1;
OptimizableGraph::VertexSet initializedVertices;
for (size_t i = 0; i < edge->vertices().size(); ++i) {
OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
AdjacencyMap::iterator ot = _adjacencyMap.find(z);
if (ot->second._distance != numeric_limits<double>::max()) {
initializedVertices.insert(z);
maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
}
}
assert(maxFrontier >= 0);
for (size_t i = 0; i < edge->vertices().size(); ++i) {
OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
if (z == u)
continue;
size_t wasInitialized = initializedVertices.erase(z);
double edgeDistance = cost(edge, initializedVertices, z);
if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
double zDistance = uDistance + edgeDistance;
//cerr << z->id() << " " << zDistance << endl;
AdjacencyMap::iterator ot = _adjacencyMap.find(z);
assert(ot!=_adjacencyMap.end());
if (zDistance < ot->second.distance() && zDistance < maxDistance){
//if (ot->second.inQueue)
//cerr << "Updating" << endl;
ot->second._distance = zDistance;
ot->second._parent = initializedVertices;
ot->second._edge = edge;
ot->second._frontierLevel = maxFrontier + 1;
frontier.push(&ot->second);
}
}
if (wasInitialized > 0)
initializedVertices.insert(z);
}
}
}
// writing debug information like cost for reaching each vertex and the parent used to initialize
#ifdef DEBUG_ESTIMATE_PROPAGATOR
cerr << "Writing cost.dat" << endl;
ofstream costStream("cost.dat");
for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
HyperGraph::Vertex* u = it->second.child();
costStream << "vertex " << u->id() << " cost " << it->second._distance << endl;
}
cerr << "Writing init.dat" << endl;
ofstream initStream("init.dat");
vector<AdjacencyMapEntry*> frontierLevels;
for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
if (it->second._frontierLevel > 0)
frontierLevels.push_back(&it->second);
}
sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
AdjacencyMapEntry* entry = *it;
OptimizableGraph::Vertex* to = entry->child();
initStream << "calling init level = " << entry->_frontierLevel << "\t (";
for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {
initStream << " " << (*pit)->id();
}
initStream << " ) -> " << to->id() << endl;
}
#endif
}
void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry)
{
assert(entry != NULL);
if (entry->inQueue) {
assert(entry->queueIt->second == entry);
erase(entry->queueIt);
}
entry->queueIt = insert(std::make_pair(entry->distance(), entry));
assert(entry->queueIt != end());
entry->inQueue = true;
}
EstimatePropagator::AdjacencyMapEntry* EstimatePropagator::PriorityQueue::pop()
{
assert(!empty());
iterator it = begin();
AdjacencyMapEntry* entry = it->second;
erase(it);
assert(entry != NULL);
entry->queueIt = end();
entry->inQueue = false;
return entry;
}
EstimatePropagatorCost::EstimatePropagatorCost (SparseOptimizer* graph) :
_graph(graph)
{
}
double EstimatePropagatorCost::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const
{
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
if (it == _graph->activeEdges().end()) // it has to be an active edge
return std::numeric_limits<double>::max();
return e->initialEstimatePossible(from, to);
}
EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(SparseOptimizer* graph) :
EstimatePropagatorCost(graph)
{
}
double EstimatePropagatorCostOdometry::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const
{
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
OptimizableGraph::Vertex* from = dynamic_cast<OptimizableGraph::Vertex*>(*from_.begin());
OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph
return std::numeric_limits<double>::max();
SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
if (it == _graph->activeEdges().end()) // it has to be an active edge
return std::numeric_limits<double>::max();
return e->initialEstimatePossible(from_, to);
}
} // end namespace

2
SLAM/Thirdparty/g2o/g2o/core/estimate_propagator.cpp:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/estimate_propagator.cpp:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/estimate_propagator.cpp:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

175
SLAM/Thirdparty/g2o/g2o/core/estimate_propagator.h vendored
View File

@ -1,175 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_ESTIMATE_PROPAGATOR_H
#define G2O_ESTIMATE_PROPAGATOR_H
#include "optimizable_graph.h"
#include "sparse_optimizer.h"
#include <map>
#include <set>
#include <limits>
#ifdef _MSC_VER
#include <unordered_map>
#else
#include <tr1/unordered_map>
#endif
namespace g2o {
/**
* \brief cost for traversing along active edges in the optimizer
*
* You may derive an own one, if necessary. The default is to return initialEstimatePossible(from, to) for the edge.
*/
class EstimatePropagatorCost {
public:
EstimatePropagatorCost (SparseOptimizer* graph);
virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const;
virtual const char* name() const { return "spanning tree";}
protected:
SparseOptimizer* _graph;
};
/**
* \brief cost for traversing only odometry edges.
*
* Initialize your graph along odometry edges. An odometry edge is assumed to connect vertices
* whose IDs only differs by one.
*/
class EstimatePropagatorCostOdometry : public EstimatePropagatorCost {
public:
EstimatePropagatorCostOdometry(SparseOptimizer* graph);
virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const;
virtual const char* name() const { return "odometry";}
};
/**
* \brief propagation of an initial guess
*/
class EstimatePropagator {
public:
/**
* \brief Applying the action for propagating.
*
* You may derive an own one, if necessary. The default is to call initialEstimate(from, to) for the edge.
*/
struct PropagateAction {
virtual void operator()(OptimizableGraph::Edge* e, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) const
{
if (! to->fixed())
e->initialEstimate(from, to);
}
};
typedef EstimatePropagatorCost PropagateCost;
class AdjacencyMapEntry;
/**
* \brief priority queue for AdjacencyMapEntry
*/
class PriorityQueue : public std::multimap<double, AdjacencyMapEntry*> {
public:
void push(AdjacencyMapEntry* entry);
AdjacencyMapEntry* pop();
};
/**
* \brief data structure for loopuk during Dijkstra
*/
class AdjacencyMapEntry {
public:
friend class EstimatePropagator;
friend class PriorityQueue;
AdjacencyMapEntry();
void reset();
OptimizableGraph::Vertex* child() const {return _child;}
const OptimizableGraph::VertexSet& parent() const {return _parent;}
OptimizableGraph::Edge* edge() const {return _edge;}
double distance() const {return _distance;}
int frontierLevel() const { return _frontierLevel;}
protected:
OptimizableGraph::Vertex* _child;
OptimizableGraph::VertexSet _parent;
OptimizableGraph::Edge* _edge;
double _distance;
int _frontierLevel;
private: // for PriorityQueue
bool inQueue;
PriorityQueue::iterator queueIt;
};
/**
* \brief hash function for a vertex
*/
class VertexIDHashFunction {
public:
size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();}
};
typedef std::tr1::unordered_map<OptimizableGraph::Vertex*, AdjacencyMapEntry, VertexIDHashFunction> AdjacencyMap;
public:
EstimatePropagator(OptimizableGraph* g);
OptimizableGraph::VertexSet& visited() {return _visited; }
AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
OptimizableGraph* graph() {return _graph;}
/**
* propagate an initial guess starting from v. The function computes a spanning tree
* whereas the cost for each edge is determined by calling cost() and the action applied to
* each vertex is action().
*/
void propagate(OptimizableGraph::Vertex* v,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action = PropagateAction(),
double maxDistance=std::numeric_limits<double>::max(),
double maxEdgeCost=std::numeric_limits<double>::max());
/**
* same as above but starting to propagate from a set of vertices instead of just a single one.
*/
void propagate(OptimizableGraph::VertexSet& vset,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action = PropagateAction(),
double maxDistance=std::numeric_limits<double>::max(),
double maxEdgeCost=std::numeric_limits<double>::max());
protected:
void reset();
AdjacencyMap _adjacencyMap;
OptimizableGraph::VertexSet _visited;
OptimizableGraph* _graph;
};
}
#endif

2
SLAM/Thirdparty/g2o/g2o/core/estimate_propagator.h:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/estimate_propagator.h:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

2
SLAM/Thirdparty/g2o/g2o/core/estimate_propagator.h:Zone.Identifier:Zone.Identifier:Zone.Identifier vendored
View File

@ -1,2 +0,0 @@
[ZoneTransfer]
ZoneId=3

217
SLAM/Thirdparty/g2o/g2o/core/factory.cpp vendored
View File

@ -1,217 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "factory.h"
#include "creators.h"
#include "parameter.h"
#include "cache.h"
#include "optimizable_graph.h"
#include "../stuff/color_macros.h"
#include <iostream>
#include <typeinfo>
#include <cassert>
using namespace std;
namespace g2o {
Factory* Factory::factoryInstance = 0;
Factory::Factory()
{
}
Factory::~Factory()
{
# ifdef G2O_DEBUG_FACTORY
cerr << "# Factory destroying " << (void*)this << endl;
# endif
for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {
delete it->second->creator;
}
_creator.clear();
_tagLookup.clear();
}
Factory* Factory::instance()
{
if (factoryInstance == 0) {
factoryInstance = new Factory;
# ifdef G2O_DEBUG_FACTORY
cerr << "# Factory allocated " << (void*)factoryInstance << endl;
# endif
}
return factoryInstance;
}
void Factory::registerType(const std::string& tag, AbstractHyperGraphElementCreator* c)
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end()) {
cerr << "FACTORY WARNING: Overwriting Vertex tag " << tag << endl;
assert(0);
}
TagLookup::const_iterator tagIt = _tagLookup.find(c->name());
if (tagIt != _tagLookup.end()) {
cerr << "FACTORY WARNING: Registering same class for two tags " << c->name() << endl;
assert(0);
}
CreatorInformation* ci = new CreatorInformation();
ci->creator = c;
#ifdef G2O_DEBUG_FACTORY
cerr << "# Factory " << (void*)this << " constructing type " << tag << " ";
#endif
// construct an element once to figure out its type
HyperGraph::HyperGraphElement* element = c->construct();
ci->elementTypeBit = element->elementType();
#ifdef G2O_DEBUG_FACTORY
cerr << "done." << endl;
cerr << "# Factory " << (void*)this << " registering " << tag;
cerr << " " << (void*) c << " ";
switch (element->elementType()) {
case HyperGraph::HGET_VERTEX:
cerr << " -> Vertex";
break;
case HyperGraph::HGET_EDGE:
cerr << " -> Edge";
break;
case HyperGraph::HGET_PARAMETER:
cerr << " -> Parameter";
break;
case HyperGraph::HGET_CACHE:
cerr << " -> Cache";
break;
case HyperGraph::HGET_DATA:
cerr << " -> Data";
break;
default:
assert(0 && "Unknown element type occured, fix elementTypes");
break;
}
cerr << endl;
#endif
_creator[tag] = ci;
_tagLookup[c->name()] = tag;
delete element;
}
void Factory::unregisterType(const std::string& tag)
{
// Look for the tag
CreatorMap::iterator tagPosition = _creator.find(tag);
if (tagPosition != _creator.end()) {
AbstractHyperGraphElementCreator* c = tagPosition->second->creator;
// If we found it, remove the creator from the tag lookup map
TagLookup::iterator classPosition = _tagLookup.find(c->name());
if (classPosition != _tagLookup.end())
{
_tagLookup.erase(classPosition);
}
_creator.erase(tagPosition);
}
}
HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag) const
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end()) {
//cerr << "tag " << tag << " -> " << (void*) foundIt->second->creator << " " << foundIt->second->creator->name() << endl;
return foundIt->second->creator->construct();
}
return 0;
}
const std::string& Factory::tag(const HyperGraph::HyperGraphElement* e) const
{
static std::string emptyStr("");
TagLookup::const_iterator foundIt = _tagLookup.find(typeid(*e).name());
if (foundIt != _tagLookup.end())
return foundIt->second;
return emptyStr;
}
void Factory::fillKnownTypes(std::vector<std::string>& types) const
{
types.clear();
for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
types.push_back(it->first);
}
bool Factory::knowsTag(const std::string& tag, int* elementType) const
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt == _creator.end()) {
if (elementType)
*elementType = -1;
return false;
}
if (elementType)
*elementType = foundIt->second->elementTypeBit;
return true;
}
void Factory::destroy()
{
delete factoryInstance;
factoryInstance = 0;
}
void Factory::printRegisteredTypes(std::ostream& os, bool comment) const
{
if (comment)
os << "# ";
os << "types:" << endl;
for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
if (comment)
os << "#";
cerr << "\t" << it->first << endl;
}
}
HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const
{
if (elemsToConstruct.none()) {
return construct(tag);
}
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end() && foundIt->second->elementTypeBit >= 0 && elemsToConstruct.test(foundIt->second->elementTypeBit)) {
return foundIt->second->creator->construct();
}
return 0;
}
} // end namespace

Some files were not shown because too many files have changed in this diff Show More