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

getting better

release/4.3a0
lcarlone 2021-03-26 20:04:42 -04:00
parent ec047ccd08
commit e8db2b6b9b
1 changed files with 67 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -192,18 +192,12 @@ 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 {
KeyVector allKeys; // includes body poses and *unique* extrinsic poses size_t nrKeys = keys_.size();
allKeys.insert(allKeys.end(), keys_.begin(), 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(nrKeys * (nrKeys + 1) / 2);
std::vector<Vector> gs(numKeys); std::vector<Vector> gs(nrKeys);
// 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->"
@ -218,25 +212,16 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
m = Matrix::Zero(DimPose,DimPose); m = Matrix::Zero(DimPose,DimPose);
for(Vector& v: gs) for(Vector& v: gs)
v = Vector::Zero(DimPose); v = Vector::Zero(DimPose);
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys, return boost::make_shared<RegularHessianFactor<DimPose> >(keys_,
Gs, gs, 0.0); 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(). // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
FBlocks Fs; FBlocks Fs;
Matrix F, E; Matrix F, E;
Vector b; Vector b;
computeJacobiansWithTriangulatedPoint(Fs, E, b, values); computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
// std::cout << "Dim "<< Dim << std::endl;
// std::cout << "numKeys "<< numKeys << std::endl;
//
// std::cout << "Fs.size() = " << Fs.size() << std::endl;
// std::cout << "E = " << E << std::endl;
// std::cout << "b = " << b << std::endl;
// Whiten using noise model // Whiten using noise model
// std::cout << "noise model1 \n " << std::endl; // std::cout << "noise model1 \n " << std::endl;
noiseModel_->WhitenSystem(E, b); noiseModel_->WhitenSystem(E, b);
@ -257,45 +242,75 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement<3,Dim>(Fs, E, P, b); 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::vector<DenseIndex> dims(nrKeys + 1); // this also includes the b term
// 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); std::fill(dims.begin(), dims.end() - 1, 6);
dims.back() = 1; dims.back() = 1;
size_t nrKeysNonUnique = w_P_body_keys_.size() + body_P_cam_keys_.size();
if ( numKeys == nrKeysNonUnique ){ // 1 calibration per camera size_t nrNonuniqueKeys = w_P_body_keys_.size() + body_P_cam_keys_.size();
SymmetricBlockMatrix augmentedHessianPP = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView())); SymmetricBlockMatrix augmentedHessianPP;
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys, if ( nrKeys == nrNonuniqueKeys ){ // 1 calibration per camera
augmentedHessianPP); augmentedHessianPP = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView()));
}else{ }else{
Matrix augmentedHessianMatrixPP = Matrix(augmentedHessian.selfadjointView()); std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
Matrix associationMatrix = Matrix::Zero( numKeys, nrKeysNonUnique ); // association from unique keys to vector with potentially repeated keys std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
std::cout << "Linearize" << std::endl; nonuniqueDims.back() = 1;
augmentedHessian = SymmetricBlockMatrix(nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
for(size_t i=0; i<numKeys;i++){ // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
for(size_t j=0; j<nrKeysNonUnique;k++){ KeyVector nonuniqueKeys;
if ( keys_[i] == ) for(size_t i=0; i < w_P_body_keys_.size();i++){
// std::cout <<"key: " << DefaultKeyFormatter(allKeys[i]) << std::endl; nonuniqueKeys.push_back(w_P_body_keys_.at(i));
} nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
} }
for (size_t i=0; i < w_P_body_keys_.size() + body_P_cam_keys_.size(); i++){ // get map from key to location in the new augmented Hessian matrix
// create map of unique keys std::map<Key,size_t> keyToSlotMap;
for(size_t k=0; k<nrKeys;k++){
keyToSlotMap[keys_[k]] = k;
} }
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2); std::cout << "linearize" << std::endl;
std::vector<Vector> gs(numKeys); for(size_t i=0; i<nrKeys;i++){
for(Matrix& m: Gs) std::cout <<"key: " << DefaultKeyFormatter(keys_[i]);
m = Matrix::Zero(DimPose,DimPose); std::cout <<" key slot: " << keyToSlotMap[keys_[i]] << std::endl;
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);
} }
for(size_t i=0; i<nrNonuniqueKeys;i++){
std::cout <<"key: " << DefaultKeyFormatter(nonuniqueKeys[i]);
std::cout <<" key slot: " << keyToSlotMap[nonuniqueKeys[i]] << std::endl;
}
// initialize matrix to zero
augmentedHessianPP = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrKeys+1,6*nrKeys+1));
std::cout <<" start for loop: " << std::endl;
// add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
for(size_t i=0; i<nrNonuniqueKeys;i++){ // rows
Key key_i = nonuniqueKeys.at(i);
std::cout <<" start for loop i: " << std::endl;
for(size_t j=0; j<nrNonuniqueKeys;j++){ // cols
std::cout <<" start for loop j: " << std::endl;
Key key_j = nonuniqueKeys.at(j);
std::cout <<"key_i: " << DefaultKeyFormatter(key_i);
std::cout <<" key_j: " << DefaultKeyFormatter(key_j);
std::cout <<" start for loop --: " << std::endl;
if(i==j){
std::cout <<" i=0: " << std::endl;
augmentedHessianPP.updateDiagonalBlock( keyToSlotMap[key_i] , augmentedHessian.diagonalBlock(i));
}else if(i < j){
std::cout <<" i<j: " << std::endl;
augmentedHessianPP.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j],
augmentedHessian.aboveDiagonalBlock(i,j));
}
else{
std::cout <<" i>j: " << std::endl;
augmentedHessianPP.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j],
augmentedHessian.aboveDiagonalBlock(j,i));
}
}
}
}
return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianPP);
//std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl; //std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
} }