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

moving to more appropriate construction of Hessian

release/4.3a0
lcarlone 2021-03-26 17:25:27 -04:00
parent b3c828f8d2
commit ec047ccd08
2 changed files with 47 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
View File

@ -35,7 +35,10 @@ void SmartStereoProjectionFactorPP::add(
w_P_body_keys_.push_back(w_P_body_key);
body_P_cam_keys_.push_back(body_P_cam_key);
keys_.push_back(body_P_cam_key);
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end())
keys_.push_back(body_P_cam_key); // add only unique keys
K_all_.push_back(K);
}
@ -48,7 +51,9 @@ void SmartStereoProjectionFactorPP::add(
assert(w_P_body_keys.size() == Ks.size());
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements[i], w_P_body_keys[i]);
keys_.push_back(body_P_cam_keys[i]);
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
keys_.push_back(body_P_cam_keys[i]); // add only unique keys
w_P_body_keys_.push_back(w_P_body_keys[i]);
body_P_cam_keys_.push_back(body_P_cam_keys[i]);
@ -65,7 +70,9 @@ void SmartStereoProjectionFactorPP::add(
assert(w_P_body_keys.size() == body_P_cam_keys.size());
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements[i], w_P_body_keys[i]);
keys_.push_back(body_P_cam_keys[i]);
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
keys_.push_back(body_P_cam_keys[i]); // add only unique keys
w_P_body_keys_.push_back(w_P_body_keys[i]);
body_P_cam_keys_.push_back(body_P_cam_keys[i]);

45
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -194,10 +194,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
KeyVector allKeys; // includes body poses and *unique* extrinsic poses
allKeys.insert(allKeys.end(), keys_.begin(), 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
@ -225,6 +221,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys,
Gs, gs, 0.0);
}
// std::cout << "result_" << *result_ << std::endl;
// std::cout << "result_2" << result_ << std::endl;
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
@ -260,14 +257,46 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement<3,Dim>(Fs, E, P, b);
// 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());
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, Matrix(augmentedHessian.selfadjointView()));
//std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
size_t nrKeysNonUnique = w_P_body_keys_.size() + body_P_cam_keys_.size();
if ( numKeys == nrKeysNonUnique ){ // 1 calibration per camera
SymmetricBlockMatrix augmentedHessianPP = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView()));
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys,
augmentedHessianPP);
}else{
Matrix augmentedHessianMatrixPP = Matrix(augmentedHessian.selfadjointView());
Matrix associationMatrix = Matrix::Zero( numKeys, nrKeysNonUnique ); // association from unique keys to vector with potentially repeated keys
std::cout << "Linearize" << std::endl;
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys,
augmentedHessianPP);
for(size_t i=0; i<numKeys;i++){
for(size_t j=0; j<nrKeysNonUnique;k++){
if ( keys_[i] == )
// std::cout <<"key: " << DefaultKeyFormatter(allKeys[i]) << std::endl;
}
}
for (size_t i=0; i < w_P_body_keys_.size() + body_P_cam_keys_.size(); i++){
// create map of unique keys
}
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
std::vector<Vector> gs(numKeys);
for(Matrix& m: Gs)
m = Matrix::Zero(DimPose,DimPose);
for(Vector& v: gs)
v = Vector::Zero(DimPose);
double e = augmentedHessianMatrixPP( augmentedHessianMatrixPP.rows()-1, augmentedHessianMatrixPP.cols()-1 );
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys,
Gs, gs, e);
}
//std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
}
/**