remove old visual SLAM example and its data
Duy-Nguyen Ta
parent
4fbdd979a1
commit
9c6eba4cf8
|
|
@ -1 +0,0 @@
|
||||||
800 600 1119.61507797 1119.61507797 399.5 299.5
|
|
||||||
|
|
@ -1,9 +0,0 @@
|
||||||
7
|
|
||||||
0 0 0 0
|
|
||||||
1 10 0 0
|
|
||||||
2 0 10 0
|
|
||||||
3 10 10 0
|
|
||||||
4 0 0 10
|
|
||||||
5 10 0 10
|
|
||||||
6 0 10 10
|
|
||||||
|
|
||||||
|
|
@ -1,12 +0,0 @@
|
||||||
10
|
|
||||||
1 ttpy10.feat
|
|
||||||
2 ttpy20.feat
|
|
||||||
3 ttpy30.feat
|
|
||||||
4 ttpy40.feat
|
|
||||||
5 ttpy50.feat
|
|
||||||
6 ttpy60.feat
|
|
||||||
7 ttpy70.feat
|
|
||||||
8 ttpy80.feat
|
|
||||||
9 ttpy90.feat
|
|
||||||
10 ttpy100.feat
|
|
||||||
|
|
||||||
|
|
@ -1,12 +0,0 @@
|
||||||
10
|
|
||||||
1 ttpy10.pose
|
|
||||||
2 ttpy20.pose
|
|
||||||
3 ttpy30.pose
|
|
||||||
4 ttpy40.pose
|
|
||||||
5 ttpy50.pose
|
|
||||||
6 ttpy60.pose
|
|
||||||
7 ttpy70.pose
|
|
||||||
8 ttpy80.pose
|
|
||||||
9 ttpy90.pose
|
|
||||||
10 ttpy100.pose
|
|
||||||
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
6 424 190
|
|
||||||
4 399.553 240.574
|
|
||||||
2 422.974 248.632
|
|
||||||
5 480.082 258.082
|
|
||||||
3 496.146 264.634
|
|
||||||
0 399.5 299.5
|
|
||||||
1 475.915 317.106
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
0.93969 0.24185 -0.24185 34.202
|
|
||||||
0.34202 -0.66446 0.66446 -93.969
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
5 321.545 223.591
|
|
||||||
4 399.553 240.574
|
|
||||||
1 325.395 282.512
|
|
||||||
6 372.434 296.453
|
|
||||||
0 399.5 299.5
|
|
||||||
3 296.479 336.708
|
|
||||||
2 373.796 355.347
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
-0.93969 -0.24185 0.24185 -34.202
|
|
||||||
-0.34202 0.66446 -0.66446 93.969
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
6 448.854 198.317
|
|
||||||
4 399.574 240.532
|
|
||||||
2 446.39 257.049
|
|
||||||
5 467.479 276.146
|
|
||||||
3 509.674 289.86
|
|
||||||
0 399.5 299.5
|
|
||||||
1 463.827 335.115
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
0.76604 0.45452 -0.45452 64.279
|
|
||||||
0.64279 -0.54168 0.54168 -76.604
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
6 468.239 211.826
|
|
||||||
4 399.319 240.617
|
|
||||||
2 464.725 270.725
|
|
||||||
5 445.7 290.2
|
|
||||||
0 399.5 299.5
|
|
||||||
3 510.239 317.674
|
|
||||||
1 443.471 349.078
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
0.5 0.61237 -0.61237 86.603
|
|
||||||
0.86603 -0.35355 0.35355 -50
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
6 480.111 229.111
|
|
||||||
4 399.556 240.556
|
|
||||||
2 476.163 287.93
|
|
||||||
5 417.685 298.056
|
|
||||||
0 399.605 299.535
|
|
||||||
3 497.1 344.3
|
|
||||||
1 416.846 357.038
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
0.17365 0.69636 -0.69636 98.481
|
|
||||||
0.98481 -0.12279 0.12279 -17.365
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
4 399.429 240.551
|
|
||||||
6 482.812 248.312
|
|
||||||
5 387.364 298.964
|
|
||||||
0 399.578 299.556
|
|
||||||
2 478.63 307.391
|
|
||||||
1 387.98 357.804
|
|
||||||
3 470.922 366.471
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
-0.17365 0.69636 -0.69636 98.481
|
|
||||||
0.98481 0.12279 -0.12279 17.365
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
4 399.478 240.565
|
|
||||||
6 474.941 267.451
|
|
||||||
5 358.473 292.364
|
|
||||||
0 399.5 299.5
|
|
||||||
2 471.043 326.447
|
|
||||||
1 360.68 351.22
|
|
||||||
3 434.473 380.709
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
-0.5 0.61237 -0.61237 86.603
|
|
||||||
0.86603 0.35355 -0.35355 50
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
4 399.553 240.617
|
|
||||||
5 335.294 279.275
|
|
||||||
6 457.717 283.698
|
|
||||||
0 399.5 299.5
|
|
||||||
1 338.531 338.265
|
|
||||||
2 454.54 342.64
|
|
||||||
3 393.218 384.691
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
-0.76604 0.45452 -0.45452 64.279
|
|
||||||
0.64279 0.54168 -0.54168 76.604
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
4 399.5 240.5
|
|
||||||
5 320.667 261.958
|
|
||||||
6 432.389 295.111
|
|
||||||
0 399.5 299.5
|
|
||||||
1 324.653 320.898
|
|
||||||
2 430.5 353.96
|
|
||||||
3 352.737 377.474
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
-0.93969 0.24185 -0.24185 34.202
|
|
||||||
0.34202 0.66446 -0.66446 93.969
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
7
|
|
||||||
4 399.5 240.5
|
|
||||||
5 316 242.5
|
|
||||||
0 397 299.5
|
|
||||||
6 402.765 299.588
|
|
||||||
1 320.5 301.5
|
|
||||||
2 402.5 358.5
|
|
||||||
3 319 360.5
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
-1 -0 0 0
|
|
||||||
0 0.70711 -0.70711 100
|
|
||||||
0 -0.70711 -0.70711 100
|
|
||||||
0 0 0 1
|
|
||||||
|
|
@ -1,14 +0,0 @@
|
||||||
# Build vSLAMexample
|
|
||||||
|
|
||||||
message(STATUS "Adding Example vISAMexample")
|
|
||||||
add_executable(vISAMexample vISAMexample.cpp vSLAMutils.cpp)
|
|
||||||
target_link_libraries(vISAMexample gtsam-static)
|
|
||||||
add_dependencies(examples vISAMexample)
|
|
||||||
|
|
||||||
message(STATUS "Adding Example vSFMexample")
|
|
||||||
add_executable(vSFMexample vSFMexample.cpp vSLAMutils.cpp)
|
|
||||||
target_link_libraries(vSFMexample gtsam-static)
|
|
||||||
add_dependencies(examples vSFMexample)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,41 +0,0 @@
|
||||||
/* ----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
||||||
* Atlanta, Georgia 30332-0415
|
|
||||||
* All Rights Reserved
|
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
||||||
|
|
||||||
* See LICENSE for the license information
|
|
||||||
|
|
||||||
* -------------------------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file Feature2D.h
|
|
||||||
* @brief
|
|
||||||
* @author Duy-Nguyen Ta
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "gtsam/geometry/Point2.h"
|
|
||||||
#include <iostream>
|
|
||||||
|
|
||||||
struct Feature2D
|
|
||||||
{
|
|
||||||
|
|
||||||
gtsam::Point2 m_p;
|
|
||||||
int m_idCamera; // id of the camera pose that makes this measurement
|
|
||||||
int m_idLandmark; // id of the 3D landmark that it is associated with
|
|
||||||
|
|
||||||
Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) :
|
|
||||||
m_p(p),
|
|
||||||
m_idCamera(idCamera),
|
|
||||||
m_idLandmark(idLandmark) {}
|
|
||||||
|
|
||||||
void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << std::endl;
|
|
||||||
std::cout << "Pose id: " << m_idCamera << " -- Landmark id: " << m_idLandmark << std::endl;
|
|
||||||
m_p.print("\tMeasurement: ");
|
|
||||||
}
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
@ -1,101 +0,0 @@
|
||||||
README - vSLAMexample
|
|
||||||
------------------------------------------------------
|
|
||||||
|
|
||||||
vSLAMexample includes 2 simple examples using GTSAM:
|
|
||||||
- vSFMexample using visualSLAM in for structure-from-motion (SFM), and
|
|
||||||
- vISAMexample using visualSLAM and ISAM for incremental SLAM updates
|
|
||||||
|
|
||||||
The two examples use the same visual SLAM graph structure which nodes are 6d camera poses (SE3) and 3d point landmarks. Measurement factors are 2D features detected on each image, connecting its camera-pose node and the corresponding landmark nodes. There are also prior factors on each pose nodes.
|
|
||||||
|
|
||||||
Synthesized data generation
|
|
||||||
---------------------------
|
|
||||||
The data are generated by using Panda3D graphics engine to render a sequence of virtual scene with 7 colorful small 3d patches viewing by camera moving around. The patches' coordinates are given in "landmarks.txt" file. Centroids of those colorful features in the rendered images are detected and stored in "ttpy*.feat" files.
|
|
||||||
|
|
||||||
Files "ttpy*.pose" contain the poses of the virtual camera that renders the scene. A *VERY IMPORTANT* note is that the values in these "ttpy*.pose" files follow Panda3D's convention for camera frame coordinate system: "z up, y view", where as in our code, we follow OpenGL's convention: "y up, -z view". Thus, we have to change it to match with our convention. Essentially, the z- and y- axes are swapped, then the z-axis is negated to stick to the right-hand rule. Please see the function "gtsam::Pose3 readPose(const char* Fn)" in "vSLAMutils.cpp" for more information.
|
|
||||||
|
|
||||||
File "calib.txt" contains intrinsic parameters of the virtual camera. The signs are correctly adjusted to match with our projection coordinate system's convention.
|
|
||||||
|
|
||||||
Files "measurements.txt" and "poses.txt" simulate typical input data for a structure-from-motion problem. Similarly, "measurementsISAM.txt" and "posesISAM.txt" simulate the data used in SLAM context with incremental-update using ISAM.
|
|
||||||
|
|
||||||
Note that for SFM, the whole graph is solved as a whole batch problem, so the camera_id's corresponding to the feature files and pose files need to be specified in "measurements.txt" and "poses.txt", but they are not necessarily in order.
|
|
||||||
|
|
||||||
On the other hand, for ISAM, we sequentially add the camera poses and features and update after every frame; so the pose files and features files in "measurementsISAM.txt" and "posesISAM.txt" need to be specified in order (time order), even though the camera id's are not necessary.
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Data file format
|
|
||||||
-----------------------------
|
|
||||||
|
|
||||||
"calib.txt":
|
|
||||||
------------
|
|
||||||
image_width image_height fx fy ox oy
|
|
||||||
|
|
||||||
|
|
||||||
"landmarks.txt"
|
|
||||||
------------
|
|
||||||
N #number of landmarks
|
|
||||||
landmark_id1 x1 y1 z1
|
|
||||||
landmark_id2 x2 y2 z2
|
|
||||||
...
|
|
||||||
landmark_idN xN yN zN
|
|
||||||
|
|
||||||
|
|
||||||
"ttpy*.feat"
|
|
||||||
------------
|
|
||||||
N #number of features
|
|
||||||
corresponding_landmark_id1 x1 y1
|
|
||||||
corresponding_landmark_id2 x2 y2
|
|
||||||
...
|
|
||||||
corresponding_landmark_idN xN yN
|
|
||||||
|
|
||||||
|
|
||||||
"ttpy*.pose"
|
|
||||||
------------
|
|
||||||
0.939693 0.34202 0 0
|
|
||||||
-0.241845 0.664463 -0.707107 0
|
|
||||||
-0.241845 0.664463 0.707107 0
|
|
||||||
34.202 -93.9693 100 1
|
|
||||||
|
|
||||||
The camera pose matrix in column order. Note that these values follows Panda3D's convention for camera coordinate frame. We have to change it to match with our convention used in the code, which follows OpenGL system. See previous section for more details.
|
|
||||||
|
|
||||||
|
|
||||||
Data For SFM:
|
|
||||||
|
|
||||||
"measurements.txt"
|
|
||||||
------------
|
|
||||||
N #number of cameras
|
|
||||||
camera_id1 featureFile1
|
|
||||||
camera_id2 featureFile2
|
|
||||||
...
|
|
||||||
camera_id3 featureFile3
|
|
||||||
|
|
||||||
"poses.txt"
|
|
||||||
------------
|
|
||||||
N #number of cameras
|
|
||||||
camera_id1 poseFile1
|
|
||||||
camera_id2 poseFile2
|
|
||||||
...
|
|
||||||
camera_id3 poseFile3
|
|
||||||
|
|
||||||
|
|
||||||
Data For ISAM:
|
|
||||||
|
|
||||||
"measurementsISAM.txt"
|
|
||||||
------------
|
|
||||||
N #number of cameras
|
|
||||||
featureFile1
|
|
||||||
featureFile2
|
|
||||||
...
|
|
||||||
featureFile3
|
|
||||||
|
|
||||||
"posesISAM.txt"
|
|
||||||
------------
|
|
||||||
N #number of cameras
|
|
||||||
poseFile1
|
|
||||||
poseFile2
|
|
||||||
...
|
|
||||||
poseFile3
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,146 +0,0 @@
|
||||||
/* ----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
||||||
* Atlanta, Georgia 30332-0415
|
|
||||||
* All Rights Reserved
|
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
||||||
|
|
||||||
* See LICENSE for the license information
|
|
||||||
|
|
||||||
* -------------------------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file vISAMexample.cpp
|
|
||||||
* @brief An ISAM example for synthesis sequence, single camera
|
|
||||||
* @author Duy-Nguyen Ta
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "vSLAMutils.h"
|
|
||||||
#include "Feature2D.h"
|
|
||||||
|
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
|
||||||
#include <gtsam/nonlinear/NonlinearISAM.h>
|
|
||||||
#include <gtsam/nonlinear/Symbol.h>
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
|
||||||
|
|
||||||
#include <boost/shared_ptr.hpp>
|
|
||||||
#include <boost/foreach.hpp>
|
|
||||||
using boost::shared_ptr;
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
using namespace gtsam;
|
|
||||||
|
|
||||||
// Convenience for named keys
|
|
||||||
using symbol_shorthand::X;
|
|
||||||
using symbol_shorthand::L;
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
#define CALIB_FILE "calib.txt"
|
|
||||||
#define LANDMARKS_FILE "landmarks.txt"
|
|
||||||
#define POSES_FILE "poses.txt"
|
|
||||||
#define MEASUREMENTS_FILE "measurements.txt"
|
|
||||||
|
|
||||||
// Base data folder
|
|
||||||
string g_dataFolder;
|
|
||||||
|
|
||||||
// Store groundtruth values, read from files
|
|
||||||
shared_ptrK g_calib;
|
|
||||||
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
|
|
||||||
map<int, Pose3> g_poses; // map: <camera_id, pose>
|
|
||||||
std::map<int, std::vector<Feature2D> > g_measurements; // feature sets detected at each frame
|
|
||||||
|
|
||||||
// Noise models
|
|
||||||
SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
|
|
||||||
SharedNoiseModel poseSigma(noiseModel::Unit::Create(1));
|
|
||||||
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Read all data: calibration file, landmarks, poses, and all features measurements
|
|
||||||
* Data is stored in global variables, which are used later to simulate incremental updates.
|
|
||||||
*/
|
|
||||||
void readAllDataISAM() {
|
|
||||||
g_calib = readCalibData(g_dataFolder + CALIB_FILE);
|
|
||||||
|
|
||||||
// Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
|
|
||||||
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
|
|
||||||
|
|
||||||
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
|
|
||||||
g_poses = readPoses(g_dataFolder, POSES_FILE);
|
|
||||||
|
|
||||||
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
|
|
||||||
g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
|
|
||||||
*/
|
|
||||||
void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
|
|
||||||
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
|
|
||||||
|
|
||||||
// Create a graph of newFactors with new measurements
|
|
||||||
newFactors = shared_ptr<visualSLAM::Graph> (new visualSLAM::Graph());
|
|
||||||
for (size_t i = 0; i < measurements.size(); i++) {
|
|
||||||
newFactors->addMeasurement(
|
|
||||||
measurements[i].m_p,
|
|
||||||
measurementSigma,
|
|
||||||
X(pose_id),
|
|
||||||
L(measurements[i].m_idLandmark),
|
|
||||||
calib);
|
|
||||||
}
|
|
||||||
|
|
||||||
// ... we need priors on the new pose and all new landmarks
|
|
||||||
newFactors->addPosePrior(pose_id, pose, poseSigma);
|
|
||||||
for (size_t i = 0; i < measurements.size(); i++) {
|
|
||||||
newFactors->addPointPrior(X(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Create initial values for all nodes in the newFactors
|
|
||||||
initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values());
|
|
||||||
initialValues->insert(X(pose_id), pose);
|
|
||||||
for (size_t i = 0; i < measurements.size(); i++) {
|
|
||||||
initialValues->insert(L(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
int main(int argc, char* argv[]) {
|
|
||||||
if (argc < 2) {
|
|
||||||
cout << "Usage: vISAMexample <DataFolder>" << endl << endl;
|
|
||||||
cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
|
|
||||||
"\tlandmarks, initial poses, and feature data." << endl;
|
|
||||||
cout << "\tSample folder is in $gtsam_source_folder$/examples/Data/" << endl << endl;
|
|
||||||
cout << "Example usage: vISAMexample '$gtsam_source_folder$/examples/Data/'" << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
g_dataFolder = string(argv[1]) + "/";
|
|
||||||
readAllDataISAM();
|
|
||||||
|
|
||||||
// Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
|
|
||||||
int relinearizeInterval = 3;
|
|
||||||
NonlinearISAM<> isam(relinearizeInterval);
|
|
||||||
|
|
||||||
// At each frame (poseId) with new camera pose and set of associated measurements,
|
|
||||||
// create a graph of new factors and update ISAM
|
|
||||||
typedef std::map<int, std::vector<Feature2D> > FeatureMap;
|
|
||||||
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
|
|
||||||
const int poseId = features.first;
|
|
||||||
shared_ptr<visualSLAM::Graph> newFactors;
|
|
||||||
shared_ptr<visualSLAM::Values> initialValues;
|
|
||||||
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
|
|
||||||
features.second, measurementSigma, g_calib);
|
|
||||||
|
|
||||||
isam.update(*newFactors, *initialValues);
|
|
||||||
Values currentEstimate = isam.estimate();
|
|
||||||
cout << "****************************************************" << endl;
|
|
||||||
currentEstimate.print("Current estimate: ");
|
|
||||||
}
|
|
||||||
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
/* ************************************************************************* */
|
|
||||||
|
|
||||||
|
|
@ -1,159 +0,0 @@
|
||||||
/* ----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
||||||
* Atlanta, Georgia 30332-0415
|
|
||||||
* All Rights Reserved
|
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
||||||
|
|
||||||
* See LICENSE for the license information
|
|
||||||
|
|
||||||
* -------------------------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file vSFMexample.cpp
|
|
||||||
* @brief A visual SLAM example for simulated sequence
|
|
||||||
* @author Duy-Nguyen Ta
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "vSLAMutils.h"
|
|
||||||
#include "Feature2D.h"
|
|
||||||
|
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
|
||||||
#include <gtsam/nonlinear/Symbol.h>
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
||||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
||||||
|
|
||||||
#include <boost/shared_ptr.hpp>
|
|
||||||
using boost::shared_ptr;
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
using namespace gtsam;
|
|
||||||
|
|
||||||
// Convenience for named keys
|
|
||||||
using symbol_shorthand::X;
|
|
||||||
using symbol_shorthand::L;
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
#define CALIB_FILE "calib.txt"
|
|
||||||
#define LANDMARKS_FILE "landmarks.txt"
|
|
||||||
#define POSES_FILE "poses.txt"
|
|
||||||
#define MEASUREMENTS_FILE "measurements.txt"
|
|
||||||
|
|
||||||
// Base data folder
|
|
||||||
string g_dataFolder;
|
|
||||||
|
|
||||||
// Store groundtruth values, read from files
|
|
||||||
shared_ptrK g_calib;
|
|
||||||
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
|
|
||||||
map<int, Pose3> g_poses; // map: <camera_id, pose>
|
|
||||||
std::vector<Feature2D> g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position}
|
|
||||||
|
|
||||||
// Noise models
|
|
||||||
SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Read all data: calibration file, landmarks, poses, and all features measurements
|
|
||||||
* Data is stored in global variables.
|
|
||||||
*/
|
|
||||||
void readAllData() {
|
|
||||||
|
|
||||||
g_calib = readCalibData(g_dataFolder + "/" + CALIB_FILE);
|
|
||||||
|
|
||||||
// Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
|
|
||||||
g_landmarks = readLandMarks(g_dataFolder + "/" + LANDMARKS_FILE);
|
|
||||||
|
|
||||||
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
|
|
||||||
g_poses = readPoses(g_dataFolder, POSES_FILE);
|
|
||||||
|
|
||||||
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
|
|
||||||
g_measurements = readAllMeasurements(g_dataFolder, MEASUREMENTS_FILE);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Setup vSLAM graph
|
|
||||||
* by adding and linking 2D features (measurements) detected in each captured image
|
|
||||||
* with their corresponding landmarks.
|
|
||||||
*/
|
|
||||||
visualSLAM::Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
|
|
||||||
|
|
||||||
visualSLAM::Graph g;
|
|
||||||
|
|
||||||
cout << "Built graph: " << endl;
|
|
||||||
for (size_t i = 0; i < measurements.size(); i++) {
|
|
||||||
measurements[i].print();
|
|
||||||
|
|
||||||
g.addMeasurement(
|
|
||||||
measurements[i].m_p,
|
|
||||||
measurementSigma,
|
|
||||||
X(measurements[i].m_idCamera),
|
|
||||||
L(measurements[i].m_idLandmark),
|
|
||||||
calib);
|
|
||||||
}
|
|
||||||
|
|
||||||
return g;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
|
|
||||||
* The returned Values structure contains all initial values for all nodes.
|
|
||||||
*/
|
|
||||||
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
|
|
||||||
|
|
||||||
visualSLAM::Values initValues;
|
|
||||||
|
|
||||||
// Initialize landmarks 3D positions.
|
|
||||||
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
|
|
||||||
initValues.insert(L(lmit->first), lmit->second);
|
|
||||||
|
|
||||||
// Initialize camera poses.
|
|
||||||
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
|
|
||||||
initValues.insert(X(poseit->first), poseit->second);
|
|
||||||
|
|
||||||
return initValues;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
int main(int argc, char* argv[]) {
|
|
||||||
if (argc < 2) {
|
|
||||||
cout << "Usage: vSFMexample <DataFolder>" << endl << endl;
|
|
||||||
cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
|
|
||||||
"\tlandmarks, initial poses, and feature data." << endl;
|
|
||||||
cout << "\tSample folder is in $gtsam_source_folder$/examples/Data" << endl << endl;
|
|
||||||
cout << "Example usage: vSFMexample '$gtsam_source_folder$/examples/Data'" << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
g_dataFolder = string(argv[1]) + "/";
|
|
||||||
readAllData();
|
|
||||||
|
|
||||||
// Create a graph using the 2D measurements (features) and the calibration data
|
|
||||||
visualSLAM::Graph graph(setupGraph(g_measurements, measurementSigma, g_calib));
|
|
||||||
|
|
||||||
// Create an initial Values structure using groundtruth values as the initial estimates
|
|
||||||
Values initialEstimates(initialize(g_landmarks, g_poses));
|
|
||||||
cout << "*******************************************************" << endl;
|
|
||||||
initialEstimates.print("INITIAL ESTIMATES: ");
|
|
||||||
|
|
||||||
// Add prior factor for all poses in the graph
|
|
||||||
map<int, Pose3>::iterator poseit = g_poses.begin();
|
|
||||||
for (; poseit != g_poses.end(); poseit++)
|
|
||||||
graph.addPosePrior(X(poseit->first), poseit->second, noiseModel::Unit::Create(1));
|
|
||||||
|
|
||||||
// Optimize the graph
|
|
||||||
cout << "*******************************************************" << endl;
|
|
||||||
LevenbergMarquardtParams params;
|
|
||||||
params.verbosityLM = LevenbergMarquardtParams::DAMPED;
|
|
||||||
visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimize();
|
|
||||||
|
|
||||||
// Print final results
|
|
||||||
cout << "*******************************************************" << endl;
|
|
||||||
result.print("FINAL RESULTS: ");
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
/* ************************************************************************* */
|
|
||||||
|
|
||||||
|
|
@ -1,176 +0,0 @@
|
||||||
/* ----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
||||||
* Atlanta, Georgia 30332-0415
|
|
||||||
* All Rights Reserved
|
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
||||||
|
|
||||||
* See LICENSE for the license information
|
|
||||||
|
|
||||||
* -------------------------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file VSLAMutils.cpp
|
|
||||||
* @brief
|
|
||||||
* @author Duy-Nguyen Ta
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "vSLAMutils.h"
|
|
||||||
#include <fstream>
|
|
||||||
#include <cstdio>
|
|
||||||
|
|
||||||
using namespace gtsam;
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::map<int, Point3> readLandMarks(const std::string& landmarkFile) {
|
|
||||||
ifstream file(landmarkFile.c_str());
|
|
||||||
if (!file) {
|
|
||||||
cout << "Cannot read landmark file: " << landmarkFile << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
int num;
|
|
||||||
file >> num;
|
|
||||||
std::map<int, Point3> landmarks;
|
|
||||||
landmarks.clear();
|
|
||||||
for (int i = 0; i<num; i++) {
|
|
||||||
int color_id;
|
|
||||||
float x, y, z;
|
|
||||||
file >> color_id >> x >> y >> z;
|
|
||||||
landmarks[color_id] = Point3(x, y, z);
|
|
||||||
}
|
|
||||||
|
|
||||||
file.close();
|
|
||||||
return landmarks;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
gtsam::Pose3 readPose(const char* Fn) {
|
|
||||||
ifstream poseFile(Fn);
|
|
||||||
if (!poseFile) {
|
|
||||||
cout << "Cannot read pose file: " << Fn << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
double v[16];
|
|
||||||
for (int i = 0; i<16; i++)
|
|
||||||
poseFile >> v[i];
|
|
||||||
poseFile.close();
|
|
||||||
|
|
||||||
Matrix T = Matrix_(4,4, v); // row order !!!
|
|
||||||
|
|
||||||
Pose3 pose(T);
|
|
||||||
return pose;
|
|
||||||
}
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn) {
|
|
||||||
ifstream posesFile((baseFolder+posesFn).c_str());
|
|
||||||
if (!posesFile) {
|
|
||||||
cout << "Cannot read all pose file: " << posesFn << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
int numPoses;
|
|
||||||
posesFile >> numPoses;
|
|
||||||
map<int, Pose3> poses;
|
|
||||||
for (int i = 0; i<numPoses; i++) {
|
|
||||||
int poseId;
|
|
||||||
posesFile >> poseId;
|
|
||||||
|
|
||||||
string poseFileName;
|
|
||||||
posesFile >> poseFileName;
|
|
||||||
|
|
||||||
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
|
|
||||||
poses[poseId] = pose;
|
|
||||||
}
|
|
||||||
|
|
||||||
return poses;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
gtsam::shared_ptrK readCalibData(const std::string& calibFn) {
|
|
||||||
ifstream calibFile(calibFn.c_str());
|
|
||||||
if (!calibFile) {
|
|
||||||
cout << "Cannot read calib file: " << calibFn << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
int imX, imY;
|
|
||||||
float fx, fy, ox, oy;
|
|
||||||
calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
|
|
||||||
calibFile.close();
|
|
||||||
|
|
||||||
return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::vector<Feature2D> readFeatures(int pose_id, const char* filename) {
|
|
||||||
ifstream file(filename);
|
|
||||||
if (!file) {
|
|
||||||
cout << "Cannot read feature file: " << filename<< endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
int numFeatures;
|
|
||||||
file >> numFeatures ;
|
|
||||||
|
|
||||||
std::vector<Feature2D> vFeatures_;
|
|
||||||
for (int i = 0; i < numFeatures; i++) {
|
|
||||||
int landmark_id; double x, y;
|
|
||||||
file >> landmark_id >> x >> y;
|
|
||||||
vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
|
|
||||||
}
|
|
||||||
|
|
||||||
file.close();
|
|
||||||
return vFeatures_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn) {
|
|
||||||
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
|
|
||||||
if (!measurementsFile) {
|
|
||||||
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
int numPoses;
|
|
||||||
measurementsFile >> numPoses;
|
|
||||||
|
|
||||||
vector<Feature2D> allFeatures;
|
|
||||||
allFeatures.clear();
|
|
||||||
|
|
||||||
for (int i = 0; i<numPoses; i++) {
|
|
||||||
int poseId;
|
|
||||||
measurementsFile >> poseId;
|
|
||||||
|
|
||||||
string featureFileName;
|
|
||||||
measurementsFile >> featureFileName;
|
|
||||||
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
|
|
||||||
allFeatures.insert( allFeatures.end(), features.begin(), features.end() );
|
|
||||||
}
|
|
||||||
|
|
||||||
return allFeatures;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::map<int, std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
|
|
||||||
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
|
|
||||||
if (!measurementsFile) {
|
|
||||||
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
int numPoses;
|
|
||||||
measurementsFile >> numPoses;
|
|
||||||
|
|
||||||
std::map<int, std::vector<Feature2D> > allFeatures;
|
|
||||||
|
|
||||||
for (int i = 0; i<numPoses; i++) {
|
|
||||||
int poseId;
|
|
||||||
measurementsFile >> poseId;
|
|
||||||
|
|
||||||
string featureFileName;
|
|
||||||
measurementsFile >> featureFileName;
|
|
||||||
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
|
|
||||||
allFeatures[poseId] = features;
|
|
||||||
}
|
|
||||||
|
|
||||||
return allFeatures;
|
|
||||||
}
|
|
||||||
|
|
@ -1,36 +0,0 @@
|
||||||
/* ----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
||||||
* Atlanta, Georgia 30332-0415
|
|
||||||
* All Rights Reserved
|
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
||||||
|
|
||||||
* See LICENSE for the license information
|
|
||||||
|
|
||||||
* -------------------------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file Feature2D.cpp
|
|
||||||
* @brief
|
|
||||||
* @author Duy-Nguyen Ta
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "Feature2D.h"
|
|
||||||
#include "gtsam/geometry/Pose3.h"
|
|
||||||
#include "gtsam/geometry/Point3.h"
|
|
||||||
#include "gtsam/geometry/Cal3_S2.h"
|
|
||||||
#include <vector>
|
|
||||||
#include <map>
|
|
||||||
|
|
||||||
std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile);
|
|
||||||
|
|
||||||
gtsam::Pose3 readPose(const char* poseFn);
|
|
||||||
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFN);
|
|
||||||
|
|
||||||
gtsam::shared_ptrK readCalibData(const std::string& calibFn);
|
|
||||||
|
|
||||||
std::vector<Feature2D> readFeatureFile(const char* filename);
|
|
||||||
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
|
|
||||||
std::map<int, std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);
|
|
||||||
Loading…
Reference in New Issue