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

implemented efficient update of Hessian matrix via Schur complement

release/4.3a0
Luca 2014-04-09 21:01:03 -04:00
parent d8fafce224
commit dc7b5d58c0
1 changed files with 147 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

266
gtsam_unstable/slam/SmartFactorBase.h
View File

@ -192,7 +192,7 @@ public:
b[2 * i + 1] = e.y(); b[2 * i + 1] = e.y();
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE); exit(EXIT_FAILURE);
} }
i += 1; i += 1;
} }
@ -219,10 +219,10 @@ public:
try { try {
Point2 reprojectionError(camera.project(point) - zi); Point2 reprojectionError(camera.project(point) - zi);
overallError += 0.5 overallError += 0.5
* this->noise_.at(i)->distance(reprojectionError.vector()); * this->noise_.at(i)->distance(reprojectionError.vector());
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE); exit(EXIT_FAILURE);
} }
i += 1; i += 1;
} }
@ -244,7 +244,7 @@ public:
cameras[i].project(point, boost::none, Ei); cameras[i].project(point, boost::none, Ei);
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE); exit(EXIT_FAILURE);
} }
this->noise_.at(i)->WhitenSystem(Ei, b); this->noise_.at(i)->WhitenSystem(Ei, b);
E.block<2, 3>(2 * i, 0) = Ei; E.block<2, 3>(2 * i, 0) = Ei;
@ -274,7 +274,7 @@ public:
-(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE); exit(EXIT_FAILURE);
} }
this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi);
@ -322,7 +322,7 @@ public:
const double lambda = 0.0) const { const double lambda = 0.0) const {
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda); lambda);
F = zeros(2 * numKeys, D * numKeys); F = zeros(2 * numKeys, D * numKeys);
@ -345,7 +345,7 @@ public:
diagonalDamping); // diagonalDamping should have no effect (only on PointCov) diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
// Do SVD on A // Do SVD on A
Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU); Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues(); Vector s = svd.singularValues();
// Enull = zeros(2 * numKeys, 2 * numKeys - 3); // Enull = zeros(2 * numKeys, 2 * numKeys - 3);
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
@ -361,7 +361,7 @@ public:
const Cameras& cameras, const Point3& point) const { const Cameras& cameras, const Point3& point) const {
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
F.resize(2 * numKeys, D * numKeys); F.resize(2 * numKeys, D * numKeys);
F.setZero(); F.setZero();
@ -380,14 +380,14 @@ public:
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks; std::vector<KeyMatrix2D> Fblocks;
Matrix E; Matrix E;
Matrix3 PointCov; Matrix3 PointCov;
Vector b; Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); diagonalDamping);
//#define HESSIAN_BLOCKS //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
#ifdef HESSIAN_BLOCKS #ifdef HESSIAN_BLOCKS
// Create structures for Hessian Factors // Create structures for Hessian Factors
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
@ -400,46 +400,23 @@ public:
//std::vector < Vector > gs2(gs.begin(), gs.end()); //std::vector < Vector > gs2(gs.begin(), gs.end());
return boost::make_shared < RegularHessianFactor<D> return boost::make_shared < RegularHessianFactor<D>
> (this->keys_, Gs, gs, f); > (this->keys_, Gs, gs, f);
#else #else // we create directly a SymmetricBlockMatrix
size_t n1 = D * numKeys + 1; size_t n1 = D * numKeys + 1;
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D); std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1; dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ... sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
augmentedHessian(numKeys,numKeys)(0,0) = f; augmentedHessian(numKeys, numKeys)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<D> >( return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
this->keys_, augmentedHessian); augmentedHessian);
#endif #endif
} }
// **************************************************************************************************** // ****************************************************************************************************
void updateAugmentedHessian( // slow version - works on full (sparse) matrices
const Cameras& cameras, const Point3& point, const double lambda,
bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const {
int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
// std::cout << "f "<< f <<std::endl;
//augmentedHessian(numKeys,numKeys)(0,0) += f;
}
// ****************************************************************************************************
void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E, void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix& PointCov, const Vector& b, const Matrix& PointCov, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const { /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
@ -466,7 +443,7 @@ public:
int GsCount2 = 0; int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * D; DenseIndex i1D = i1 * D;
gs.at(i1) = gs_vector.segment < D > (i1D); gs.at(i1) = gs_vector.segment<D>(i1D);
for (DenseIndex i2 = 0; i2 < numKeys; i2++) { for (DenseIndex i2 = 0; i2 < numKeys; i2++) {
if (i2 >= i1) { if (i2 >= i1) {
Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D); Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
@ -476,69 +453,6 @@ public:
} }
} }
// ****************************************************************************************************
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& 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(); // cameras observing current point
size_t aug_numKeys = (augmentedHessian.rows() - 1)/D; // all cameras in the group
// MatrixDD delta = eye(D);
// size_t n1 = numKeys+1;
// for (size_t i1=0; i1 < n1-1; i1++){
// MatrixDD Z1 = augmentedHessian(i1,i1).selfadjointView();
// std::cout << i1 << " " << "\n" << Z1 << std::endl;
// augmentedHessian(i1,i1) = Z1 + delta;
// MatrixDD Z2 = augmentedHessian(i1,i1).selfadjointView();
// std::cout << i1 << " " << "\n" << Z2 << std::endl;
//// for (size_t i2=i1+1; i2 < n1-1; i2++){
//// Z = augmentedHessian(i1,i2).knownOffDiagonal(); // + delta;
//// std::cout << i1 << " " << i2 << "\n" << Z << std::endl;
//// }
// }
MatrixDD matrixBlock;
VectorD vectorBlock;
// 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<2, 3>(2 * i1, 0) * P;
// D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
size_t aug_i1 = this->keys_[i1];
// augmentedHessian(aug_i1,aug_numKeys)
vectorBlock = augmentedHessian(aug_i1,aug_numKeys).knownOffDiagonal();
augmentedHessian(aug_i1,aug_numKeys) = vectorBlock +
Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
matrixBlock = augmentedHessian(aug_i1,aug_i1).selfadjointView();
augmentedHessian(aug_i1,aug_i1) = matrixBlock+
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * 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;
size_t aug_i2 = this->keys_[i2];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
augmentedHessian(aug_i1, aug_i2) = matrixBlock
- Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
}
// **************************************************************************************************** // ****************************************************************************************************
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks, void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
@ -558,20 +472,20 @@ public:
// D = (Dx2) * (2) // D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b // (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
augmentedHessian(i1,i1) = augmentedHessian(i1, i1) = Fi1.transpose()
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian // upper triangular part of the hessian
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second; const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) // (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1,i2) = augmentedHessian(i1, i2) = -Fi1.transpose()
-Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
} }
} // end of for over cameras } // end of for over cameras
} }
@ -602,24 +516,138 @@ public:
{ // for i1 = i2 { // for i1 = i2
// D = (Dx2) * (2) // D = (Dx2) * (2)
gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
// D = (Dx2) * (2x3) * (3*2m) * (2m x 1) -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b));
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); Gs.at(GsIndex) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
GsIndex++; GsIndex++;
} }
// upper triangular part of the hessian // upper triangular part of the hessian
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second; const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) // (DxD) = (Dx2) * ( (2x2) * (2xD) )
Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); Gs.at(GsIndex) = -Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
GsIndex++; GsIndex++;
} }
} // end of for over cameras } // end of for over cameras
} }
// ****************************************************************************************************
void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian,
const FastVector<Key> allKeys) const {
// int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
}
// ****************************************************************************************************
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& 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;
VectorD vectorBlock;
FastMap<Key,size_t> KeySlotMap;
for (size_t slot=0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair<Key,size_t>(allKeys[slot],slot));
bool debug= false;
// 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) / D; // 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<2, 3>(2 * i1, 0) * P;
// D = (Dx2) * (2)
// 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];
size_t aug_i1 = KeySlotMap[cameraKey_i1];
// information vector - store previous vector
vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_numKeys << ")= \n" <<
vectorBlock << std::endl;
// add contribution of current factor
augmentedHessian(aug_i1, aug_numKeys) = vectorBlock
+ Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_numKeys << ")= \n" <<
vectorBlock << std::endl;
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i1);
if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_i1 << ")= \n" <<
matrixBlock << std::endl;
// add contribution of current factor
augmentedHessian(aug_i1, aug_i1) = matrixBlock
+ Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
matrixBlock = augmentedHessian(aug_i1, aug_i1);
if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_i1 << ")= \n" <<
matrixBlock << std::endl;
// 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];
size_t aug_i2 = KeySlotMap[cameraKey_i2];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
// off diagonal block - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
if(debug) std::cout << "(aug_i1= " << aug_i1 << ", aug_i2= " << aug_i2 << ") (i2= " <<i2 << ", aug_i2=" << aug_i2 << ")" << std::endl;
if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" <<
augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() << std::endl;
// add contribution of current factor
augmentedHessian(aug_i1, aug_i2) = matrixBlock
- Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" <<
augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() << std::endl;
matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
if(debug) std::cout << "(after, after) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" <<
matrixBlock<< std::endl;
}
} // end of for over cameras
augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
}
// **************************************************************************************************** // ****************************************************************************************************
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor( boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
@ -636,13 +664,13 @@ public:
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
std::vector < KeyMatrix2D > Fblocks; std::vector<KeyMatrix2D> Fblocks;
Matrix E; Matrix E;
Matrix3 PointCov; Matrix3 PointCov;
Vector b; Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); diagonalDamping);
return boost::make_shared < JacobianFactorQ<D> > (Fblocks, E, PointCov, b); return boost::make_shared<JacobianFactorQ<D> >(Fblocks, E, PointCov, b);
} }
private: private: