updated visual SLAM examples to follow correct coordinate system conventions. The ISAM example now also uses the same input files as the general SFM example.
Chris Beall
parent
6a0da1519a
commit
3c8a7a29f9
|
|
@ -1 +1 @@
|
|||
800 600 -1119.61507797 1119.61507797 399.5 299.5
|
||||
800 600 1119.61507797 1119.61507797 399.5 299.5
|
||||
|
|
|
|||
|
|
@ -1,12 +0,0 @@
|
|||
10
|
||||
ttpy10.feat
|
||||
ttpy20.feat
|
||||
ttpy30.feat
|
||||
ttpy40.feat
|
||||
ttpy50.feat
|
||||
ttpy60.feat
|
||||
ttpy70.feat
|
||||
ttpy80.feat
|
||||
ttpy90.feat
|
||||
ttpy100.feat
|
||||
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
10
|
||||
ttpy10.pose
|
||||
ttpy20.pose
|
||||
ttpy30.pose
|
||||
ttpy40.pose
|
||||
ttpy50.pose
|
||||
ttpy60.pose
|
||||
ttpy70.pose
|
||||
ttpy80.pose
|
||||
ttpy90.pose
|
||||
ttpy100.pose
|
||||
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
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
|
||||
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,4 +1,4 @@
|
|||
-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
|
||||
-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,4 +1,4 @@
|
|||
0.766044 0.642788 0 0
|
||||
-0.454519 0.541675 -0.707107 0
|
||||
-0.454519 0.541675 0.707107 0
|
||||
64.2788 -76.6044 100 1
|
||||
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,4 +1,4 @@
|
|||
0.5 0.866025 0 0
|
||||
-0.612372 0.353553 -0.707107 0
|
||||
-0.612372 0.353553 0.707107 0
|
||||
86.6025 -50 100 1
|
||||
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,4 +1,4 @@
|
|||
0.173648 0.984808 0 0
|
||||
-0.696364 0.122788 -0.707107 0
|
||||
-0.696364 0.122788 0.707107 0
|
||||
98.4808 -17.3648 100 1
|
||||
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,4 +1,4 @@
|
|||
-0.173648 0.984808 0 0
|
||||
-0.696364 -0.122788 -0.707107 0
|
||||
-0.696364 -0.122788 0.707107 0
|
||||
98.4808 17.3648 100 1
|
||||
-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,4 +1,4 @@
|
|||
-0.5 0.866025 0 0
|
||||
-0.612372 -0.353553 -0.707107 0
|
||||
-0.612372 -0.353553 0.707107 0
|
||||
86.6025 50 100 1
|
||||
-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,4 +1,4 @@
|
|||
-0.766044 0.642788 0 0
|
||||
-0.454519 -0.541675 -0.707107 0
|
||||
-0.454519 -0.541675 0.707107 0
|
||||
64.2788 76.6044 100 1
|
||||
-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,4 +1,4 @@
|
|||
-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
|
||||
-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,4 +1,4 @@
|
|||
-1 0 0 0
|
||||
0 -0.707107 -0.707107 0
|
||||
0 -0.707107 0.707107 0
|
||||
0 100 100 1
|
||||
-1 -0 0 0
|
||||
0 0.70711 -0.70711 100
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
*/
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
using namespace boost;
|
||||
|
||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
||||
|
|
@ -40,8 +41,8 @@ using namespace boost;
|
|||
/* ************************************************************************* */
|
||||
#define CALIB_FILE "calib.txt"
|
||||
#define LANDMARKS_FILE "landmarks.txt"
|
||||
#define POSES_FILE "posesISAM.txt"
|
||||
#define MEASUREMENTS_FILE "measurementsISAM.txt"
|
||||
#define POSES_FILE "poses.txt"
|
||||
#define MEASUREMENTS_FILE "measurements.txt"
|
||||
|
||||
// Base data folder
|
||||
string g_dataFolder;
|
||||
|
|
@ -49,8 +50,8 @@ string g_dataFolder;
|
|||
// Store groundtruth values, read from files
|
||||
shared_ptrK g_calib;
|
||||
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
|
||||
std::vector<Pose3> g_poses; // prior camera poses at each frame
|
||||
std::vector<std::vector<Feature2D> > g_measurements; // feature sets detected at each frame
|
||||
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));
|
||||
|
|
@ -69,7 +70,7 @@ void readAllDataISAM() {
|
|||
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
|
||||
|
||||
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
|
||||
g_poses = readPosesISAM(g_dataFolder, POSES_FILE);
|
||||
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);
|
||||
|
|
@ -80,7 +81,7 @@ void readAllDataISAM() {
|
|||
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
|
||||
*/
|
||||
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
|
||||
int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
|
||||
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<Graph> (new Graph());
|
||||
|
|
@ -121,16 +122,19 @@ int main(int argc, char* argv[]) {
|
|||
g_dataFolder = string(argv[1]) + "/";
|
||||
readAllDataISAM();
|
||||
|
||||
// Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
|
||||
// Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
|
||||
int relinearizeInterval = 3;
|
||||
NonlinearISAM<visualSLAM::Values> isam(relinearizeInterval);
|
||||
|
||||
// At each frame i with new camera pose and new set of measurements associated with it,
|
||||
// At each frame (poseId) with new camera pose and set of associated measurements,
|
||||
// create a graph of new factors and update ISAM
|
||||
for (size_t i = 0; i < g_measurements.size(); i++) {
|
||||
typedef std::map<int, std::vector<Feature2D> > FeatureMap;
|
||||
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
|
||||
const int poseId = features.first;
|
||||
shared_ptr<Graph> newFactors;
|
||||
shared_ptr<visualSLAM::Values> initialValues;
|
||||
createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib);
|
||||
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
|
||||
features.second, measurementSigma, g_calib);
|
||||
|
||||
isam.update(*newFactors, *initialValues);
|
||||
visualSLAM::Values currentEstimate = isam.estimate();
|
||||
|
|
|
|||
|
|
@ -46,10 +46,6 @@ std::map<int, Point3> readLandMarks(const std::string& landmarkFile) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Read pose from file, output by Panda3D.
|
||||
* Warning: row major!!!
|
||||
*/
|
||||
gtsam::Pose3 readPose(const char* Fn) {
|
||||
ifstream poseFile(Fn);
|
||||
if (!poseFile) {
|
||||
|
|
@ -62,18 +58,7 @@ gtsam::Pose3 readPose(const char* Fn) {
|
|||
poseFile >> v[i];
|
||||
poseFile.close();
|
||||
|
||||
// Because panda3d's camera is z-up, y-view,
|
||||
// we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule
|
||||
//... similar to OpenGL's camera
|
||||
for (int i = 0; i<3; i++) {
|
||||
float t = v[4+i];
|
||||
v[4+i] = v[8+i];
|
||||
v[8+i] = -t;
|
||||
}
|
||||
|
||||
::Vector vec = Vector_(16, v);
|
||||
|
||||
Matrix T = Matrix_(4,4, vec); // column order !!!
|
||||
Matrix T = Matrix_(4,4, v); // row order !!!
|
||||
|
||||
Pose3 pose(T);
|
||||
return pose;
|
||||
|
|
@ -166,28 +151,7 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn) {
|
||||
ifstream posesFile((baseFolder+posesFn).c_str());
|
||||
if (!posesFile) {
|
||||
cout << "Cannot read all pose ISAM file: " << posesFn << endl;
|
||||
exit(0);
|
||||
}
|
||||
int numPoses;
|
||||
posesFile >> numPoses;
|
||||
vector<Pose3> poses;
|
||||
for (int i = 0; i<numPoses; i++) {
|
||||
string poseFileName;
|
||||
posesFile >> poseFileName;
|
||||
|
||||
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
|
||||
poses.push_back(pose);
|
||||
}
|
||||
|
||||
return poses;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
|
||||
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;
|
||||
|
|
@ -196,13 +160,16 @@ std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string&
|
|||
int numPoses;
|
||||
measurementsFile >> numPoses;
|
||||
|
||||
std::vector<std::vector<Feature2D> > allFeatures;
|
||||
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(-1, (baseFolder+featureFileName).c_str()); // we don't care about pose id in ISAM
|
||||
allFeatures.push_back(features);
|
||||
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
|
||||
allFeatures[poseId] = features;
|
||||
}
|
||||
|
||||
return allFeatures;
|
||||
|
|
|
|||
|
|
@ -29,11 +29,9 @@ 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);
|
||||
std::vector<gtsam::Pose3> readPosesISAM(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::vector< std::vector<Feature2D> > readAllMeasurementsISAM(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