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

Rename ImplicitSchurFactor to RegularImplicitSchurFactor ('Regular' means fixed-size)

release/4.3a0
Sungtae An 2014-11-10 23:37:42 -05:00
parent f2b7cc0f3c
commit 3dbc9929a4
4 changed files with 34 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam/slam/ImplicitSchurFactor.h → gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -1,5 +1,5 @@
/** /**
* @file ImplicitSchurFactor.h * @file RegularImplicitSchurFactor.h
* @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor * @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
* @author Frank Dellaert * @author Frank Dellaert
* @author Luca Carlone * @author Luca Carlone
@ -17,13 +17,13 @@
namespace gtsam { namespace gtsam {
/** /**
* ImplicitSchurFactor * RegularImplicitSchurFactor
*/ */
template<size_t D> // template<size_t D> //
class ImplicitSchurFactor: public GaussianFactor { class RegularImplicitSchurFactor: public GaussianFactor {
public: public:
typedef ImplicitSchurFactor This; ///< Typedef to this class typedef RegularImplicitSchurFactor This; ///< Typedef to this class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected: protected:
@ -41,11 +41,11 @@ protected:
public: public:
/// Constructor /// Constructor
ImplicitSchurFactor() { RegularImplicitSchurFactor() {
} }
/// Construct from blcoks of F, E, inv(E'*E), and RHS vector b /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b
ImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E, RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b) : const Matrix3& P, const Vector& b) :
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
initKeys(); initKeys();
@ -59,7 +59,7 @@ public:
} }
/// Destructor /// Destructor
virtual ~ImplicitSchurFactor() { virtual ~RegularImplicitSchurFactor() {
} }
// Write access, only use for construction! // Write access, only use for construction!
@ -88,7 +88,7 @@ public:
/// print /// print
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << " ImplicitSchurFactor " << std::endl; std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s); Factor::print(s);
std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
std::cout << " E_ \n" << E_ << std::endl; std::cout << " E_ \n" << E_ << std::endl;
@ -97,7 +97,7 @@ public:
/// equals /// equals
bool equals(const GaussianFactor& lf, double tol) const { bool equals(const GaussianFactor& lf, double tol) const {
if (!dynamic_cast<const ImplicitSchurFactor*>(&lf)) if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
return false; return false;
else { else {
return false; return false;
@ -111,21 +111,21 @@ public:
virtual Matrix augmentedJacobian() const { virtual Matrix augmentedJacobian() const {
throw std::runtime_error( throw std::runtime_error(
"ImplicitSchurFactor::augmentedJacobian non implemented"); "RegularImplicitSchurFactor::augmentedJacobian non implemented");
return Matrix(); return Matrix();
} }
virtual std::pair<Matrix, Vector> jacobian() const { virtual std::pair<Matrix, Vector> jacobian() const {
throw std::runtime_error("ImplicitSchurFactor::jacobian non implemented"); throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented");
return std::make_pair(Matrix(), Vector()); return std::make_pair(Matrix(), Vector());
} }
virtual Matrix augmentedInformation() const { virtual Matrix augmentedInformation() const {
throw std::runtime_error( throw std::runtime_error(
"ImplicitSchurFactor::augmentedInformation non implemented"); "RegularImplicitSchurFactor::augmentedInformation non implemented");
return Matrix(); return Matrix();
} }
virtual Matrix information() const { virtual Matrix information() const {
throw std::runtime_error( throw std::runtime_error(
"ImplicitSchurFactor::information non implemented"); "RegularImplicitSchurFactor::information non implemented");
return Matrix(); return Matrix();
} }
@ -211,18 +211,18 @@ public:
} }
virtual GaussianFactor::shared_ptr clone() const { virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_, return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_); PointCovariance_, E_, b_);
throw std::runtime_error("ImplicitSchurFactor::clone non implemented"); throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
} }
virtual bool empty() const { virtual bool empty() const {
return false; return false;
} }
virtual GaussianFactor::shared_ptr negate() const { virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_, return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_); PointCovariance_, E_, b_);
throw std::runtime_error("ImplicitSchurFactor::negate non implemented"); throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented");
} }
// Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
@ -455,7 +455,7 @@ public:
} }
}; };
// ImplicitSchurFactor // RegularImplicitSchurFactor
} }

