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

Added lagrange access function to the SQPOptimizer 

Added a test testSQP to try changing the initial Lagrange multiplier values to find a way around the stability problem, but there is no effect.
release/4.3a0
Alex Cunningham 2009-11-24 14:44:46 +00:00
parent 140e79572f
commit e3e79e3888
2 changed files with 74 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cpp/SQPOptimizer.h
View File

@ -74,6 +74,7 @@ public:
const FactorGraph* graph() const { return graph_; }
const Ordering* ordering() const { return ordering_; }
shared_config config() const { return config_; }
shared_vconfig configLagrange() const { return lagrange_config_; }
double error() const { return error_; }
/**

73
cpp/testSQP.cpp
View File

@ -790,6 +790,79 @@ TEST (SQP, stereo_sqp_noisy ) {
//CHECK(assert_equal(*truthConfig,*(optimizer.config())));
}
// TEST DISABLED: There is a stability problem that sends the solution
// further away with every iteration, and eventually will result in the linearized
// system being underconstrained
/**
* SQP version of the above stereo example,
* with noise in the initial estimate and manually specified
* lagrange multipliers
*/
TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
bool verbose = false;
// get a graph
VSLAMGraph graph = stereoExampleGraph();
// create initial data
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
Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away
Point3 landmark2(1.5, 5.0, 0.0);
// noisy config
boost::shared_ptr<VSLAMConfig> initConfig(new VSLAMConfig);
initConfig->addCameraPose(1, pose1);
initConfig->addCameraPose(2, pose2);
initConfig->addLandmarkPoint(1, landmark1);
initConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place
// create ordering with lagrange multiplier included
Ordering ord;
ord += "x1", "x2", "l1", "l2", "L_l1l2";
// create lagrange multipliers
SOptimizer::shared_vconfig initLagrangeConfig(new VectorConfig);
initLagrangeConfig->insert("L_l1l2", Vector_(3, 0.0, 0.0, 0.0));
// create optimizer
SOptimizer optimizer(graph, ord, initConfig, initLagrangeConfig);
// optimize
double start_error = optimizer.error();
int maxIt = 5;
// for (int i=0; i<maxIt; ++i) {
// if (verbose) {
// cout << "\n ************************** \n"
// << " Iteration: " << i << endl;
// optimizer.config()->print("Config Before Iteration");
// optimizer.configLagrange()->print("Lagrange Before Iteration");
// optimizer = optimizer.iterate(SOptimizer::FULL);
// }
// else
// optimizer = optimizer.iterate(SOptimizer::SILENT);
// }
if (verbose) cout << "Initial Error: " << start_error << "\n"
<< "Final Error: " << optimizer.error() << endl;
// get the truth config
boost::shared_ptr<VSLAMConfig> truthConfig = stereoExampleTruthConfig();
if (verbose) {
initConfig->print("Initial Config");
truthConfig->print("Truth Config");
optimizer.config()->print("After optimization");
}
// check if correct
//CHECK(assert_equal(*truthConfig,*(optimizer.config())));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */