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

working on new sym matrix

release/4.3a0
lcarlone 2021-03-19 23:09:26 -04:00
parent 6d118da82d
commit 2dc908c986
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -195,6 +195,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
std::vector<Vector> gs(numKeys); std::vector<Vector> gs(numKeys);
std::cout <<"using my hessian!!!!!!!!!!1" << std::endl; std::cout <<"using my hessian!!!!!!!!!!1" << std::endl;
for(size_t i=0; i<numKeys;i++){
std::cout <<"key: " << DefaultKeyFormatter(allKeys[i]) << std::endl;
}
if (this->measured_.size() != cameras(values).size()) if (this->measured_.size() != cameras(values).size())
throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
@ -235,8 +238,13 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement<3,Dim>(Fs, E, P, b); Cameras::SchurComplement<3,Dim>(Fs, E, P, b);
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, 6);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessianPP(dims, augmentedHessian.full());
return boost::make_shared<RegularHessianFactor<Dim> >(allKeys, return boost::make_shared<RegularHessianFactor<Dim> >(allKeys,
augmentedHessian); augmentedHessianPP);
} }
/** /**