Switched to (i,j) instead of (i1,i2)
parent
575a06b042
commit
6a024259e5
|
@ -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_
|
||||
|
|
|
@ -15,6 +15,7 @@
|
|||
* @author Chris Beall
|
||||
* @author Luca Carlone
|
||||
* @author Zsolt Kira
|
||||
* @author Frank Dellaert
|
||||
* @date Sept 2013
|
||||
*/
|
||||
|
||||
|
|
Loading…
Reference in New Issue