Fixing compilation errors
Richard Roberts
parent
ff7a78b854
commit
3a1175323c
|
|
@ -146,11 +146,11 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
|
|||
vector<GeneralCamera> X = genCameraDefaultCalibration();
|
||||
|
||||
// add measurement with noise
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -158,23 +158,22 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
|
|||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
Point3 pt(L[i].x()+noise*getGaussian(),
|
||||
L[i].y()+noise*getGaussian(),
|
||||
L[i].z()+noise*getGaussian());
|
||||
values->insert(Symbol('l',i), pt) ;
|
||||
values.insert(Symbol('l',i), pt) ;
|
||||
}
|
||||
|
||||
graph->addCameraConstraint(0, X[0]);
|
||||
graph.addCameraConstraint(0, X[0]);
|
||||
|
||||
// Create an ordering of the variables
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
@ -183,11 +182,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
|
|||
vector<Point3> L = genPoint3();
|
||||
vector<GeneralCamera> X = genCameraVariableCalibration();
|
||||
// add measurement with noise
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -195,9 +194,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
|
|||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
|
||||
// add noise only to the first landmark
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
|
|
@ -205,19 +204,18 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
|
|||
Point3 pt(L[i].x()+noise*getGaussian(),
|
||||
L[i].y()+noise*getGaussian(),
|
||||
L[i].z()+noise*getGaussian());
|
||||
values->insert(Symbol('l',i), pt) ;
|
||||
values.insert(Symbol('l',i), pt) ;
|
||||
}
|
||||
else {
|
||||
values->insert(Symbol('l',i), L[i]) ;
|
||||
values.insert(Symbol('l',i), L[i]) ;
|
||||
}
|
||||
}
|
||||
|
||||
graph->addCameraConstraint(0, X[0]);
|
||||
graph.addCameraConstraint(0, X[0]);
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
@ -229,36 +227,35 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
|
|||
|
||||
// add measurement with noise
|
||||
const double noise = baseline*0.1;
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = L.size()*X.size();
|
||||
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
Point3 pt(L[i].x()+noise*getGaussian(),
|
||||
L[i].y()+noise*getGaussian(),
|
||||
L[i].z()+noise*getGaussian());
|
||||
//Point3 pt(L[i].x(), L[i].y(), L[i].z());
|
||||
values->insert(Symbol('l',i), pt) ;
|
||||
values.insert(Symbol('l',i), pt) ;
|
||||
}
|
||||
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
graph->addCameraConstraint(i, X[i]);
|
||||
graph.addCameraConstraint(i, X[i]);
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
@ -269,17 +266,17 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
|
|||
vector<GeneralCamera> X = genCameraVariableCalibration();
|
||||
|
||||
// add measurement with noise
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = L.size()*X.size();
|
||||
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i ) {
|
||||
const double
|
||||
rot_noise = 1e-5,
|
||||
|
|
@ -287,7 +284,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
|
|||
focal_noise = 1,
|
||||
skew_noise = 1e-5;
|
||||
if ( i == 0 ) {
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
}
|
||||
else {
|
||||
|
||||
|
|
@ -298,24 +295,23 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
|
|||
skew_noise, // s
|
||||
trans_noise, trans_noise // ux, uy
|
||||
) ;
|
||||
values->insert(Symbol('x',i), X[i].retract(delta)) ;
|
||||
values.insert(Symbol('x',i), X[i].retract(delta)) ;
|
||||
}
|
||||
}
|
||||
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
values->insert(Symbol('l',i), L[i]) ;
|
||||
values.insert(Symbol('l',i), L[i]) ;
|
||||
}
|
||||
|
||||
// fix X0 and all landmarks, allow only the X[1] to move
|
||||
graph->addCameraConstraint(0, X[0]);
|
||||
graph.addCameraConstraint(0, X[0]);
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i )
|
||||
graph->addPoint3Constraint(i, L[i]);
|
||||
graph.addPoint3Constraint(i, L[i]);
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
@ -325,11 +321,11 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
|
|||
vector<GeneralCamera> X = genCameraVariableCalibration();
|
||||
|
||||
// add measurement with noise
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -337,24 +333,23 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
|
|||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
|
||||
// add noise only to the first landmark
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
Point3 pt(L[i].x()+noise*getGaussian(),
|
||||
L[i].y()+noise*getGaussian(),
|
||||
L[i].z()+noise*getGaussian());
|
||||
values->insert(Symbol('l',i), pt) ;
|
||||
values.insert(Symbol('l',i), pt) ;
|
||||
}
|
||||
|
||||
graph->addCameraConstraint(0, X[0]);
|
||||
graph.addCameraConstraint(0, X[0]);
|
||||
const double reproj_error = 1e-5 ;
|
||||
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -148,11 +148,11 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
|
|||
vector<GeneralCamera> X = genCameraDefaultCalibration();
|
||||
|
||||
// add measurement with noise
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -160,23 +160,22 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
|
|||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
Point3 pt(L[i].x()+noise*getGaussian(),
|
||||
L[i].y()+noise*getGaussian(),
|
||||
L[i].z()+noise*getGaussian());
|
||||
values->insert(Symbol('l',i), pt) ;
|
||||
values.insert(Symbol('l',i), pt) ;
|
||||
}
|
||||
|
||||
graph->addCameraConstraint(0, X[0]);
|
||||
graph.addCameraConstraint(0, X[0]);
|
||||
|
||||
// Create an ordering of the variables
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
@ -185,11 +184,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
|
|||
vector<Point3> L = genPoint3();
|
||||
vector<GeneralCamera> X = genCameraVariableCalibration();
|
||||
// add measurement with noise
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -197,9 +196,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
|
|||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
|
||||
// add noise only to the first landmark
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
|
|
@ -207,19 +206,18 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
|
|||
Point3 pt(L[i].x()+noise*getGaussian(),
|
||||
L[i].y()+noise*getGaussian(),
|
||||
L[i].z()+noise*getGaussian());
|
||||
values->insert(Symbol('l',i), pt) ;
|
||||
values.insert(Symbol('l',i), pt) ;
|
||||
}
|
||||
else {
|
||||
values->insert(Symbol('l',i), L[i]) ;
|
||||
values.insert(Symbol('l',i), L[i]) ;
|
||||
}
|
||||
}
|
||||
|
||||
graph->addCameraConstraint(0, X[0]);
|
||||
graph.addCameraConstraint(0, X[0]);
|
||||
const double reproj_error = 1e-5;
|
||||
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
@ -231,36 +229,35 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
|
|||
|
||||
// add measurement with noise
|
||||
const double noise = baseline*0.1;
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = L.size()*X.size();
|
||||
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
Point3 pt(L[i].x()+noise*getGaussian(),
|
||||
L[i].y()+noise*getGaussian(),
|
||||
L[i].z()+noise*getGaussian());
|
||||
//Point3 pt(L[i].x(), L[i].y(), L[i].z());
|
||||
values->insert(Symbol('l',i), pt) ;
|
||||
values.insert(Symbol('l',i), pt) ;
|
||||
}
|
||||
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
graph->addCameraConstraint(i, X[i]);
|
||||
graph.addCameraConstraint(i, X[i]);
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
@ -271,23 +268,23 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
|
|||
vector<GeneralCamera> X = genCameraVariableCalibration();
|
||||
|
||||
// add measurement with noise
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
const size_t nMeasurements = L.size()*X.size();
|
||||
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i ) {
|
||||
const double
|
||||
rot_noise = 1e-5, trans_noise = 1e-3,
|
||||
focal_noise = 1, distort_noise = 1e-3;
|
||||
if ( i == 0 ) {
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
}
|
||||
else {
|
||||
|
||||
|
|
@ -296,24 +293,23 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
|
|||
trans_noise, trans_noise, trans_noise, // translation
|
||||
focal_noise, distort_noise, distort_noise // f, k1, k2
|
||||
) ;
|
||||
values->insert(Symbol('x',i), X[i].retract(delta)) ;
|
||||
values.insert(Symbol('x',i), X[i].retract(delta)) ;
|
||||
}
|
||||
}
|
||||
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
values->insert(Symbol('l',i), L[i]) ;
|
||||
values.insert(Symbol('l',i), L[i]) ;
|
||||
}
|
||||
|
||||
// fix X0 and all landmarks, allow only the X[1] to move
|
||||
graph->addCameraConstraint(0, X[0]);
|
||||
graph.addCameraConstraint(0, X[0]);
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i )
|
||||
graph->addPoint3Constraint(i, L[i]);
|
||||
graph.addPoint3Constraint(i, L[i]);
|
||||
|
||||
const double reproj_error = 1e-5 ;
|
||||
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
@ -323,11 +319,11 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
|
|||
vector<GeneralCamera> X = genCameraVariableCalibration();
|
||||
|
||||
// add measurement with noise
|
||||
shared_ptr<Graph> graph(new Graph());
|
||||
Graph graph;
|
||||
for ( size_t j = 0 ; j < X.size() ; ++j) {
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i) {
|
||||
Point2 pt = X[j].project(L[i]) ;
|
||||
graph->addMeasurement(j, i, pt, sigma1);
|
||||
graph.addMeasurement(j, i, pt, sigma1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -335,24 +331,23 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
|
|||
|
||||
// add initial
|
||||
const double noise = baseline*0.1;
|
||||
boost::shared_ptr<Values> values(new Values);
|
||||
Values values;
|
||||
for ( size_t i = 0 ; i < X.size() ; ++i )
|
||||
values->insert(Symbol('x',i), X[i]) ;
|
||||
values.insert(Symbol('x',i), X[i]) ;
|
||||
|
||||
// add noise only to the first landmark
|
||||
for ( size_t i = 0 ; i < L.size() ; ++i ) {
|
||||
Point3 pt(L[i].x()+noise*getGaussian(),
|
||||
L[i].y()+noise*getGaussian(),
|
||||
L[i].z()+noise*getGaussian());
|
||||
values->insert(Symbol('l',i), pt) ;
|
||||
values.insert(Symbol('l',i), pt) ;
|
||||
}
|
||||
|
||||
graph->addCameraConstraint(0, X[0]);
|
||||
graph.addCameraConstraint(0, X[0]);
|
||||
const double reproj_error = 1e-5 ;
|
||||
|
||||
shared_ptr<Ordering> ordering = getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer =
|
||||
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
|
||||
Ordering ordering = *getOrdering(X,L);
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
|
||||
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -53,33 +53,32 @@ TEST(Pose3Graph, optimizeCircle) {
|
|||
Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(PoseKey(1));
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
shared_ptr<Pose3Graph> fg(new Pose3Graph);
|
||||
fg->addHardConstraint(0, gT0);
|
||||
Pose3Graph fg;
|
||||
fg.addHardConstraint(0, gT0);
|
||||
Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1
|
||||
double theta = M_PI/3.0;
|
||||
CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1));
|
||||
fg->addConstraint(0,1, _0T1, covariance);
|
||||
fg->addConstraint(1,2, _0T1, covariance);
|
||||
fg->addConstraint(2,3, _0T1, covariance);
|
||||
fg->addConstraint(3,4, _0T1, covariance);
|
||||
fg->addConstraint(4,5, _0T1, covariance);
|
||||
fg->addConstraint(5,0, _0T1, covariance);
|
||||
fg.addConstraint(0,1, _0T1, covariance);
|
||||
fg.addConstraint(1,2, _0T1, covariance);
|
||||
fg.addConstraint(2,3, _0T1, covariance);
|
||||
fg.addConstraint(3,4, _0T1, covariance);
|
||||
fg.addConstraint(4,5, _0T1, covariance);
|
||||
fg.addConstraint(5,0, _0T1, covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Values> initial(new Values());
|
||||
initial->insert(PoseKey(0), gT0);
|
||||
initial->insert(PoseKey(1), hexagon.at<Pose3>(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
initial->insert(PoseKey(2), hexagon.at<Pose3>(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
|
||||
initial->insert(PoseKey(3), hexagon.at<Pose3>(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
initial->insert(PoseKey(4), hexagon.at<Pose3>(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
|
||||
initial->insert(PoseKey(5), hexagon.at<Pose3>(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
Values initial;
|
||||
initial.insert(PoseKey(0), gT0);
|
||||
initial.insert(PoseKey(1), hexagon.at<Pose3>(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
initial.insert(PoseKey(2), hexagon.at<Pose3>(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
|
||||
initial.insert(PoseKey(3), hexagon.at<Pose3>(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
initial.insert(PoseKey(4), hexagon.at<Pose3>(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
|
||||
initial.insert(PoseKey(5), hexagon.at<Pose3>(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
|
||||
// Choose an ordering and optimize
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += kx0,kx1,kx2,kx3,kx4,kx5;
|
||||
Ordering ordering;
|
||||
ordering += kx0,kx1,kx2,kx3,kx4,kx5;
|
||||
|
||||
Values actual = *LevenbergMarquardtOptimizer(
|
||||
fg, initial, LevenbergMarquardtOptimizer::SharedLMParams(), ordering).optimize()->values();
|
||||
Values actual = *LevenbergMarquardtOptimizer(fg, initial, ordering).optimize()->values();
|
||||
|
||||
// Check with ground truth
|
||||
CHECK(assert_equal(hexagon, actual,1e-4));
|
||||
|
|
|
|||
|
|
@ -59,13 +59,13 @@ TEST( StereoFactor, singlePoint)
|
|||
graph.add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K));
|
||||
|
||||
// Create a configuration corresponding to the ground truth
|
||||
Values values(new Values());
|
||||
Values values;
|
||||
values.insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
|
||||
|
||||
Point3 l1(0, 0, 0);
|
||||
values.insert(PointKey(1), l1); // add point at origin;
|
||||
|
||||
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, values)));
|
||||
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, values));
|
||||
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
|
||||
|
|
|
|||
|
|
@ -81,28 +81,28 @@ visualSLAM::Graph testGraph() {
|
|||
TEST( Graph, optimizeLM)
|
||||
{
|
||||
// build a graph
|
||||
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
|
||||
visualSLAM::Graph graph(testGraph());
|
||||
// add 3 landmark constraints
|
||||
graph->addPointConstraint(1, landmark1);
|
||||
graph->addPointConstraint(2, landmark2);
|
||||
graph->addPointConstraint(3, landmark3);
|
||||
graph.addPointConstraint(1, landmark1);
|
||||
graph.addPointConstraint(2, landmark2);
|
||||
graph.addPointConstraint(3, landmark3);
|
||||
|
||||
// Create an initial values structure corresponding to the ground truth
|
||||
boost::shared_ptr<Values> initialEstimate(new Values);
|
||||
initialEstimate->insert(PoseKey(1), camera1);
|
||||
initialEstimate->insert(PoseKey(2), camera2);
|
||||
initialEstimate->insert(PointKey(1), landmark1);
|
||||
initialEstimate->insert(PointKey(2), landmark2);
|
||||
initialEstimate->insert(PointKey(3), landmark3);
|
||||
initialEstimate->insert(PointKey(4), landmark4);
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(PoseKey(1), camera1);
|
||||
initialEstimate.insert(PoseKey(2), camera2);
|
||||
initialEstimate.insert(PointKey(1), landmark1);
|
||||
initialEstimate.insert(PointKey(2), landmark2);
|
||||
initialEstimate.insert(PointKey(3), landmark3);
|
||||
initialEstimate.insert(PointKey(4), landmark4);
|
||||
|
||||
// Create an ordering of the variables
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
|
||||
Ordering ordering;
|
||||
ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
|
||||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, GaussNewtonParams(), ordering));
|
||||
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, ordering));
|
||||
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
|
||||
|
||||
// Iterate once, and the config should not have changed because we started
|
||||
|
|
@ -111,7 +111,7 @@ TEST( Graph, optimizeLM)
|
|||
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
|
||||
|
||||
// check if correct
|
||||
CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values())));
|
||||
CHECK(assert_equal(initialEstimate,*(afterOneIteration->values())));
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -119,27 +119,27 @@ TEST( Graph, optimizeLM)
|
|||
TEST( Graph, optimizeLM2)
|
||||
{
|
||||
// build a graph
|
||||
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
|
||||
visualSLAM::Graph graph(testGraph());
|
||||
// add 2 camera constraints
|
||||
graph->addPoseConstraint(1, camera1);
|
||||
graph->addPoseConstraint(2, camera2);
|
||||
graph.addPoseConstraint(1, camera1);
|
||||
graph.addPoseConstraint(2, camera2);
|
||||
|
||||
// Create an initial values structure corresponding to the ground truth
|
||||
boost::shared_ptr<Values> initialEstimate(new Values);
|
||||
initialEstimate->insert(PoseKey(1), camera1);
|
||||
initialEstimate->insert(PoseKey(2), camera2);
|
||||
initialEstimate->insert(PointKey(1), landmark1);
|
||||
initialEstimate->insert(PointKey(2), landmark2);
|
||||
initialEstimate->insert(PointKey(3), landmark3);
|
||||
initialEstimate->insert(PointKey(4), landmark4);
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(PoseKey(1), camera1);
|
||||
initialEstimate.insert(PoseKey(2), camera2);
|
||||
initialEstimate.insert(PointKey(1), landmark1);
|
||||
initialEstimate.insert(PointKey(2), landmark2);
|
||||
initialEstimate.insert(PointKey(3), landmark3);
|
||||
initialEstimate.insert(PointKey(4), landmark4);
|
||||
|
||||
// Create an ordering of the variables
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
|
||||
Ordering ordering;
|
||||
ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
|
||||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, GaussNewtonParams(), ordering));
|
||||
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, ordering));
|
||||
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
|
||||
|
||||
// Iterate once, and the config should not have changed because we started
|
||||
|
|
@ -148,7 +148,7 @@ TEST( Graph, optimizeLM2)
|
|||
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
|
||||
|
||||
// check if correct
|
||||
CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values())));
|
||||
CHECK(assert_equal(initialEstimate,*(afterOneIteration->values())));
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
#include <gtsam/slam/simulated2DConstraints.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
||||
namespace iq2D = simulated2D::inequality_constraints;
|
||||
using namespace std;
|
||||
|
|
@ -228,7 +228,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
|
|||
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
|
||||
Symbol x1('x',1), x2('x',2);
|
||||
|
||||
Graph graph;
|
||||
NonlinearFactorGraph graph;
|
||||
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
|
||||
graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2));
|
||||
graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0));
|
||||
|
|
@ -254,7 +254,7 @@ TEST( testBoundingConstraint, avoid_demo) {
|
|||
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
|
||||
Point2 odo(2.0, 0.0);
|
||||
|
||||
Graph graph;
|
||||
NonlinearFactorGraph graph;
|
||||
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1));
|
||||
graph.add(simulated2D::Odometry(odo, soft_model2_alt, x1, x2));
|
||||
graph.add(iq2D::LandmarkAvoid(x2, l1, radius));
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/linear/GaussianMultifrontalSolver.h>
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
#include <gtsam/slam/planarSLAM.h>
|
||||
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@
|
|||
#include <gtsam/slam/visualSLAM.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -35,9 +35,6 @@ typedef PriorFactor<Pose2> PosePrior;
|
|||
typedef NonlinearEquality<Pose2> PoseNLE;
|
||||
typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
|
||||
|
||||
typedef NonlinearFactorGraph PoseGraph;
|
||||
typedef NonlinearOptimizer<PoseGraph> PoseOptimizer;
|
||||
|
||||
Symbol key('x',1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -188,25 +185,23 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
|
|||
PoseNLE nle(key1, feasible1, error_gain);
|
||||
|
||||
// add to a graph
|
||||
boost::shared_ptr<PoseGraph> graph(new PoseGraph());
|
||||
graph->add(nle);
|
||||
NonlinearFactorGraph graph;
|
||||
graph.add(nle);
|
||||
|
||||
// initialize away from the ideal
|
||||
Pose2 initPose(0.0, 2.0, 3.0);
|
||||
boost::shared_ptr<Values> init(new Values());
|
||||
init->insert(key1, initPose);
|
||||
Values init;
|
||||
init.insert(key1, initPose);
|
||||
|
||||
// optimize
|
||||
boost::shared_ptr<Ordering> ord(new Ordering());
|
||||
ord->push_back(key1);
|
||||
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5);
|
||||
PoseOptimizer optimizer(graph, init, ord, params);
|
||||
PoseOptimizer result = optimizer.levenbergMarquardt();
|
||||
Ordering ordering;
|
||||
ord.push_back(key1);
|
||||
NonlinearOptimizer::auto_ptr result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
|
||||
|
||||
// verify
|
||||
Values expected;
|
||||
expected.insert(key1, feasible1);
|
||||
EXPECT(assert_equal(expected, *result.values()));
|
||||
EXPECT(assert_equal(expected, *result->values()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -217,9 +212,9 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
|
|||
Pose2 feasible1(1.0, 2.0, 3.0);
|
||||
|
||||
// initialize away from the ideal
|
||||
boost::shared_ptr<Values> init(new Values());
|
||||
Values init;
|
||||
Pose2 initPose(0.0, 2.0, 3.0);
|
||||
init->insert(key1, initPose);
|
||||
init.insert(key1, initPose);
|
||||
|
||||
double error_gain = 500.0;
|
||||
PoseNLE nle(key1, feasible1, error_gain);
|
||||
|
|
@ -228,32 +223,25 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
|
|||
PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1));
|
||||
|
||||
// add to a graph
|
||||
boost::shared_ptr<PoseGraph> graph(new PoseGraph());
|
||||
graph->add(nle);
|
||||
graph->add(prior);
|
||||
NonlinearFactorGraph graph;
|
||||
graph.add(nle);
|
||||
graph.add(prior);
|
||||
|
||||
// optimize
|
||||
boost::shared_ptr<Ordering> ord(new Ordering());
|
||||
ord->push_back(key1);
|
||||
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5);
|
||||
PoseOptimizer optimizer(graph, init, ord, params);
|
||||
PoseOptimizer result = optimizer.levenbergMarquardt();
|
||||
Ordering ordering;
|
||||
ord.push_back(key1);
|
||||
Values actual = *LevenbergMarquardtOptimizer(graph, values, ordering).optimize()->values();
|
||||
|
||||
// verify
|
||||
Values expected;
|
||||
expected.insert(key1, feasible1);
|
||||
EXPECT(assert_equal(expected, *result.values()));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SharedDiagonal hard_model = noiseModel::Constrained::All(2);
|
||||
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
|
||||
typedef NonlinearFactorGraph Graph;
|
||||
typedef boost::shared_ptr<Graph> shared_graph;
|
||||
typedef boost::shared_ptr<Values> shared_values;
|
||||
typedef NonlinearOptimizer<Graph> Optimizer;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testNonlinearEqualityConstraint, unary_basics ) {
|
||||
Point2 pt(1.0, 2.0);
|
||||
|
|
@ -314,23 +302,23 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
|
|||
simulated2D::Prior::shared_ptr factor(
|
||||
new simulated2D::Prior(badPt, soft_model, key));
|
||||
|
||||
shared_graph graph(new Graph());
|
||||
graph->push_back(constraint);
|
||||
graph->push_back(factor);
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(constraint);
|
||||
graph.push_back(factor);
|
||||
|
||||
shared_values initValues(new Values());
|
||||
initValues->insert(key, badPt);
|
||||
Values initValues;
|
||||
initValues.insert(key, badPt);
|
||||
|
||||
// verify error values
|
||||
EXPECT(constraint->active(*initValues));
|
||||
EXPECT(constraint->active(initValues));
|
||||
|
||||
Values expected;
|
||||
expected.insert(key, truth_pt);
|
||||
EXPECT(constraint->active(expected));
|
||||
EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
|
||||
|
||||
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
|
||||
EXPECT(assert_equal(expected, *actual, tol));
|
||||
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values();
|
||||
EXPECT(assert_equal(expected, actual, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -411,20 +399,20 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
|
|||
new eq2D::OdoEqualityConstraint(
|
||||
truth_pt1.between(truth_pt2), key1, key2));
|
||||
|
||||
shared_graph graph(new Graph());
|
||||
graph->push_back(constraint1);
|
||||
graph->push_back(constraint2);
|
||||
graph->push_back(factor);
|
||||
Graph graph;
|
||||
graph.push_back(constraint1);
|
||||
graph.push_back(constraint2);
|
||||
graph.push_back(factor);
|
||||
|
||||
shared_values initValues(new Values());
|
||||
initValues->insert(key1, Point2());
|
||||
initValues->insert(key2, badPt);
|
||||
Values initValues;
|
||||
initValues.insert(key1, Point2());
|
||||
initValues.insert(key2, badPt);
|
||||
|
||||
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
|
||||
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values();
|
||||
Values expected;
|
||||
expected.insert(key1, truth_pt1);
|
||||
expected.insert(key2, truth_pt2);
|
||||
CHECK(assert_equal(expected, *actual, tol));
|
||||
CHECK(assert_equal(expected, actual, tol));
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
|
|
@ -435,44 +423,44 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
|
|||
* constrained to a particular value
|
||||
*/
|
||||
|
||||
shared_graph graph(new Graph());
|
||||
Graph graph;
|
||||
|
||||
Symbol x1('x',1), x2('x',2);
|
||||
Symbol l1('l',1), l2('l',2);
|
||||
Point2 pt_x1(1.0, 1.0),
|
||||
pt_x2(5.0, 6.0);
|
||||
graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1));
|
||||
graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2));
|
||||
graph.add(eq2D::UnaryEqualityConstraint(pt_x1, x1));
|
||||
graph.add(eq2D::UnaryEqualityConstraint(pt_x2, x2));
|
||||
|
||||
Point2 z1(0.0, 5.0);
|
||||
SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1));
|
||||
graph->add(simulated2D::Measurement(z1, sigma, x1,l1));
|
||||
graph.add(simulated2D::Measurement(z1, sigma, x1,l1));
|
||||
|
||||
Point2 z2(-4.0, 0.0);
|
||||
graph->add(simulated2D::Measurement(z2, sigma, x2,l2));
|
||||
graph.add(simulated2D::Measurement(z2, sigma, x2,l2));
|
||||
|
||||
graph->add(eq2D::PointEqualityConstraint(l1, l2));
|
||||
graph.add(eq2D::PointEqualityConstraint(l1, l2));
|
||||
|
||||
shared_values initialEstimate(new Values());
|
||||
initialEstimate->insert(x1, pt_x1);
|
||||
initialEstimate->insert(x2, Point2());
|
||||
initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth
|
||||
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(x1, pt_x1);
|
||||
initialEstimate.insert(x2, Point2());
|
||||
initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth
|
||||
initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
|
||||
|
||||
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
|
||||
Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimize()->values();
|
||||
|
||||
Values expected;
|
||||
expected.insert(x1, pt_x1);
|
||||
expected.insert(l1, Point2(1.0, 6.0));
|
||||
expected.insert(l2, Point2(1.0, 6.0));
|
||||
expected.insert(x2, Point2(5.0, 6.0));
|
||||
CHECK(assert_equal(expected, *actual, 1e-5));
|
||||
CHECK(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
TEST (testNonlinearEqualityConstraint, map_warp ) {
|
||||
// get a graph
|
||||
shared_graph graph(new Graph());
|
||||
Graph graph;
|
||||
|
||||
// keys
|
||||
Symbol x1('x',1), x2('x',2);
|
||||
|
|
@ -480,37 +468,37 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
|
|||
|
||||
// constant constraint on x1
|
||||
Point2 pose1(1.0, 1.0);
|
||||
graph->add(eq2D::UnaryEqualityConstraint(pose1, x1));
|
||||
graph.add(eq2D::UnaryEqualityConstraint(pose1, x1));
|
||||
|
||||
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1);
|
||||
|
||||
// measurement from x1 to l1
|
||||
Point2 z1(0.0, 5.0);
|
||||
graph->add(simulated2D::Measurement(z1, sigma, x1, l1));
|
||||
graph.add(simulated2D::Measurement(z1, sigma, x1, l1));
|
||||
|
||||
// measurement from x2 to l2
|
||||
Point2 z2(-4.0, 0.0);
|
||||
graph->add(simulated2D::Measurement(z2, sigma, x2, l2));
|
||||
graph.add(simulated2D::Measurement(z2, sigma, x2, l2));
|
||||
|
||||
// equality constraint between l1 and l2
|
||||
graph->add(eq2D::PointEqualityConstraint(l1, l2));
|
||||
graph.add(eq2D::PointEqualityConstraint(l1, l2));
|
||||
|
||||
// create an initial estimate
|
||||
shared_values initialEstimate(new Values());
|
||||
initialEstimate->insert(x1, Point2( 1.0, 1.0));
|
||||
initialEstimate->insert(l1, Point2( 1.0, 6.0));
|
||||
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
|
||||
initialEstimate->insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(x1, Point2( 1.0, 1.0));
|
||||
initialEstimate.insert(l1, Point2( 1.0, 6.0));
|
||||
initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
|
||||
initialEstimate.insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin
|
||||
|
||||
// optimize
|
||||
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
|
||||
Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimize()->values();
|
||||
|
||||
Values expected;
|
||||
expected.insert(x1, Point2(1.0, 1.0));
|
||||
expected.insert(l1, Point2(1.0, 6.0));
|
||||
expected.insert(l2, Point2(1.0, 6.0));
|
||||
expected.insert(x2, Point2(5.0, 6.0));
|
||||
CHECK(assert_equal(expected, *actual, tol));
|
||||
CHECK(assert_equal(expected, actual, tol));
|
||||
}
|
||||
|
||||
// make a realistic calibration matrix
|
||||
|
|
@ -520,9 +508,7 @@ 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<Values> shared_vconfig;
|
||||
typedef visualSLAM::Graph VGraph;
|
||||
typedef NonlinearOptimizer<VGraph> VOptimizer;
|
||||
|
||||
// factors for visual slam
|
||||
typedef NonlinearEquality2<Point3> Point3Equality;
|
||||
|
|
@ -546,32 +532,32 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
|
|||
Symbol l1('l',1), l2('l',2);
|
||||
|
||||
// create graph
|
||||
VGraph::shared_graph graph(new VGraph());
|
||||
VGraph graph;
|
||||
|
||||
// create equality constraints for poses
|
||||
graph->addPoseConstraint(1, camera1.pose());
|
||||
graph->addPoseConstraint(2, camera2.pose());
|
||||
graph.addPoseConstraint(1, camera1.pose());
|
||||
graph.addPoseConstraint(2, camera2.pose());
|
||||
|
||||
// create factors
|
||||
SharedDiagonal vmodel = noiseModel::Unit::Create(3);
|
||||
graph->addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK);
|
||||
graph->addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK);
|
||||
graph.addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK);
|
||||
graph.addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK);
|
||||
|
||||
// add equality constraint
|
||||
graph->add(Point3Equality(l1, l2));
|
||||
graph.add(Point3Equality(l1, l2));
|
||||
|
||||
// create initial data
|
||||
Point3 landmark1(0.5, 5.0, 0.0);
|
||||
Point3 landmark2(1.5, 5.0, 0.0);
|
||||
|
||||
shared_vconfig initValues(new Values());
|
||||
initValues->insert(x1, pose1);
|
||||
initValues->insert(x2, pose2);
|
||||
initValues->insert(l1, landmark1);
|
||||
initValues->insert(l2, landmark2);
|
||||
Values initValues;
|
||||
initValues.insert(x1, pose1);
|
||||
initValues.insert(x2, pose2);
|
||||
initValues.insert(l1, landmark1);
|
||||
initValues.insert(l2, landmark2);
|
||||
|
||||
// optimize
|
||||
VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues);
|
||||
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values();
|
||||
|
||||
// create config
|
||||
Values truthValues;
|
||||
|
|
@ -581,7 +567,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
|
|||
truthValues.insert(l2, landmark);
|
||||
|
||||
// check if correct
|
||||
CHECK(assert_equal(truthValues, *actual, 1e-5));
|
||||
CHECK(assert_equal(truthValues, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -43,66 +43,6 @@ const double tol = 1e-5;
|
|||
Key kx(size_t i) { return Symbol('x',i); }
|
||||
Key kl(size_t i) { return Symbol('l',i); }
|
||||
|
||||
typedef NonlinearOptimizer<example::Graph> Optimizer;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
|
||||
{
|
||||
shared_ptr<example::Graph> fg(new example::Graph(
|
||||
example::createNonlinearFactorGraph()));
|
||||
Optimizer::shared_values initial = example::sharedNoisyValues();
|
||||
|
||||
// Expected values structure is the difference between the noisy config
|
||||
// and the ground-truth config. One step only because it's linear !
|
||||
Ordering ord1; ord1 += kx(2),kl(1),kx(1);
|
||||
VectorValues expected(initial->dims(ord1));
|
||||
Vector dl1(2);
|
||||
dl1(0) = -0.1;
|
||||
dl1(1) = 0.1;
|
||||
expected[ord1[kl(1)]] = dl1;
|
||||
Vector dx1(2);
|
||||
dx1(0) = -0.1;
|
||||
dx1(1) = -0.1;
|
||||
expected[ord1[kx(1)]] = dx1;
|
||||
Vector dx2(2);
|
||||
dx2(0) = 0.1;
|
||||
dx2(1) = -0.2;
|
||||
expected[ord1[kx(2)]] = dx2;
|
||||
|
||||
// Check one ordering
|
||||
Optimizer optimizer1(fg, initial, Optimizer::shared_ordering(new Ordering(ord1)));
|
||||
|
||||
VectorValues actual1 = optimizer1.linearizeAndOptimizeForDelta();
|
||||
CHECK(assert_equal(actual1,expected));
|
||||
|
||||
// SL-FIX // Check another
|
||||
// shared_ptr<Ordering> ord2(new Ordering());
|
||||
// *ord2 += kx(1),kx(2),kl(1);
|
||||
// solver = Optimizer::shared_solver(new Optimizer::solver(ord2));
|
||||
// Optimizer optimizer2(fg, initial, solver);
|
||||
//
|
||||
// VectorValues actual2 = optimizer2.linearizeAndOptimizeForDelta();
|
||||
// CHECK(assert_equal(actual2,expected));
|
||||
//
|
||||
// // And yet another...
|
||||
// shared_ptr<Ordering> ord3(new Ordering());
|
||||
// *ord3 += kl(1),kx(1),kx(2);
|
||||
// solver = Optimizer::shared_solver(new Optimizer::solver(ord3));
|
||||
// Optimizer optimizer3(fg, initial, solver);
|
||||
//
|
||||
// VectorValues actual3 = optimizer3.linearizeAndOptimizeForDelta();
|
||||
// CHECK(assert_equal(actual3,expected));
|
||||
//
|
||||
// // More...
|
||||
// shared_ptr<Ordering> ord4(new Ordering());
|
||||
// *ord4 += kx(1),kx(2), kl(1);
|
||||
// solver = Optimizer::shared_solver(new Optimizer::solver(ord4));
|
||||
// Optimizer optimizer4(fg, initial, solver);
|
||||
//
|
||||
// VectorValues actual4 = optimizer4.linearizeAndOptimizeForDelta();
|
||||
// CHECK(assert_equal(actual4,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, iterateLM )
|
||||
{
|
||||
|
|
@ -120,22 +60,13 @@ TEST( NonlinearOptimizer, iterateLM )
|
|||
ord->push_back(kx(1));
|
||||
|
||||
// create initial optimization state, with lambda=0
|
||||
Optimizer optimizer(fg, config, ord, NonlinearOptimizationParameters::newLambda(0.));
|
||||
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0);
|
||||
|
||||
// normal iterate
|
||||
Optimizer iterated1 = optimizer.iterate();
|
||||
NonlinearOptimizer::auto_ptr iterated1 = GaussNewtonOptimizer(fg, config, GaussNewtonParams(), ord).iterate();
|
||||
|
||||
// LM iterate with lambda 0 should be the same
|
||||
Optimizer iterated2 = optimizer.iterateLM();
|
||||
|
||||
// Try successive iterates. TODO: ugly pointers, better way ?
|
||||
Optimizer *pointer = new Optimizer(iterated2);
|
||||
for (int i=0;i<10;i++) {
|
||||
Optimizer* newOptimizer = new Optimizer(pointer->iterateLM());
|
||||
delete pointer;
|
||||
pointer = newOptimizer;
|
||||
}
|
||||
delete(pointer);
|
||||
NonlinearOptimizer::auto_ptr iterated2 = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0)->iterate();
|
||||
|
||||
CHECK(assert_equal(*iterated1.values(), *iterated2.values(), 1e-9));
|
||||
}
|
||||
|
|
@ -169,12 +100,12 @@ TEST( NonlinearOptimizer, optimize )
|
|||
Optimizer optimizer(fg, c0, ord, params);
|
||||
|
||||
// Gauss-Newton
|
||||
Optimizer actual1 = optimizer.gaussNewton();
|
||||
DOUBLES_EQUAL(0,fg->error(*(actual1.values())),tol);
|
||||
NonlinearOptimizer::auto_ptr actual1 = *GaussNewtonOptimizer(fg, c0, GaussNewtonParams(), ord).optimize();
|
||||
DOUBLES_EQUAL(0,fg->error(*(actual1->values())),tol);
|
||||
|
||||
// Levenberg-Marquardt
|
||||
Optimizer actual2 = optimizer.levenbergMarquardt();
|
||||
DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol);
|
||||
Optimizer actual2 = *LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimizer();
|
||||
DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue