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

Merge branch 'TriangulationResult' into feature/SmartFactors3

release/4.3a0
dellaert 2015-03-02 08:54:54 -08:00
parent 69e56cee1c ca18c13cd2
commit 59ab204f9d
10 changed files with 528 additions and 572 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
.cproject
View File

@ -1309,6 +1309,7 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1348,6 +1349,7 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1355,6 +1357,7 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1458,7 +1461,6 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1536,10 +1538,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testRegularImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testImplicitSchurFactor.run</buildTarget>
<buildArguments>-j4</buildArguments>
<buildTarget>testRegularImplicitSchurFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1793,7 +1795,6 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1801,7 +1802,6 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1809,7 +1809,6 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1817,7 +1816,6 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2009,6 +2007,7 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2160,7 +2159,6 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2168,7 +2166,6 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2216,7 +2213,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2224,7 +2220,6 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2232,7 +2227,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2248,7 +2242,6 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3392,7 +3385,6 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3400,7 +3392,6 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3408,7 +3399,6 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

2
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -129,7 +129,7 @@ TEST(PinholeSet, Pinhole) {
// Instantiate triangulateSafe
TriangulationParameters params;
TriangulationResult actual = set.triangulateSafe(z,params);
CHECK(actual.degenerate);
CHECK(actual.degenerate());
}
/* ************************************************************************* */

144
gtsam/geometry/triangulation.h
View File

@ -316,12 +316,57 @@ struct TriangulationParameters {
_landmarkDistanceThreshold), dynamicOutlierRejectionThreshold(
_dynamicOutlierRejectionThreshold) {
}
// stream to output
friend std::ostream &operator<<(std::ostream &os,
const TriangulationParameters& p) {
os << "rankTolerance = " << p.rankTolerance << std::endl;
os << "enableEPI = " << p.enableEPI << std::endl;
os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
<< std::endl;
os << "dynamicOutlierRejectionThreshold = "
<< p.dynamicOutlierRejectionThreshold << std::endl;
return os;
}
};
struct TriangulationResult {
Point3 point;
bool degenerate;
bool cheiralityException;
/**
* TriangulationResult is an optional point, along with the reasons why it is invalid.
*/
class TriangulationResult: public boost::optional<Point3> {
enum Status {
VALID, DEGENERATE, BEHIND_CAMERA
};
Status status_;
TriangulationResult(Status s) :
status_(s) {
}
public:
TriangulationResult(const Point3& p) :
status_(VALID) {
reset(p);
}
static TriangulationResult Degenerate() {
return TriangulationResult(DEGENERATE);
}
static TriangulationResult BehindCamera() {
return TriangulationResult(BEHIND_CAMERA);
}
bool degenerate() const {
return status_ == DEGENERATE;
}
bool behindCamera() const {
return status_ == BEHIND_CAMERA;
}
// stream to output
friend std::ostream &operator<<(std::ostream &os,
const TriangulationResult& result) {
if (result)
os << "point = " << *result << std::endl;
else
os << "no point, status = " << result.status_ << std::endl;
return os;
}
};
/// triangulateSafe: extensive checking of the outcome
@ -330,62 +375,53 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
const std::vector<Point2>& measured,
const TriangulationParameters& params) {
TriangulationResult result;
size_t m = cameras.size();
// if we have a single pose the corresponding factor is uninformative
if (m < 2) {
result.degenerate = true;
return result;
}
if (m < 2)
return TriangulationResult::Degenerate();
else
// We triangulate the 3D position of the landmark
try {
Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
params.rankTolerance, params.enableEPI);
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
result.point = triangulatePoint3<CAMERA>(cameras, measured,
params.rankTolerance, params.enableEPI);
result.degenerate = false;
result.cheiralityException = false;
// Check landmark distance and reprojection errors to avoid outliers
size_t i = 0;
double totalReprojError = 0.0;
BOOST_FOREACH(const CAMERA& camera, cameras) {
// we discard smart factors corresponding to points that are far away
Point3 cameraTranslation = camera.pose().translation();
if (cameraTranslation.distance(point) > params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate();
// Also flag if point is behind any of the cameras
try {
const Point2& zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
return TriangulationResult::BehindCamera();
}
i += 1;
}
// we discard smart factors that have large reprojection error
if (params.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params.dynamicOutlierRejectionThreshold)
return TriangulationResult::Degenerate();
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i = 0;
BOOST_FOREACH(const CAMERA& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(result.point)
> params.landmarkDistanceThreshold) {
result.degenerate = true;
break;
}
const Point2& zi = measured.at(i);
try {
Point2 reprojectionError(camera.project(result.point) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
result.cheiralityException = true;
}
i += 1;
// all good!
return TriangulationResult(point);
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
return TriangulationResult::Degenerate();
} catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
return TriangulationResult::BehindCamera();
}
// we discard smart factors that have large reprojection error
if (params.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params.dynamicOutlierRejectionThreshold)
result.degenerate = true;
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
result.degenerate = true;
result.cheiralityException = false;
} catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
result.cheiralityException = true;
}
return result;
}
} // \namespace gtsam

195
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -9,6 +9,7 @@
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <boost/foreach.hpp>
#include <iosfwd>
@ -17,7 +18,7 @@ namespace gtsam {
/**
* RegularImplicitSchurFactor
*/
template<size_t D> //
template<size_t D, size_t Z = 2> //
class RegularImplicitSchurFactor: public GaussianFactor {
public:
@ -26,12 +27,12 @@ public:
protected:
typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block
typedef Eigen::Matrix<double, Z, D> Matrix2D; ///< type of an F block
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
const std::vector<KeyMatrix2D> Fblocks_; ///< All 2*D F blocks (one for each camera)
const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
const std::vector<KeyMatrix2D> Fblocks_; ///< All Z*D F blocks (one for each camera)
const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (Z*Z if degenerate)
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
const Vector b_; ///< 2m-dimensional RHS vector
@ -122,15 +123,141 @@ public:
"RegularImplicitSchurFactor::jacobian non implemented");
return std::make_pair(Matrix(), Vector());
}
virtual Matrix augmentedInformation() const {
throw std::runtime_error(
"RegularImplicitSchurFactor::augmentedInformation non implemented");
return Matrix();
/**
* Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
* Fast version - works on with sparsity
*/
static void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
// Schur complement trick
// G = F' * F - F' * E * P * E' * F
// g = F' * (b - E * P * E' * b)
// a single point is observed in m cameras
size_t m = Fblocks.size();
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const Matrix2D& Fi = Fblocks.at(i).second;
const Matrix23 Ei_P = E.block<Z, 3>(Z * i, 0) * P;
// D = (Dx2) * (Z)
augmentedHessian(i, m) = Fi.transpose() * b.segment<Z>(Z * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian(i, i) = Fi.transpose()
* (Fi - Ei_P * E.block<Z, 3>(Z * i, 0).transpose() * Fi);
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Matrix2D& Fj = Fblocks.at(j).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i, j) = -Fi.transpose()
* (Ei_P * E.block<Z, 3>(Z * j, 0).transpose() * Fj);
}
} // end of for over cameras
}
/**
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
static void updateSparseSchurComplement(
const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P /*Point Covariance*/, const Vector& b, const double f,
const FastVector<Key>& allKeys, const FastVector<Key>& keys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
// Schur complement trick
// G = F' * F - F' * E * P * E' * F
// g = F' * (b - E * P * E' * b)
MatrixDD matrixBlock;
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
// a single point is observed in m cameras
size_t m = Fblocks.size(); // cameras observing current point
size_t aug_m = (augmentedHessian.rows() - 1) / D; // all cameras in the group
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera in the current factor
const Matrix2D& Fi = Fblocks.at(i).second;
const Matrix23 Ei_P = E.block<Z, 3>(Z * i, 0) * P;
// D = (DxZDim) * (Z)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i = this->keys_[i];
DenseIndex aug_i = KeySlotMap.at(keys[i]);
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, aug_m) =
augmentedHessian(aug_i, aug_m).knownOffDiagonal()
+ Fi.transpose() * b.segment<Z>(Z * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i, aug_i);
// add contribution of current factor
augmentedHessian(aug_i, aug_i) = matrixBlock
+ (Fi.transpose()
* (Fi - Ei_P * E.block<Z, 3>(Z * i, 0).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Matrix2D& Fj = Fblocks.at(j).second;
//Key cameraKey_j = this->keys_[j];
DenseIndex aug_j = KeySlotMap.at(keys[j]);
// (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, aug_j) =
augmentedHessian(aug_i, aug_j).knownOffDiagonal()
- Fi.transpose()
* (Ei_P * E.block<Z, 3>(Z * j, 0).transpose() * Fj);
}
} // end of for over cameras
augmentedHessian(aug_m, aug_m)(0, 0) += f;
}
/// *Compute* full augmented information matrix
virtual Matrix augmentedInformation() const {
// Create a SymmetricBlockMatrix
int m = this->keys_.size();
size_t M1 = D * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Do the Schur complement
sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian);
return augmentedHessian.matrix();
}
/// *Compute* full information matrix
virtual Matrix information() const {
throw std::runtime_error(
"RegularImplicitSchurFactor::information non implemented");
return Matrix();
Matrix augmented = augmentedInformation();
int m = this->keys_.size();
size_t M = D * m;
return augmented.block(0,0,M,M);
}
/// Return the diagonal of the Hessian for this factor
@ -142,10 +269,10 @@ public:
Key j = keys_[pos];
// Calculate Fj'*Ej for the current camera (observing a single point)
// D x 3 = (D x 2) * (2 x 3)
// D x 3 = (D x Z) * (Z x 3)
const Matrix2D& Fj = Fblocks_[pos].second;
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<2, 3>(2 * pos, 0);
* E_.block<Z, 3>(Z * pos, 0);
Eigen::Matrix<double, D, 1> dj;
for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@ -174,10 +301,10 @@ public:
Key j = keys_[pos];
// Calculate Fj'*Ej for the current camera (observing a single point)
// D x 3 = (D x 2) * (2 x 3)
// D x 3 = (D x Z) * (Z x 3)
const Matrix2D& Fj = Fblocks_[pos].second;
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<2, 3>(2 * pos, 0);
* E_.block<Z, 3>(Z * pos, 0);
DVector dj;
for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@ -195,28 +322,28 @@ public:
// F'*(I - E*P*E')*F
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos];
// F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
// F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
const Matrix2D& Fj = Fblocks_[pos].second;
// Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
// * E_.block<2, 3>(2 * pos, 0);
// * E_.block<Z, 3>(Z * pos, 0);
// blocks[j] = Fj.transpose() * Fj
// - FtE * PointCovariance_ * FtE.transpose();
const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0);
const Matrix23& Ej = E_.block<Z, 3>(Z * pos, 0);
blocks[j] = Fj.transpose()
* (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
// F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
// static const Eigen::Matrix<double, Z, Z> I2 = eye(Z);
// Matrix2 Q = //
// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
// I2 - E_.block<Z, 3>(Z * pos, 0) * PointCovariance_ * E_.block<Z, 3>(Z * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj;
}
return blocks;
}
virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
return boost::make_shared<RegularImplicitSchurFactor<D, Z> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error(
"RegularImplicitSchurFactor::clone non implemented");
@ -226,7 +353,7 @@ public:
}
virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
return boost::make_shared<RegularImplicitSchurFactor<D, Z> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error(
"RegularImplicitSchurFactor::negate non implemented");
@ -247,23 +374,23 @@ public:
typedef std::vector<Vector2> Error2s;
/**
* @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b)
* @brief Calculate corrected error Q*(e-Z*b) = (I - E*P*E')*(e-Z*b)
*/
void projectError2(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * (e1-2*b) = (3*2m)*2m
// d1 = E.transpose() * (e1-Z*b) = (3*2m)*2m
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); k++)
d1 += E_.block<2, 3>(2 * k, 0).transpose()
* (e1[k] - 2 * b_.segment<2>(k * 2));
d1 += E_.block<Z, 3>(Z * k, 0).transpose()
* (e1[k] - Z * b_.segment<Z>(k * Z));
// d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - 2 * b_.segment<2>(k * 2) - E_.block<2, 3>(2 * k, 0) * d2;
e2[k] = e1[k] - Z * b_.segment<Z>(k * Z) - E_.block<Z, 3>(Z * k, 0) * d2;
}
/*
@ -305,7 +432,7 @@ public:
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment<2>(k * 2);
e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment<Z>(k * Z);
projectError(e1, e2);
double result = 0;
@ -324,14 +451,14 @@ public:
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); k++)
d1 += E_.block<2, 3>(2 * k, 0).transpose() * e1[k];
d1 += E_.block<Z, 3>(Z * k, 0).transpose() * e1[k];
// d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - E_.block<2, 3>(2 * k, 0) * d2;
e2[k] = e1[k] - E_.block<Z, 3>(Z * k, 0) * d2;
}
/// Scratch space for multiplyHessianAdd
@ -426,7 +553,7 @@ public:
e1.resize(size());
e2.resize(size());
for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment<2>(2 * k);
e1[k] = b_.segment<Z>(Z * k);
projectError(e1, e2);
// g = F.transpose()*e2
@ -453,7 +580,7 @@ public:
e1.resize(size());
e2.resize(size());
for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment<2>(2 * k);
e1[k] = b_.segment<Z>(Z * k);
projectError(e1, e2);
for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
@ -472,8 +599,8 @@ public:
// end class RegularImplicitSchurFactor
// traits
template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
RegularImplicitSchurFactor<D> > {
template<size_t Z, size_t D> struct traits<RegularImplicitSchurFactor<D, Z> > : public Testable<
RegularImplicitSchurFactor<D, Z> > {
};
}

155
gtsam/slam/SmartFactorBase.h
View File

@ -337,14 +337,14 @@ public:
double f = computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping);
// we create directly a SymmetricBlockMatrix
// Create a SymmetricBlockMatrix
size_t M1 = Dim * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, Dim);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// build augmented hessian
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
sparseSchurComplement(Fblocks, E, P, b, augmentedHessian);
augmentedHessian(m, m)(0, 0) = f;
@ -352,121 +352,6 @@ public:
augmentedHessian);
}
/**
* Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
* Fast version - works on with sparsity
*/
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size();
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
// Dim = (Dx2) * (2)
// (augmentedHessian.matrix()).block<Dim,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
augmentedHessian(i1, numKeys) = Fi1.transpose()
* b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // Dim = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian(i1, i1) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1, i2) = -Fi1.transpose()
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
}
/**
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
const double f, const FastVector<Key> allKeys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
MatrixDD matrixBlock;
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size(); // cameras observing current point
size_t aug_numKeys = (augmentedHessian.rows() - 1) / Dim; // all cameras in the group
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
// Dim = (DxZDim) * (ZDim)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i1 = this->keys_[i1];
DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]];
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1,
aug_numKeys).knownOffDiagonal()
+ Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // Dim = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i1);
// add contribution of current factor
augmentedHessian(aug_i1, aug_i1) =
matrixBlock
+ (Fi1.transpose()
* (Fi1
- Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1));
// upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
//Key cameraKey_i2 = this->keys_[i2];
DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
// (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_i2) =
augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
- Fi1.transpose()
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
}
/**
* Add the contribution of the smart factor to a pre-allocated Hessian,
* using sparse linear algebra. More efficient than the creation of the
@ -476,33 +361,38 @@ public:
const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian,
const FastVector<Key> allKeys) const {
// int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Vector b;
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping);
updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<Dim,Dim> (i1,i2) = ...
RegularImplicitSchurFactor<Dim, ZDim>::updateSparseSchurComplement(Fblocks,
E, P, b, f, allKeys, keys_, augmentedHessian);
}
/// Whiten the Jacobians computed by computeJacobians using noiseModel_
void whitenJacobians(std::vector<KeyMatrix2D>& F, Matrix& E,
Vector& b) const {
noiseModel_->WhitenSystem(E, b);
// TODO make WhitenInPlace work with any dense matrix type
BOOST_FOREACH(KeyMatrix2D& Fblock,F)
Fblock.second = noiseModel_->Whiten(Fblock.second);
}
/**
* Return Jacobians as RegularImplicitSchurFactor with raw access
*/
boost::shared_ptr<RegularImplicitSchurFactor<Dim> > createRegularImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
std::vector<KeyMatrix2D> F;
boost::shared_ptr<RegularImplicitSchurFactor<Dim, ZDim> > //
createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
double lambda = 0.0, bool diagonalDamping = false) const {
Matrix E;
Vector b;
std::vector<KeyMatrix2D> F;
computeJacobians(F, E, b, cameras, point);
noiseModel_->WhitenSystem(E, b);
whitenJacobians(F, E, b);
Matrix3 P = PointCov(E, lambda, diagonalDamping);
// TODO make WhitenInPlace work with any dense matrix type
BOOST_FOREACH(KeyMatrix2D& Fblock,F)
Fblock.second = noiseModel_->Whiten(Fblock.second);
return boost::make_shared<RegularImplicitSchurFactor<Dim> >(F, E, P, b);
return boost::make_shared<RegularImplicitSchurFactor<Dim, ZDim> >(F, E, P,
b);
}
/**
@ -511,12 +401,11 @@ 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> F;
Matrix E;
Vector b;
std::vector<KeyMatrix2D> F;
computeJacobians(F, E, b, cameras, point);
const size_t M = b.size();
std::cout << M << std::endl;
Matrix3 P = PointCov(E, lambda, diagonalDamping);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(F, E, P, b, n);

1
gtsam/slam/SmartProjectionCameraFactor.h
View File

@ -15,6 +15,7 @@
* @author Luca Carlone
* @author Chris Beall
* @author Zsolt Kira
* @author Frank Dellaert
*/
#pragma once

358
gtsam/slam/SmartProjectionFactor.h
View File

@ -35,14 +35,8 @@ namespace gtsam {
* Structure for storing some state memory, used to speed up optimization
* @addtogroup SLAM
*/
class SmartProjectionFactorState {
struct SmartProjectionFactorState {
protected:
public:
SmartProjectionFactorState() {
}
// Hessian representation (after Schur complement)
bool calculatedHessian;
Matrix H;
@ -68,38 +62,31 @@ private:
protected:
// Some triangulation parameters
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
/// @name Caching triangulation
/// @{
const TriangulationParameters parameters_;
mutable TriangulationResult result_; ///< result from triangulateSafe
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
const bool enableEPI_; ///< if set to true, will refine triangulation using LM
/// @name Caching linearization
/// @{
/// shorthand for smart projection factor state variable
typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
SmartFactorStatePtr state_; ///< cached linearization
const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
mutable Point3 point_; ///< Current estimate of the 3D point
mutable bool degenerate_;
mutable bool cheiralityException_;
// verbosity handling for Cheirality Exceptions
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
boost::shared_ptr<SmartProjectionFactorState> state_;
/// shorthand for smart projection factor state variable
typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
// average reprojection error is smaller than this threshold after triangulation,
// and the factor is disregarded if the error is large
/// @}
public:
@ -117,17 +104,18 @@ public:
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
*/
SmartProjectionFactor(const double rankTol, const double linThreshold,
SmartProjectionFactor(const double rankTolerance, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
SmartFactorStatePtr(new SmartProjectionFactorState())) :
rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_(
manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
dynamicOutlierRejectionThreshold) {
parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
dynamicOutlierRejectionThreshold), //
result_(TriangulationResult::Degenerate()), //
retriangulationThreshold_(1e-5), //
manageDegeneracy_(manageDegeneracy), //
throwCheirality_(false), verboseCheirality_(false), //
state_(state), linearizationThreshold_(linThreshold) {
}
/** Virtual destructor */
@ -141,24 +129,23 @@ public:
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartProjectionFactor, z = \n";
std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
std::cout << "degenerate_ = " << degenerate_ << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
std::cout << s << "SmartProjectionFactor\n";
std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
std::cout << "result:\n" << result_ << std::endl;
Base::print("", keyFormatter);
}
/// Check if the new linearization point_ is the same as the one used for previous triangulation
/// Check if the new linearization point is the same as the one used for previous triangulation
bool decideIfTriangulate(const Cameras& cameras) const {
// several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
// several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
// Note that this is not yet "selecting linearization", that will come later, and we only check if the
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point
size_t m = cameras.size();
bool retriangulate = false;
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
// if we do not have a previous linearization point or the new linearization point includes more poses
if (cameraPosesTriangulation_.empty()
|| cameras.size() != cameraPosesTriangulation_.size())
retriangulate = true;
@ -181,19 +168,19 @@ public:
cameraPosesTriangulation_.push_back(cameras[i].pose());
}
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation
}
/// This function checks if the new linearization point_ is 'close' to the previous one used for linearization
/// This function checks if the new linearization point is 'close' to the previous one used for linearization
bool decideIfLinearize(const Cameras& cameras) const {
// "selective linearization"
// The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
// (we only care about the "rigidity" of the poses, not about their absolute pose)
if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
if (linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
return true;
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
// if we do not have a previous linearization point or the new linearization point includes more poses
if (cameraPosesLinearization_.empty()
|| (cameras.size() != cameraPosesLinearization_.size()))
return true;
@ -211,100 +198,29 @@ public:
Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose());
Pose3 localCameraPoseOld = firstCameraPoseOld.between(
cameraPosesLinearization_[i]);
if (!localCameraPose.equals(localCameraPoseOld,
this->linearizationThreshold_))
if (!localCameraPose.equals(localCameraPoseOld, linearizationThreshold_))
return true; // at least two "relative" poses are different, hence we re-linearize
}
return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize
return false; // if we arrive to this point all poses are the same and we don't need re-linearize
}
/// triangulateSafe
size_t triangulateSafe(const Values& values) const {
return triangulateSafe(this->cameras(values));
}
/// triangulateSafe
size_t triangulateSafe(const Cameras& cameras) const {
TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size();
if (m < 2) { // if we have a single pose the corresponding factor is uninformative
degenerate_ = true;
return m;
}
if (m < 2) // if we have a single pose the corresponding factor is uninformative
return TriangulationResult::Degenerate();
bool retriangulate = decideIfTriangulate(cameras);
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
point_ = triangulatePoint3<CAMERA>(cameras, this->measured_,
rankTolerance_, enableEPI_);
degenerate_ = false;
cheiralityException_ = false;
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i = 0;
BOOST_FOREACH(const CAMERA& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
degenerate_ = true;
break;
}
const Point2& zi = this->measured_.at(i);
try {
Point2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
}
i += 1;
}
// we discard smart factors that have large reprojection error
if (dynamicOutlierRejectionThreshold_ > 0
&& totalReprojError / m > dynamicOutlierRejectionThreshold_)
degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
degenerate_ = true;
cheiralityException_ = false;
} catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
cheiralityException_ = true;
}
}
return m;
if (retriangulate)
result_ = gtsam::triangulateSafe(cameras, this->measured_, parameters_);
return result_;
}
/// triangulate
bool triangulateForLinearize(const Cameras& cameras) const {
bool isDebug = false;
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) {
std::cout
<< "createRegularImplicitSchurFactor: degenerate configuration"
<< std::endl;
}
return false;
} else {
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
}
return true;
}
triangulateSafe(cameras); // imperative, might reset result_
return (manageDegeneracy_ || result_);
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
@ -324,12 +240,10 @@ public:
exit(1);
}
this->triangulateSafe(cameras);
triangulateSafe(cameras);
if (numKeys < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
// std::cout << "In linearize: exception" << std::endl;
if (!manageDegeneracy_ && !result_) {
// put in "empty" Hessian
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs)
@ -338,23 +252,19 @@ public:
Gs, gs, 0.0);
}
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
}
// decide whether to re-linearize
bool doLinearize = this->decideIfLinearize(cameras);
if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
if (linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
for (size_t i = 0; i < cameras.size(); i++)
this->cameraPosesLinearization_[i] = cameras[i].pose();
if (!doLinearize) { // return the previous Hessian factor
std::cout << "=============================" << std::endl;
std::cout << "doLinearize " << doLinearize << std::endl;
std::cout << "this->linearizationThreshold_ "
<< this->linearizationThreshold_ << std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
std::cout << "linearizationThreshold_ " << linearizationThreshold_
<< std::endl;
std::cout << "valid: " << isValid() << std::endl;
std::cout
<< "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled"
<< std::endl;
@ -370,6 +280,7 @@ public:
{
std::vector<typename Base::KeyMatrix2D> Fblocks;
f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
Base::whitenJacobians(Fblocks,E,b);
Base::FillDiagonalF(Fblocks, F); // expensive !
}
@ -404,7 +315,7 @@ public:
}
}
// ==================================================================
if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
if (linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
this->state_->Gs = Gs;
this->state_->gs = gs;
this->state_->f = f;
@ -417,7 +328,7 @@ public:
boost::shared_ptr<RegularImplicitSchurFactor<Base::Dim> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
else
return boost::shared_ptr<RegularImplicitSchurFactor<Base::Dim> >();
}
@ -426,7 +337,7 @@ public:
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda);
return Base::createJacobianQFactor(cameras, *result_, lambda);
else
return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
}
@ -434,63 +345,27 @@ public:
/// Create a factor, takes values
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Values& values, double lambda) const {
Cameras cameras;
// TODO triangulate twice ??
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
return createJacobianQFactor(cameras, lambda);
else
return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
return createJacobianQFactor(this->cameras(values), lambda);
}
/// different (faster) way to compute Jacobian factor
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
return Base::createJacobianSVDFactor(cameras, *result_, lambda);
else
return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
}
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& cameras) const {
Values valuesFactor;
// Select only the cameras
BOOST_FOREACH(const Key key, this->keys_)
valuesFactor.insert(key, values.at(key));
cameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_)))
return false;
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
if (this->degenerate_) {
std::cout << "SmartProjectionFactor: this is not ready" << std::endl;
std::cout << "this->cheiralityException_ " << this->cheiralityException_
<< std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
}
return true;
}
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
cameras.project2(point_, boost::none, E);
cameras.project2(*result_, boost::none, E);
return nonDegenerate;
}
@ -501,31 +376,18 @@ public:
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const {
if (this->degenerate_) {
std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
std::cout << "point " << point_ << std::endl;
std::cout
<< "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used"
<< std::endl;
if (Base::Dim > 6) {
std::cout
<< "Management of degeneracy is not yet ready when one also optimizes for the calibration "
<< std::endl;
}
if (!result_) {
// TODO Luca clarify whether this works or not
result_ = TriangulationResult(
cameras[0].backprojectPointAtInfinity(this->measured_.at(0)));
// TODO replace all this by Call to CameraSet
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
this->point_ = cameras[i].backprojectPointAtInfinity(
this->measured_.at(i));
// 3D parametrization of point at infinity: [px py 1]
}
Matrix Fi, Ei;
Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
Vector bi = -(cameras[i].projectPointAtInfinity(*result_, Fi, Ei)
- this->measured_.at(i)).vector();
f += bi.squaredNorm();
@ -535,17 +397,17 @@ public:
}
return f;
} else {
// nondegenerate: just return Base version
return Base::computeJacobians(Fblocks, E, b, cameras, point_);
} // end else
// valid result: just return Base version
return Base::computeJacobians(Fblocks, E, b, cameras, *result_);
}
}
/// 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);
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
return nonDegenerate;
@ -555,19 +417,19 @@ public:
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)
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
return true;
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
return nonDegenerate;
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
return Base::reprojectionError(cameras, point_);
return Base::reprojectionError(cameras, *result_);
else
return zero(cameras.size() * 2);
}
@ -581,65 +443,61 @@ public:
double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const {
size_t nrCameras;
if (externalPoint) {
nrCameras = this->keys_.size();
point_ = *externalPoint;
degenerate_ = false;
cheiralityException_ = false;
} else {
nrCameras = this->triangulateSafe(cameras);
}
if (externalPoint)
result_ = TriangulationResult(*externalPoint);
else
result_ = triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
// if we don't want to manage the exceptions we discard the factor
// std::cout << "In error evaluation: exception" << std::endl;
// if we don't want to manage the exceptions we discard the factor
if (!manageDegeneracy_ && !result_)
return 0.0;
}
if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
if (isPointBehindCamera()) { // if we want to manage the exceptions with rotation-only factors
std::cout
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
<< std::endl;
this->degenerate_ = true;
}
if (this->degenerate_) {
if (isDegenerate()) {
// return 0.0; // TODO: this maybe should be zero?
std::cout
<< "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
<< std::endl;
// 3D parameterization of point at infinity
const Point2& zi = this->measured_.at(0);
this->point_ = cameras.front().backprojectPointAtInfinity(zi);
return Base::totalReprojectionErrorAtInfinity(cameras, this->point_);
const Point2& z0 = this->measured_.at(0);
result_ = TriangulationResult(
cameras.front().backprojectPointAtInfinity(z0));
return Base::totalReprojectionErrorAtInfinity(cameras, *result_);
} else {
// Just use version in base class
return Base::totalReprojectionError(cameras, point_);
return Base::totalReprojectionError(cameras, *result_);
}
}
/** return the landmark */
boost::optional<Point3> point() const {
return point_;
TriangulationResult point() const {
return result_;
}
/** COMPUTE the landmark */
boost::optional<Point3> point(const Values& values) const {
triangulateSafe(values);
return point_;
TriangulationResult point(const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateSafe(cameras);
}
/// Is result valid?
inline bool isValid() const {
return result_;
}
/** return the degenerate state */
inline bool isDegenerate() const {
return (cheiralityException_ || degenerate_);
return result_.degenerate();
}
/** return the cheirality status flag */
inline bool isPointBehindCamera() const {
return cheiralityException_;
return result_.behindCamera();
}
/** return cheirality verbosity */

12
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -255,6 +255,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
// augmentedInformation (test just checks diagonals)
Matrix actualInfo = factor.augmentedInformation();
EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0)));
EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6)));
EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12)));
// information (test just checks diagonals)
Matrix actualInfo2 = factor.information();
EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0)));
EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6)));
EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12)));
}
/* ************************************************************************* */

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

@ -15,6 +15,7 @@
* @author Chris Beall
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
* @date Sept 2013
*/
@ -133,9 +134,8 @@ TEST( SmartProjectionCameraFactor, noisy ) {
using namespace vanilla;
// 1. Project two landmarks into two cameras and triangulate
Point2 pixelError(0.2, 0.2);
Point2 level_uv = level_camera.project(landmark1) + pixelError;
// Project one landmark into two cameras and add noise on first
Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2);
Point2 level_uv_right = level_camera_right.project(landmark1);
Values values;
@ -147,7 +147,24 @@ TEST( SmartProjectionCameraFactor, noisy ) {
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
// Point is now uninitialized before a triangulation event
EXPECT(!factor1->point());
double expectedError = 58640;
double actualError1 = factor1->error(values);
EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1);
// Check triangulated point
CHECK(factor1->point());
EXPECT(assert_equal(Point3(13.7587, 1.43851, -1.14274),*factor1->point(), 1e-4));
// Check whitened errors
Vector expected(4);
expected << -7, 235, 58, -242;
SmartFactor::Cameras cameras1 = factor1->cameras(values);
Point3 point1 = *factor1->point();
Vector actual = factor1->whitenedErrors(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1));
SmartFactor::shared_ptr factor2(new SmartFactor());
vector<Point2> measurements;
@ -165,8 +182,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
factor2->add(measurements, views, noises);
double actualError2 = factor2->error(values);
DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
}
/* *************************************************************************/
@ -174,57 +190,81 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
using namespace vanilla;
// Project three landmarks into three cameras
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create and fill smartfactors
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
// Create factor graph and add priors on two cameras
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
Values values;
values.insert(c1, cam1);
values.insert(c2, cam2);
values.insert(c3, perturbCameraPose(cam3));
// Create initial estimate
Values initial;
initial.insert(c1, cam1);
initial.insert(c2, cam2);
initial.insert(c3, perturbCameraPose(cam3));
if (isDebugTest)
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
initial.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
// Points are now uninitialized before a triangulation event
EXPECT(!smartFactor1->point());
EXPECT(!smartFactor2->point());
EXPECT(!smartFactor3->point());
EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1);
EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1);
EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1);
// Error should trigger this and initialize the points, abort if not so
CHECK(smartFactor1->point());
CHECK(smartFactor2->point());
CHECK(smartFactor3->point());
EXPECT(assert_equal(Point3(2.57696, -0.182566, 1.04085),*smartFactor1->point(), 1e-4));
EXPECT(assert_equal(Point3(2.80114, -0.702153, 1.06594),*smartFactor2->point(), 1e-4));
EXPECT(assert_equal(Point3(1.82593, -0.289569, 2.13438),*smartFactor3->point(), 1e-4));
// Check whitened errors
Vector expected(6);
expected << 256, 29, -26, 29, -206, -202;
SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
Point3 point1 = *smartFactor1->point();
Vector actual = smartFactor1->whitenedErrors(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1));
// Optimize
LevenbergMarquardtParams params;
if (isDebugTest)
if (isDebugTest) {
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
params.verbosity = NonlinearOptimizerParams::ERROR;
}
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
Values result = optimizer.optimize();
Values result;
gttic_(SmartProjectionCameraFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionCameraFactor);
tictoc_finishedIteration_();
EXPECT(assert_equal(landmark1,*smartFactor1->point(), 1e-7));
EXPECT(assert_equal(landmark2,*smartFactor2->point(), 1e-7));
EXPECT(assert_equal(landmark3,*smartFactor3->point(), 1e-7));
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
// VectorValues delta = GFG->optimize();
if (isDebugTest)
@ -243,8 +283,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
using namespace vanilla;
// Project three landmarks into three cameras
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
@ -300,11 +340,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_(SmartProjectionCameraFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionCameraFactor);
tictoc_finishedIteration_();
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
// VectorValues delta = GFG->optimize();
@ -383,11 +420,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_(SmartProjectionCameraFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionCameraFactor);
tictoc_finishedIteration_();
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
// VectorValues delta = GFG->optimize();
@ -465,11 +499,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_(SmartProjectionCameraFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionCameraFactor);
tictoc_finishedIteration_();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
@ -544,11 +575,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_(SmartProjectionCameraFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionCameraFactor);
tictoc_finishedIteration_();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");

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

@ -15,6 +15,7 @@
* @author Chris Beall
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
* @date Sept 2013
*/
@ -38,7 +39,7 @@ static const bool manageDegeneracy = true;
// Create a noise model for the pixel error
static const double sigma = 0.1;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, sigma));
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
// Convenience for named keys
using symbol_shorthand::X;
@ -289,7 +290,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
cameras.push_back(cam2);
// Make sure triangulation works
LONGS_EQUAL(2, smartFactor1->triangulateSafe(cameras));
CHECK(smartFactor1->triangulateSafe(cameras));
CHECK(!smartFactor1->isDegenerate());
CHECK(!smartFactor1->isPointBehindCamera());
boost::optional<Point3> p = smartFactor1->point();
@ -298,8 +299,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// After eliminating the point, A1 and A2 contain 2-rank information on cameras:
Matrix16 A1, A2;
A1 << -1000, 0, 0, 0, 100, 0;
A2 << 1000, 0, 100, 0, -100, 0;
A1 << -10, 0, 0, 0, 1, 0;
A2 << 10, 0, 1, 0, -1, 0;
A1 *= 10. / sigma;
A2 *= 10. / sigma;
Matrix expectedInformation; // filled below
{
// createHessianFactor
Matrix66 G11 = 0.5 * A1.transpose() * A1;
@ -314,10 +318,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
double f = 0;
RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
expectedInformation = expected.information();
boost::shared_ptr<RegularHessianFactor<6> > actual =
smartFactor1->createHessianFactor(cameras, 0.0);
EXPECT(assert_equal(expected.information(), actual->information(), 1e-8));
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
EXPECT(assert_equal(expected, *actual, 1e-8));
}
@ -336,36 +341,45 @@ TEST( SmartProjectionPoseFactor, Factors ) {
F2(1, 0) = 100;
F2(1, 2) = 10;
F2(1, 4) = -10;
Matrix43 E;
Matrix E(4, 3);
E.setZero();
E(0, 0) = 100;
E(1, 1) = 100;
E(2, 0) = 100;
E(2, 2) = 10;
E(3, 1) = 100;
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
E(0, 0) = 10;
E(1, 1) = 10;
E(2, 0) = 10;
E(2, 2) = 1;
E(3, 1) = 10;
vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(x1, F1))(make_pair(x2, F2));
Matrix3 P = (E.transpose() * E).inverse();
Vector4 b;
Vector b(4);
b.setZero();
// createJacobianQFactor
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
Matrix3 P = (E.transpose() * E).inverse();
JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n);
EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
smartFactor1->createJacobianQFactor(cameras, 0.0);
CHECK(actualQ);
EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8));
EXPECT(assert_equal(expectedQ, *actualQ));
// Whiten for RegularImplicitSchurFactor (does not have noise model)
model->WhitenSystem(E, b);
Matrix3 whiteP = (E.transpose() * E).inverse();
BOOST_FOREACH(SmartFactor::KeyMatrix2D& Fblock,Fblocks)
Fblock.second = model->Whiten(Fblock.second);
// createRegularImplicitSchurFactor
RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
RegularImplicitSchurFactor<6> expected(Fblocks, E, whiteP, b);
boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
CHECK(actual);
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
EXPECT(assert_equal(expected, *actual));
// createJacobianQFactor
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);
CHECK(actual);
EXPECT(assert_equal(expectedQ.information(), actualQ->information(), 1e-8));
EXPECT(assert_equal(expectedQ, *actualQ));
}
{
@ -374,11 +388,12 @@ TEST( SmartProjectionPoseFactor, Factors ) {
b.setZero();
double s = sin(M_PI_4);
JacobianFactor expected(x1, s * A1, x2, s * A2, b);
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
boost::shared_ptr<JacobianFactor> actual =
smartFactor1->createJacobianSVDFactor(cameras, 0.0);
CHECK(actual);
EXPECT(assert_equal(expected.information(), actual->information(), 1e-8));
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
EXPECT(assert_equal(expected, *actual));
}
}
@ -976,7 +991,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
values.insert(x1, cam2);
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose,sharedK));
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
@ -1070,9 +1085,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues;
rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK));
rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK));
rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK));
rotValues.insert(x1, Camera(poseDrift.compose(level_pose), sharedK));
rotValues.insert(x2, Camera(poseDrift.compose(pose_right), sharedK));
rotValues.insert(x3, Camera(poseDrift.compose(pose_above), sharedK));
boost::shared_ptr<GaussianFactor> hessianFactorRot =
smartFactorInstance->linearize(rotValues);
@ -1086,9 +1101,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
Point3(10, -4, 5));
Values tranValues;
tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK));
tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK));
tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK));
tranValues.insert(x1, Camera(poseDrift2.compose(level_pose), sharedK));
tranValues.insert(x2, Camera(poseDrift2.compose(pose_right), sharedK));
tranValues.insert(x3, Camera(poseDrift2.compose(pose_above), sharedK));
boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactorInstance->linearize(tranValues);
@ -1130,9 +1145,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues;
rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK2));
rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK2));
rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK2));
rotValues.insert(x1, Camera(poseDrift.compose(level_pose), sharedK2));
rotValues.insert(x2, Camera(poseDrift.compose(pose_right), sharedK2));
rotValues.insert(x3, Camera(poseDrift.compose(pose_above), sharedK2));
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
rotValues);
@ -1148,9 +1163,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
Point3(10, -4, 5));
Values tranValues;
tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK2));
tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK2));
tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK2));
tranValues.insert(x1, Camera(poseDrift2.compose(level_pose), sharedK2));
tranValues.insert(x2, Camera(poseDrift2.compose(pose_right), sharedK2));
tranValues.insert(x3, Camera(poseDrift2.compose(pose_above), sharedK2));
boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactor->linearize(tranValues);
@ -1230,7 +1245,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
values.insert(x1, cam2);
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose,sharedBundlerK));
values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
@ -1336,7 +1351,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
values.insert(x1, cam2);
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose,sharedBundlerK));
values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");