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

Faster linearize now in class

release/4.3a0
Frank Dellaert 2015-06-10 18:37:09 -04:00
parent 171793aad3
commit 5dc149fe23
2 changed files with 47 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
gtsam/slam/GeneralSFMFactor.h
View File

@ -116,7 +116,6 @@ namespace gtsam {
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
try {
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
return reprojError.vector();
@ -124,12 +123,50 @@ namespace gtsam {
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, DimC);
if (H2) *H2 = zeros(2, DimL);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
// TODO warn if verbose output asked for
return zero(2);
}
}
/// Linearize using fixed-size matrices
boost::shared_ptr<GaussianFactor> linearize(const Values& values) {
// Only linearize if the factor is active
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
const Key key1 = this->key1(), key2 = this->key2();
Eigen::Matrix<double, 2, DimC> H1;
Eigen::Matrix<double, 2, DimL> H2;
Vector2 b;
try {
const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2);
Point2 reprojError(camera.project2(point, H1, H2) - measured());
b = -reprojError.vector();
} catch (CheiralityException& e) {
// TODO warn if verbose output asked for
return boost::make_shared<JacobianFactor>();
}
// Whiten the system if needed
const SharedNoiseModel& noiseModel = this->get_noiseModel();
if (noiseModel && !noiseModel->isUnit()) {
// TODO: implement WhitenSystem for fixed size matrices and include
// above
H1 = noiseModel->Whiten(H1);
H2 = noiseModel->Whiten(H2);
b = noiseModel->Whiten(b);
}
if (noiseModel && noiseModel->isConstrained()) {
using noiseModel::Constrained;
return boost::make_shared<JacobianFactor>(
key1, H1, key2, H2, b,
boost::static_pointer_cast<Constrained>(noiseModel)->unit());
} else {
return boost::make_shared<JacobianFactor>(key1, H1, key2, H2, b);
}
}
/** return the measured */
inline const Point2 measured() const {
return measured_;

57
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -430,49 +430,6 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
EXPECT(assert_equal(expected, actual, 1e-4));
}
/* ************************************************************************* */
static const int DimC = 11, DimL = 3;
/// Linearize using fixed-size matrices
boost::shared_ptr<GaussianFactor> linearize(const Projection& factor,
const Values& values) {
// Only linearize if the factor is active
if (!factor.active(values)) return boost::shared_ptr<JacobianFactor>();
const Key key1 = factor.key1(), key2 = factor.key2();
Eigen::Matrix<double, 2, DimC> H1;
Eigen::Matrix<double, 2, DimL> H2;
Vector2 b;
try {
const GeneralCamera& camera = values.at<GeneralCamera>(key1);
const Point3& point = values.at<Point3>(key2);
Point2 reprojError(camera.project2(point, H1, H2) - factor.measured());
b = -reprojError.vector();
} catch (CheiralityException& e) {
// TODO warn if verbose output asked for
return boost::make_shared<JacobianFactor>();
}
// Whiten the system if needed
const SharedNoiseModel& noiseModel = factor.get_noiseModel();
if (noiseModel && !noiseModel->isUnit()) {
// TODO: implement WhitenSystem for fixed size matrices and include above
H1 = noiseModel->Whiten(H1);
H2 = noiseModel->Whiten(H2);
b = noiseModel->Whiten(b);
}
if (noiseModel && noiseModel->isConstrained()) {
using noiseModel::Constrained;
return boost::make_shared<JacobianFactor>(
key1, H1, key2, H2, b,
boost::static_pointer_cast<Constrained>(noiseModel)->unit());
} else {
return boost::make_shared<JacobianFactor>(key1, H1, key2, H2, b);
}
}
/* ************************************************************************* */
TEST(GeneralSFMFactor, Linearize) {
Point2 z(3.,0.);
@ -490,31 +447,31 @@ TEST(GeneralSFMFactor, Linearize) {
const SharedNoiseModel model;
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr actual = linearize(factor, values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected,*actual,1e-9));
}
// Test with Unit Model
{
const SharedNoiseModel model(noiseModel::Unit::Create(2));
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.linearize(values);
GaussianFactor::shared_ptr actual = linearize(factor, values);
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected,*actual,1e-9));
}
// Test with Isotropic noise
{
const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2,0.5));
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.linearize(values);
GaussianFactor::shared_ptr actual = linearize(factor, values);
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected,*actual,1e-9));
}
// Test with Constrained Model
{
const SharedNoiseModel model(noiseModel::Constrained::All(2));
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.linearize(values);
GaussianFactor::shared_ptr actual = linearize(factor, values);
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected,*actual,1e-9));
}
}