Fixed key bug in GeneralSFMFactor_Cal3Bundler
parent
453c5132e1
commit
9816d4fe61
|
|
@ -366,7 +366,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Constrain position of system with the first camera constrained to the origin
|
// Constrain position of system with the first camera constrained to the origin
|
||||||
graph.addCameraConstraint(X(0), cameras[0]);
|
graph.addCameraConstraint(0, cameras[0]);
|
||||||
|
|
||||||
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
|
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
|
||||||
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
|
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue