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

add todo for error in SmartStereoProjectionFactorPP.h

release/4.3a0
Varun Agrawal 2023-06-20 17:31:49 -04:00
parent 254e3128e6
commit 62fe1a9379
1 changed files with 22 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -201,10 +201,9 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
} }
/// linearize and return a Hessianfactor that is an approximation of error(p) /// linearize and return a Hessianfactor that is an approximation of error(p)
std::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor( std::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping = const Values& values, const double lambda = 0.0,
false) const { bool diagonalDamping = false) const {
// we may have multiple cameras sharing the same extrinsic cals, hence the number // we may have multiple cameras sharing the same extrinsic cals, hence the number
// of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
// have a body key and an extrinsic calibration key for each measurement) // have a body key and an extrinsic calibration key for each measurement)
@ -212,7 +211,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
// Create structures for Hessian Factors // Create structures for Hessian Factors
KeyVector js; KeyVector js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Vector> gs(nrUniqueKeys); std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() != cameras(values).size()) if (this->measured_.size() != cameras(values).size())
@ -222,13 +221,12 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
// triangulate 3D point at given linearization point // triangulate 3D point at given linearization point
triangulateSafe(cameras(values)); triangulateSafe(cameras(values));
if (!result_) { // failed: return "empty/zero" Hessian // failed: return "empty/zero" Hessian
for (Matrix& m : Gs) if (!result_) {
m = Matrix::Zero(DimPose, DimPose); for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs) for (Vector& v : gs) v = Vector::Zero(DimPose);
v = Vector::Zero(DimPose); return std::make_shared<RegularHessianFactor<DimPose>>(keys_, Gs, gs,
return std::make_shared < RegularHessianFactor<DimPose> 0.0);
> (keys_, Gs, gs, 0.0);
} }
// compute Jacobian given triangulated 3D Point // compute Jacobian given triangulated 3D Point
@ -239,12 +237,13 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
// Whiten using noise model // Whiten using noise model
noiseModel_->WhitenSystem(E, b); noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++) for (size_t i = 0; i < Fs.size(); i++) {
Fs[i] = noiseModel_->Whiten(Fs[i]); Fs[i] = noiseModel_->Whiten(Fs[i]);
}
// build augmented Hessian (with last row/column being the information vector) // build augmented Hessian (with last row/column being the information vector)
Matrix3 P; Matrix3 P;
Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping); Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
KeyVector nonuniqueKeys; KeyVector nonuniqueKeys;
@ -253,12 +252,15 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
} }
// but we need to get the augumented hessian wrt the unique keys in key_ // but we need to get the augumented hessian wrt the unique keys in key_
SymmetricBlockMatrix augmentedHessianUniqueKeys = //TODO(Varun) SchurComplementAndRearrangeBlocks is causing the multiply defined symbol error
Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b, // SymmetricBlockMatrix augmentedHessianUniqueKeys =
nonuniqueKeys, keys_); // Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock,
// DimPose>(
// Fs, E, P, b, nonuniqueKeys, keys_);
return std::make_shared < RegularHessianFactor<DimPose> // return std::make_shared<RegularHessianFactor<DimPose>>(
> (keys_, augmentedHessianUniqueKeys); // keys_, augmentedHessianUniqueKeys);
return nullptr;
} }
/** /**