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

Switched to (i,j) instead of (i1,i2)

release/4.3a0
dellaert 2015-03-01 14:28:13 +01:00
parent 575a06b042
commit 6a024259e5
2 changed files with 36 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
gtsam/slam/SmartFactorBase.h
View File

@ -367,28 +367,27 @@ public:
size_t numKeys = this->keys_.size();
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
for (size_t i = 0; i < numKeys; i++) { // for each camera
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
const Matrix2D& Fi = Fblocks.at(i).second;
const Matrix23 Ei_P = E.block<ZDim, 3>(ZDim * i, 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)
// (augmentedHessian.matrix()).block<Dim,1> (i,numKeys+1) = Fi.transpose() * b.segment < 2 > (2 * i); // F' * b
augmentedHessian(i, numKeys) = Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
- Fi.transpose() * (Ei_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);
augmentedHessian(i, i) = Fi.transpose()
* (Fi - Ei_P * E.block<ZDim, 3>(ZDim * i, 0).transpose() * Fi);
// 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;
for (size_t j = i + 1; j < numKeys; j++) { // for each camera
const Matrix2D& Fj = Fblocks.at(j).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1, i2) = -Fi1.transpose()
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
augmentedHessian(i, j) = -Fi.transpose()
* (Ei_P * E.block<ZDim, 3>(ZDim * j, 0).transpose() * Fj);
}
} // end of for over cameras
}
@ -417,50 +416,48 @@ public:
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
for (size_t i = 0; i < numKeys; i++) { // 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;
const Matrix2D& Fi = Fblocks.at(i).second;
const Matrix23 Ei_P = E.block<ZDim, 3>(ZDim * i, 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]];
// Key cameraKey_i = this->keys_[i];
DenseIndex aug_i = KeySlotMap[this->keys_[i]];
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
// vectorBlock = augmentedHessian(aug_i, aug_numKeys).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1,
augmentedHessian(aug_i, aug_numKeys) = augmentedHessian(aug_i,
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)
+ Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
- Fi.transpose() * (Ei_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);
matrixBlock = augmentedHessian(aug_i, aug_i);
// 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));
augmentedHessian(aug_i, aug_i) = matrixBlock
+ (Fi.transpose()
* (Fi - Ei_P * E.block<ZDim, 3>(ZDim * i, 0).transpose() * Fi));
// 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;
for (size_t j = i + 1; j < numKeys; j++) { // for each camera
const Matrix2D& Fj = Fblocks.at(j).second;
//Key cameraKey_i2 = this->keys_[i2];
DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
//Key cameraKey_j = this->keys_[j];
DenseIndex aug_j = KeySlotMap[this->keys_[j]];
// (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
// matrixBlock = augmentedHessian(aug_i, aug_j).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);
augmentedHessian(aug_i, aug_j) =
augmentedHessian(aug_i, aug_j).knownOffDiagonal()
- Fi.transpose()
* (Ei_P * E.block<ZDim, 3>(ZDim * j, 0).transpose() * Fj);
}
} // end of for over cameras
@ -484,7 +481,7 @@ public:
Vector b;
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) = ...
updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<Dim,Dim> (i,j) = ...
}
/// Whiten the Jacobians computed by computeJacobians using noiseModel_

1
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
*/