8
gtsam/slam/SmartFactorBase.h
View File

@ -21,7 +21,7 @@
#include "JacobianFactorQ.h" #include "JacobianFactorQ.h"
#include "JacobianFactorSVD.h" #include "JacobianFactorSVD.h"
#include "ImplicitSchurFactor.h" #include "RegularImplicitSchurFactor.h"
#include "RegularHessianFactor.h" #include "RegularHessianFactor.h"
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
@ -629,11 +629,11 @@ public:
} }
// **************************************************************************************************** // ****************************************************************************************************
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor( boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
typename boost::shared_ptr<ImplicitSchurFactor<D> > f( typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
new ImplicitSchurFactor<D>()); new RegularImplicitSchurFactor<D>());
computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(),
cameras, point, lambda, diagonalDamping); cameras, point, lambda, diagonalDamping);
f->initKeys(); f->initKeys();

8
gtsam/slam/SmartProjectionFactor.h
View File

@ -298,7 +298,7 @@ public:
|| (!this->manageDegeneracy_ || (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) { && (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) { if (isDebug) {
std::cout << "createImplicitSchurFactor: degenerate configuration" std::cout << "createRegularImplicitSchurFactor: degenerate configuration"
<< std::endl; << std::endl;
} }
return false; return false;
@ -409,12 +409,12 @@ public:
} }
// create factor // create factor
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor( boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createImplicitSchurFactor(cameras, point_, lambda); return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
else else
return boost::shared_ptr<ImplicitSchurFactor<D> >(); return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
} }
/// create factor /// create factor

16
gtsam/slam/tests/testImplicitSchurFactor.cpp → gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -6,7 +6,7 @@
*/ */
//#include <gtsam_unstable/slam/ImplicitSchurFactor.h> //#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/ImplicitSchurFactor.h> #include <gtsam/slam/RegularImplicitSchurFactor.h>
//#include <gtsam_unstable/slam/JacobianFactorQ.h> //#include <gtsam_unstable/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQ.h> #include <gtsam/slam/JacobianFactorQ.h>
//#include "gtsam_unstable/slam/JacobianFactorQR.h" //#include "gtsam_unstable/slam/JacobianFactorQR.h"
@ -38,19 +38,19 @@ const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.); const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.);
//************************************************************************************* //*************************************************************************************
TEST( implicitSchurFactor, creation ) { TEST( regularImplicitSchurFactor, creation ) {
// Matrix E = Matrix::Ones(6,3); // Matrix E = Matrix::Ones(6,3);
Matrix E = zeros(6, 3); Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2); E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3); E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse(); Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> expected(Fblocks, E, P, b); RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
Matrix expectedP = expected.getPointCovariance(); Matrix expectedP = expected.getPointCovariance();
EXPECT(assert_equal(expectedP, P)); EXPECT(assert_equal(expectedP, P));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( implicitSchurFactor, addHessianMultiply ) { TEST( regularImplicitSchurFactor, addHessianMultiply ) {
Matrix E = zeros(6, 3); Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2); E.block<2,2>(0, 0) = eye(2);
@ -81,11 +81,11 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
keys += 0,1,2,3; keys += 0,1,2,3;
Vector x = xvalues.vector(keys); Vector x = xvalues.vector(keys);
Vector expected = zero(24); Vector expected = zero(24);
ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
// Create ImplicitSchurFactor // Create ImplicitSchurFactor
ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
VectorValues zero = 0 * yExpected;// quick way to get zero w right structure VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
{ // First Version { // First Version
@ -190,7 +190,7 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(implicitSchurFactor, hessianDiagonal) TEST(regularImplicitSchurFactor, hessianDiagonal)
{ {
/* TESTED AGAINST MATLAB /* TESTED AGAINST MATLAB
* F = [ones(2,6) zeros(2,6) zeros(2,6) * F = [ones(2,6) zeros(2,6) zeros(2,6)
@ -207,7 +207,7 @@ TEST(implicitSchurFactor, hessianDiagonal)
E.block<2,3>(2, 0) << 1,2,3,4,5,6; E.block<2,3>(2, 0) << 1,2,3,4,5,6;
E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
Matrix3 P = (E.transpose() * E).inverse(); Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> factor(Fblocks, E, P, b); RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
// hessianDiagonal // hessianDiagonal
VectorValues expected; VectorValues expected;