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

amended

release/4.3a0
lcarlone 2021-03-25 21:42:28 -04:00
parent 7c052ff48a
commit b3c828f8d2
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -205,9 +205,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2); std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
std::vector<Vector> gs(numKeys); std::vector<Vector> gs(numKeys);
for(size_t i=0; i<numKeys;i++){ // for(size_t i=0; i<numKeys;i++){
std::cout <<"key: " << DefaultKeyFormatter(allKeys[i]) << std::endl; // 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->"
@ -264,7 +264,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
std::fill(dims.begin(), dims.end() - 1, 6); std::fill(dims.begin(), dims.end() - 1, 6);
dims.back() = 1; dims.back() = 1;
SymmetricBlockMatrix augmentedHessianPP(dims, Matrix(augmentedHessian.selfadjointView())); SymmetricBlockMatrix augmentedHessianPP(dims, Matrix(augmentedHessian.selfadjointView()));
std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl; //std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys, return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys,
augmentedHessianPP); augmentedHessianPP);

10
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -698,11 +698,11 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); // GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
GFG->print("GFG \n"); // // GFG->print("GFG \n");
VectorValues delta = GFG->optimize(); // VectorValues delta = GFG->optimize();
VectorValues expected = VectorValues::Zero(delta); // VectorValues expected = VectorValues::Zero(delta);
EXPECT(assert_equal(expected, delta, 1e-6)); // EXPECT(assert_equal(expected, delta, 1e-6));
} }
/* ************************************************************************* /* *************************************************************************