New example now uses EssentialTransferFactor
Frank Dellaert
parent
9335d2c0dd
commit
5b94956a59
|
|
@ -17,28 +17,30 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/EssentialMatrix.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/EdgeKey.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/sfm/TransferFactor.h>
|
||||
#include <gtsam/sfm/TransferFactor.h> // Contains EssentialTransferFactor
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "SFMdata.h"
|
||||
#include "gtsam/inference/Key.h"
|
||||
#include "SFMdata.h" // For createPoints() and posesOnCircle()
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace symbol_shorthand; // For K(symbol)
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Main function
|
||||
int main(int argc, char* argv[]) {
|
||||
// Define the camera calibration parameters
|
||||
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
|
||||
Cal3_S2 K_initial(50.0, 50.0, 0.0, 50.0, 50.0);
|
||||
|
||||
// Create the set of 8 ground-truth landmarks
|
||||
vector<Point3> points = createPoints();
|
||||
|
|
@ -46,28 +48,22 @@ int main(int argc, char* argv[]) {
|
|||
// Create the set of 4 ground-truth poses
|
||||
vector<Pose3> poses = posesOnCircle(4, 30);
|
||||
|
||||
// Calculate ground truth fundamental matrices, 1 and 2 poses apart
|
||||
auto F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K);
|
||||
auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K);
|
||||
// Calculate ground truth essential matrices, 1 and 2 poses apart
|
||||
auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1]));
|
||||
auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2]));
|
||||
|
||||
// Simulate measurements from each camera pose
|
||||
std::array<std::array<Point2, 8>, 4> p;
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
PinholeCamera<Cal3_S2> camera(poses[i], K);
|
||||
PinholeCamera<Cal3_S2> camera(poses[i], K_initial);
|
||||
for (size_t j = 0; j < 8; ++j) {
|
||||
p[i][j] = camera.project(points[j]);
|
||||
}
|
||||
}
|
||||
|
||||
// This section of the code is inspired by the work of Sweeney et al.
|
||||
// [link](sites.cs.ucsb.edu/~holl/pubs/Sweeney-2015-ICCV.pdf) on view-graph
|
||||
// calibration. The graph is made up of transfer factors that enforce the
|
||||
// epipolar constraint between corresponding points across three views, as
|
||||
// described in the paper. Rather than adding one ternary error term per point
|
||||
// in a triplet, we add three binary factors for sparsity during optimization.
|
||||
// In this version, we only include triplets between 3 successive cameras.
|
||||
// Create the factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
using Factor = TransferFactor<FundamentalMatrix>;
|
||||
using Factor = EssentialTransferFactor<Cal3_S2>;
|
||||
|
||||
for (size_t a = 0; a < 4; ++a) {
|
||||
size_t b = (a + 1) % 4; // Next camera
|
||||
|
|
@ -83,54 +79,60 @@ int main(int argc, char* argv[]) {
|
|||
tuples3.emplace_back(p[c][j], p[b][j], p[a][j]);
|
||||
}
|
||||
|
||||
// Add transfer factors between views a, b, and c. Note that the EdgeKeys
|
||||
// are crucial in performing the transfer in the right direction. We use
|
||||
// exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental
|
||||
// matrices we will optimize for.
|
||||
// Add transfer factors between views a, b, and c.
|
||||
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), tuples1);
|
||||
graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), tuples2);
|
||||
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), tuples3);
|
||||
}
|
||||
|
||||
// Formatter for printing keys
|
||||
auto formatter = [](Key key) {
|
||||
EdgeKey edge(key);
|
||||
return (std::string)edge;
|
||||
if (Symbol(key).chr() == 'k') {
|
||||
return (string)Symbol(key);
|
||||
} else {
|
||||
EdgeKey edge(key);
|
||||
return (std::string)edge;
|
||||
}
|
||||
};
|
||||
|
||||
graph.print("Factor Graph:\n", formatter);
|
||||
|
||||
// Create a delta vector to perturb the ground truth
|
||||
// We can't really go far before convergence becomes problematic :-(
|
||||
Vector7 delta;
|
||||
delta << 1, 2, 3, 4, 5, 6, 7;
|
||||
delta *= 1e-5;
|
||||
// Create a delta vector to perturb the ground truth (small perturbation)
|
||||
Vector5 delta;
|
||||
delta << 1, 1, 1, 1, 1;
|
||||
delta *= 1e-2;
|
||||
|
||||
// Create the data structure to hold the initial estimate to the solution
|
||||
// Create the initial estimate for essential matrices
|
||||
Values initialEstimate;
|
||||
for (size_t a = 0; a < 4; ++a) {
|
||||
size_t b = (a + 1) % 4; // Next camera
|
||||
size_t c = (a + 2) % 4; // Camera after next
|
||||
initialEstimate.insert(EdgeKey(a, b), F1.retract(delta));
|
||||
initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
|
||||
initialEstimate.insert(EdgeKey(a, b), E1.retract(delta));
|
||||
initialEstimate.insert(EdgeKey(a, c), E2.retract(delta));
|
||||
}
|
||||
initialEstimate.print("Initial Estimates:\n", formatter);
|
||||
graph.printErrors(initialEstimate, "errors: ", formatter);
|
||||
|
||||
/* Optimize the graph and print results */
|
||||
// Insert initial calibrations (using K symbol)
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
initialEstimate.insert(K(i), K_initial);
|
||||
}
|
||||
|
||||
initialEstimate.print("Initial Estimates:\n", formatter);
|
||||
graph.printErrors(initialEstimate, "Initial Errors:\n", formatter);
|
||||
|
||||
// Optimize the graph and print results
|
||||
LevenbergMarquardtParams params;
|
||||
params.setlambdaInitial(1000.0); // Initialize lambda to a high value
|
||||
params.setVerbosityLM("SUMMARY");
|
||||
Values result =
|
||||
LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize();
|
||||
|
||||
cout << "initial error = " << graph.error(initialEstimate) << endl;
|
||||
cout << "final error = " << graph.error(result) << endl;
|
||||
cout << "Initial error = " << graph.error(initialEstimate) << endl;
|
||||
cout << "Final error = " << graph.error(result) << endl;
|
||||
|
||||
result.print("Final results:\n", formatter);
|
||||
result.print("Final Results:\n", formatter);
|
||||
|
||||
cout << "Ground Truth F1:\n" << F1.matrix() << endl;
|
||||
cout << "Ground Truth F2:\n" << F2.matrix() << endl;
|
||||
cout << "Ground Truth E1:\n" << E1.matrix() << endl;
|
||||
cout << "Ground Truth E2:\n" << E2.matrix() << endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
}
|
||||
Loading…
Reference in New Issue