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

rename of computeJacobians overloads to better reflect functionality

release/4.3a0
dellaert 2015-02-25 19:30:17 +01:00
parent c5394da29e
commit 850470ef06
6 changed files with 121 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/slam/JacobianFactorSVD.h
View File

@ -9,7 +9,7 @@
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
* JacobianFactor for Schur complement
*/
template<size_t D, size_t ZDim>
class JacobianFactorSVD: public RegularJacobianFactor<D> {
@ -38,7 +38,14 @@ public:
JacobianFactor::fillTerms(QF, zeroVector, model);
}
/// Constructor
/**
* @brief Constructor
* Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
* and a reduced point derivative, Enull
* and creates a reduced-rank Jacobian factor on the CameraSet
*
* @Fblocks:
*/
JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
const Matrix& Enull, const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :

88
gtsam/slam/SmartFactorBase.h
View File

@ -81,6 +81,7 @@ protected:
boost::optional<Pose3> body_P_sensor_;
// check that noise model is isotropic and the same
// TODO, this is hacky, we should just do this via constructor, not add
void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
if (!sharedNoiseModel)
return;
@ -213,6 +214,36 @@ public:
&& body_P_sensor_->equals(*e->body_P_sensor_)));
}
/// Compute reprojection errors
Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
return cameras.reprojectionError(point, measured_);
}
/**
* Compute reprojection errors and derivatives
* TODO: the treatment of body_P_sensor_ is weird: the transformation
* is applied in the caller, but the derivatives are computed here.
*/
Vector reprojectionError(const Cameras& cameras, const Point3& point,
typename Cameras::FBlocks& F, Matrix& E) const {
Vector b = cameras.reprojectionError(point, measured_, F, E);
// Apply sensor chain rule if needed TODO: no simpler way ??
if (body_P_sensor_) {
size_t m = keys_.size();
for (size_t i = 0; i < m; i++) {
const Pose3& pose_i = cameras[i].pose();
Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
Matrix66 J;
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
F[i].leftCols(6) *= J;
}
}
return b;
}
/// Calculate vector of re-projection errors, noise model applied
Vector whitenedErrors(const Cameras& cameras, const Point3& point) const {
Vector b = cameras.reprojectionError(point, measured_);
@ -251,36 +282,6 @@ public:
return 0.5 * b.dot(b);
}
/// Compute reprojection errors
Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
return cameras.reprojectionError(point, measured_);
}
/**
* Compute reprojection errors and derivatives
* TODO: the treatment of body_P_sensor_ is weird: the transformation
* is applied in the caller, but the derivatives are computed here.
*/
Vector reprojectionError(const Cameras& cameras, const Point3& point,
typename Cameras::FBlocks& F, Matrix& E) const {
Vector b = cameras.reprojectionError(point, measured_, F, E);
// Apply sensor chain rule if needed TODO: no simpler way ??
if (body_P_sensor_) {
size_t m = keys_.size();
for (size_t i = 0; i < m; i++) {
const Pose3& pose_i = cameras[i].pose();
Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
Matrix66 J;
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
F[i].leftCols(6) *= J;
}
}
return b;
}
/// Computes Point Covariance P from E
static Matrix3 PointCov(Matrix& E) {
return (E.transpose() * E).inverse();
@ -371,7 +372,6 @@ public:
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues();
size_t m = this->keys_.size();
// Enull = zeros(ZDim * m, ZDim * m - 3);
Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns
return f;
@ -661,7 +661,7 @@ public:
Matrix E;
Vector b;
computeJacobians(F, E, b, cameras, point);
noiseModel_->WhitenSystem(E,b);
noiseModel_->WhitenSystem(E, b);
Matrix3 P = PointCov(E, lambda, diagonalDamping);
// TODO make WhitenInPlace work with any dense matrix type
BOOST_FOREACH(KeyMatrix2D& Fblock,F)
@ -675,13 +675,15 @@ public:
boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
std::vector<KeyMatrix2D> Fblocks;
std::vector<KeyMatrix2D> F;
Matrix E;
Vector b;
computeJacobians(Fblocks, E, b, cameras, point);
computeJacobians(F, E, b, cameras, point);
const size_t M = b.size();
std::cout << M << std::endl;
Matrix3 P = PointCov(E, lambda, diagonalDamping);
return boost::make_shared<JacobianFactorQ<Dim, ZDim> > //
(Fblocks, E, P, b, noiseModel_);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(F, E, P, b, n);
}
/**
@ -690,13 +692,15 @@ public:
*/
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
size_t m = this->keys_.size();
std::vector<KeyMatrix2D> F;
Vector b;
Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
return boost::make_shared<JacobianFactorSVD<Dim, ZDim> > //
(Fblocks, Enull, b, noiseModel_);
const size_t M = ZDim * m;
Matrix E0(M, M - 3);
computeJacobiansSVD(F, E0, b, cameras, point);
std::cout << M << std::endl;
SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(F, E0, b, n);
}
private:

