12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added ground truth for VSLAM example in testSQP

release/4.3a0
Alex Cunningham 2009-11-23 16:45:53 +00:00
parent f272e2f6fe
commit 36877532ad
1 changed files with 77 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
cpp/testSQP.cpp
View File

@ -18,6 +18,10 @@
#include <Simulated2DMeasurement.h>
#include <simulated2D.h>
#include <Ordering.h>
#include <VSLAMConfig.h>
#include <VSLAMFactor.h>
#include <VSLAMGraph.h>
#include <SimpleCamera.h>
// templated implementations
#include <NonlinearFactorGraph-inl.h>
@ -253,7 +257,7 @@ typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_eq;
typedef boost::shared_ptr<VectorConfig> shared_cfg;
typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
/**
* Determining a ground truth nonlinear system
* Determining a ground truth linear system
* with two poses seeing one landmark, with each pose
* constrained to a particular value
*/
@ -346,7 +350,8 @@ Matrix grad_g(const VectorConfig& config, const std::string& key) {
* Version that actually uses nonlinear equality constraints
* to to perform optimization. Same as above, but no
* equality constraint on x2, and two landmarks that
* should be the same.
* should be the same. Note that this is a linear system,
* so it will converge in one step.
*/
TEST (SQP, two_pose ) {
bool verbose = false;
@ -445,6 +450,76 @@ TEST (SQP, two_pose ) {
CHECK(assert_equal(expected, state, 1e-5));
}
/* ********************************************************************* */
// VSLAM Examples
/* ********************************************************************* */
// make a realistic calibration matrix
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2 K(fov,w,h);
boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
// typedefs for visual SLAM example
typedef boost::shared_ptr<VSLAMFactor> shared_vf;
typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> VOptimizer;
/**
* Ground truth for a visual slam example with stereo vision
*/
TEST (SQP, stereo_truth ) {
bool verbose = false;
// create initial estimates
Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 0.0, 1.0,
0.0, 1.0, 0.0));
Pose3 pose1(faceDownY, Point3()); // origin, left camera
SimpleCamera camera1(K, pose1);
Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
SimpleCamera camera2(K, pose2);
Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
// create config
boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig);
truthConfig->addCameraPose(1, camera1.pose());
truthConfig->addCameraPose(2, camera2.pose());
truthConfig->addLandmarkPoint(1, landmark);
// create graph
VSLAMGraph graph;
// create equality constraints for poses
graph.addCameraConstraint(1, camera1.pose());
graph.addCameraConstraint(2, camera2.pose());
// create VSLAM factors
Point2 z1 = camera1.project(landmark);
if (verbose) z1.print("z1");
shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK));
graph.push_back(vf1);
Point2 z2 = camera2.project(landmark);
if (verbose) z2.print("z2");
shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK));
graph.push_back(vf2);
if (verbose) graph.print("Graph after construction");
// create ordering
Ordering ord;
ord += "x1", "x2", "l1";
// create optimizer
VOptimizer optimizer(graph, ord, truthConfig, 1e-5);
// optimize
VOptimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */