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

Fully compiles now

release/4.3a0
dellaert 2015-03-06 08:48:35 -08:00
parent 88d8543f12
commit faadf5b06f
4 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/slam/JacobianFactorQ.h
View File

@ -65,7 +65,7 @@ public:
for (size_t k = 0; k < FBlocks.size(); ++k) { for (size_t k = 0; k < FBlocks.size(); ++k) {
Key key = keys[k]; Key key = keys[k];
QF.push_back( QF.push_back(
KeyMatrix(key, Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k])); KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
} }
// Which is then passed to the normal JacobianFactor constructor // Which is then passed to the normal JacobianFactor constructor
JacobianFactor::fillTerms(QF, - Q * b, model); JacobianFactor::fillTerms(QF, - Q * b, model);

2
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -160,7 +160,7 @@ TEST (EssentialMatrixFactor2, factor) {
// Check evaluation // Check evaluation
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pi = SimpleCamera::project_to_camera(P2); const Point2 pi = PinholeBase::Project(P2);
Point2 reprojectionError(pi - pB(i)); Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();

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

@ -160,10 +160,10 @@ TEST( SmartProjectionCameraFactor, noisy ) {
// Check whitened errors // Check whitened errors
Vector expected(4); Vector expected(4);
expected << -7, 235, 58, -242; expected << 7, -235, -58, 242;
SmartFactor::Cameras cameras1 = factor1->cameras(values); SmartFactor::Cameras cameras1 = factor1->cameras(values);
Point3 point1 = *factor1->point(); Point3 point1 = *factor1->point();
Vector actual = factor1->whitenedErrors(cameras1, point1); Vector actual = factor1->whitenedError(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1)); EXPECT(assert_equal(expected, actual, 1));
SmartFactor::shared_ptr factor2(new SmartFactor()); SmartFactor::shared_ptr factor2(new SmartFactor());
@ -245,10 +245,10 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
// Check whitened errors // Check whitened errors
Vector expected(6); Vector expected(6);
expected << 256, 29, -26, 29, -206, -202; expected << -256, -29, 26, -29, 206, 202;
SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial); SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
Point3 point1 = *smartFactor1->point(); Point3 point1 = *smartFactor1->point();
Vector actual = smartFactor1->whitenedErrors(cameras1, point1); Vector actual = smartFactor1->whitenedError(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1)); EXPECT(assert_equal(expected, actual, 1));
// Optimize // Optimize

9
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -415,7 +415,7 @@ public:
std::vector<typename Base::MatrixZD> Fblocks; std::vector<typename Base::MatrixZD> Fblocks;
Matrix F, E; Matrix F, E;
Vector b; Vector b;
double f = computeJacobians(Fblocks, E, b, cameras); computeJacobians(Fblocks, E, b, cameras);
Base::FillDiagonalF(Fblocks, F); // expensive !!! Base::FillDiagonalF(Fblocks, F); // expensive !!!
// Schur complement trick // Schur complement trick
@ -446,6 +446,7 @@ public:
} }
} }
// ================================================================== // ==================================================================
double f = b.squaredNorm();
if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables if (this->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_->gs = gs; this->state_->gs = gs;
@ -549,7 +550,7 @@ public:
/// Compute F, E only (called below in both vanilla and SVD versions) /// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
double computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks, void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Cameras& cameras) const { Matrix& E, Vector& b, const Cameras& cameras) const {
if (this->degenerate_) { if (this->degenerate_) {
throw("FIXME: computeJacobians degenerate case commented out!"); throw("FIXME: computeJacobians degenerate case commented out!");
@ -587,7 +588,7 @@ public:
// return f; // return f;
} else { } else {
// nondegenerate: just return Base version // nondegenerate: just return Base version
return Base::computeJacobians(Fblocks, E, b, cameras, point_); Base::computeJacobians(Fblocks, E, b, cameras, point_);
} // end else } // end else
} }
@ -607,7 +608,7 @@ public:
Cameras cameras; Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate) if (nonDegenerate)
return Base::reprojectionError(cameras, point_); return Base::unwhitenedError(cameras, point_);
else else
return zero(cameras.size() * 3); return zero(cameras.size() * 3);
} }