65
gtsam/slam/SmartProjectionFactor.h
View File

@ -368,7 +368,12 @@ public:
// ==================================================================
Matrix F, E;
Vector b;
double f = computeJacobians(F, E, b, cameras);
double f;
{
std::vector<typename Base::KeyMatrix2D> Fblocks;
f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
Base::FillDiagonalF(Fblocks,F); // expensive !
}
// Schur complement trick
// Frank says: should be possible to do this more efficiently?
@ -486,21 +491,12 @@ public:
return nonDegenerate;
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeJacobians(Fblocks, E, b, cameras);
return nonDegenerate;
}
/// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Vector& b, const Cameras& cameras) const {
double computeJacobiansWithTriangulatedPoint(
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const {
if (this->degenerate_) {
std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
@ -515,9 +511,9 @@ public:
}
// TODO replace all this by Call to CameraSet
int numKeys = this->keys_.size();
E = zeros(2 * numKeys, 2);
b = zero(2 * numKeys);
int m = this->keys_.size();
E = zeros(2 * m, 2);
b = zero(2 * m);
double f = 0;
for (size_t i = 0; i < this->measured_.size(); i++) {
if (i == 0) { // first pose
@ -541,35 +537,27 @@ public:
} // end else
}
/// Version that takes values, and creates the point
bool triangulateAndComputeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
return nonDegenerate;
}
/// takes values
bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& Enull, Vector& b, const Values& values) const {
bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& Enull,
Vector& b, const Values& values) const {
typename Base::Cameras cameras;
double good = computeCamerasAndTriangulate(values, cameras);
if (good)
computeJacobiansSVD(Fblocks, Enull, b, cameras);
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
return true;
}
/// SVD version
double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& Enull, Vector& b, const Cameras& cameras) const {
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
}
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
// TODO should there be a lambda?
double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
const Cameras& cameras) const {
return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
}
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
double computeJacobians(Matrix& F, Matrix& E, Vector& b,
const Cameras& cameras) const {
return Base::computeJacobians(F, E, b, cameras, point_);
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras;
@ -652,6 +640,7 @@ public:
inline bool isPointBehindCamera() const {
return cheiralityException_;
}
/** return cheirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;

8
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -754,17 +754,17 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
cameras.push_back(level_camera_right);
factor1->error(values); // this is important to have a triangulation of the point
Point3 expectedPoint;
Point3 point;
if (factor1->point())
expectedPoint = *(factor1->point());
factor1->computeJacobians(expectedF, expectedE, expectedb, cameras);
point = *(factor1->point());
factor1->computeJacobians(expectedF, expectedE, expectedb, cameras, point);
NonlinearFactorGraph generalGraph;
SFMFactor sfm1(level_uv, unit2, c1, l1);
SFMFactor sfm2(level_uv_right, unit2, c2, l1);
generalGraph.push_back(sfm1);
generalGraph.push_back(sfm2);
values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
values.insert(l1, point); // note: we get rid of possible errors in the triangulation
Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse();

41
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -153,7 +153,7 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) {
// Calculate using computeJacobians
Vector b;
vector<SmartFactor::KeyMatrix2D> Fblocks;
double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras);
double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras, *point);
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8);
}
@ -364,19 +364,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) {
// Calculate expected derivative for point (easiest to check)
SmartFactor::Cameras cameras = smartFactor1->cameras(values);
boost::function<Vector(Point3)> f = //
boost::bind(&SmartFactor::whitenedErrors, *smartFactor1, cameras, _1);
boost::bind(&SmartFactor::reprojectionError, *smartFactor1, cameras, _1);
boost::optional<Point3> point = smartFactor1->point();
CHECK(point);
// Note ! After refactor the noiseModel is only in the factors, not these matrices
Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
// TODO, this is really a test of CameraSet
Matrix expectedE = numericalDerivative11<Vector, Point3>(f, *point);
// Calculate using computeEP
Matrix actualE, PointCov;
smartFactor1->computeEP(actualE, PointCov, values);
EXPECT(assert_equal(expectedE, actualE, 1e-7));
// Calculate using whitenedError
// Calculate using reprojectionError
Matrix E;
SmartFactor::Cameras::FBlocks F;
Vector actualErrors = smartFactor1->reprojectionError(cameras, *point, F, E);
@ -472,7 +472,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
E(2, 2) = 10;
E(3, 1) = 100;
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(x1, 10 * F1))(make_pair(x2, 10 * F2));
(make_pair(x1, F1))(make_pair(x2, F2));
Matrix3 P = (E.transpose() * E).inverse();
Vector4 b;
b.setZero();
@ -483,10 +483,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
CHECK(actual);
CHECK(assert_equal(expected, *actual));
EXPECT(assert_equal(expected, *actual));
// createJacobianQFactor
JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b);
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n);
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
smartFactor1->createJacobianQFactor(cameras, 0.0);
@ -890,8 +891,10 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
using namespace vanillaPose;
// Two slightly different cameras
Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose2 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Camera cam2(pose2, sharedK);
Camera cam3(pose3, sharedK);
@ -970,8 +973,10 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
views.push_back(x3);
// Two different cameras
Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose2 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Camera cam2(pose2, sharedK2);
Camera cam3(pose3, sharedK2);
@ -1050,8 +1055,10 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
views.push_back(x3);
// Two different cameras
Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose2 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Camera cam2(pose2, sharedK);
Camera cam3(pose3, sharedK);
@ -1390,8 +1397,10 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
views.push_back(x3);
// Two different cameras
Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose2 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Camera cam2(pose2, sharedBundlerK);
Camera cam3(pose3, sharedBundlerK);

45
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -582,41 +582,16 @@ public:
} // end else
}
// /// Version that computes PointCov, with optional lambda parameter
// double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
// Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
// const double lambda = 0.0) const {
//
// double f = computeJacobians(Fblocks, E, b, cameras);
//
// // Point covariance inv(E'*E)
// PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
//
// return f;
// }
//
// /// takes values
// bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
// Matrix& Enull, Vector& b, const Values& values) const {
// typename Base::Cameras cameras;
// double good = computeCamerasAndTriangulate(values, cameras);
// if (good)
// computeJacobiansSVD(Fblocks, Enull, b, cameras);
// return true;
// }
//
// /// SVD version
// double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
// Matrix& Enull, Vector& b, const Cameras& cameras) const {
// return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
// }
//
// /// Returns Matrix, TODO: maybe should not exist -> not sparse !
// // TODO should there be a lambda?
// double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
// const Cameras& cameras) const {
// return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
// }
/// takes values
bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& Enull,
Vector& b, const Values& values) const {
typename Base::Cameras cameras;
double good = computeCamerasAndTriangulate(values, cameras);
if (good)
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
return true;
}
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
double computeJacobians(Matrix& F, Matrix& E, Vector& b,