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

34
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -202,9 +202,8 @@ 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)
@ -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,8 +237,9 @@ 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;
@ -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;
} }
/** /**