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

still segfaults

release/4.3a0
lcarlone 2021-03-14 22:43:53 -04:00
parent e571d2c188
commit 6d118da82d
2 changed files with 30 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/CameraSet.h
View File

@ -146,13 +146,14 @@ public:
* Fixed size version * Fixed size version
*/ */
template<int N, int ND> // N = 2 or 3, ND is the camera dimension template<int N, int ND> // N = 2 or 3, ND is the camera dimension
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, static SymmetricBlockMatrix SchurComplement(
const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) { const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
// a single point is observed in m cameras // a single point is observed in m cameras
size_t m = Fs.size(); size_t m = Fs.size();
// Create a SymmetricBlockMatrix // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
size_t M1 = ND * m + 1; size_t M1 = ND * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, ND); std::fill(dims.begin(), dims.end() - 1, ND);
@ -162,7 +163,7 @@ public:
// Blockwise Schur complement // Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& Fi = Fs[i]; const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose(); const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = // const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P; E.block(ZDim * i, 0, ZDim, N) * P;
@ -177,7 +178,7 @@ public:
// upper triangular part of the hessian // upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j]; const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) // (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT augmentedHessian.setOffDiagonalBlock(i, j, -FiT

38
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -181,7 +181,14 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
const Values& values, const double lambda = 0.0, bool diagonalDamping = const Values& values, const double lambda = 0.0, bool diagonalDamping =
false) const { false) const {
size_t numKeys = this->keys_.size(); KeyVector allKeys; // includes body poses and *unique* extrinsic poses
allKeys.insert(allKeys.end(), this->keys_.begin(), this->keys_.end());
KeyVector sorted_body_P_cam_keys(body_P_cam_keys_); // make a copy that we can edit
std::sort(sorted_body_P_cam_keys.begin(), sorted_body_P_cam_keys.end()); // required by unique
std::unique(sorted_body_P_cam_keys.begin(), sorted_body_P_cam_keys.end());
allKeys.insert(allKeys.end(), sorted_body_P_cam_keys.begin(), sorted_body_P_cam_keys.end());
size_t numKeys = allKeys.size();
// Create structures for Hessian Factors // Create structures for Hessian Factors
KeyVector js; KeyVector js;
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2); std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
@ -202,7 +209,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
m = Matrix::Zero(Dim,Dim); m = Matrix::Zero(Dim,Dim);
for(Vector& v: gs) for(Vector& v: gs)
v = Vector::Zero(Dim); v = Vector::Zero(Dim);
return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_, return boost::make_shared<RegularHessianFactor<Dim> >(allKeys,
Gs, gs, 0.0); Gs, gs, 0.0);
} }
@ -214,17 +221,22 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
computeJacobiansWithTriangulatedPoint(Fs, E, b, values); computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
std::cout << "Fs.at(0) \n"<< Fs.at(0) << std::endl; std::cout << "Fs.at(0) \n"<< Fs.at(0) << std::endl;
// // Whiten using noise model std::cout << "Dim "<< Dim << std::endl;
// Base::whitenJacobians(Fs, E, b); std::cout << "numKeys "<< numKeys << std::endl;
//
// // build augmented hessian // Whiten using noise model
// SymmetricBlockMatrix augmentedHessian = // noiseModel_->WhitenSystem(E, b);
// Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping); for (size_t i = 0; i < Fs.size(); i++)
// Fs[i] = noiseModel_->Whiten(Fs[i]);
// return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_,
// augmentedHessian); // build augmented hessian
return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_, Matrix3 P;
Gs, gs, 0.0); Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement<3,Dim>(Fs, E, P, b);
return boost::make_shared<RegularHessianFactor<Dim> >(allKeys,
augmentedHessian);
} }
/** /**