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

verbose printing of exceptions

release/4.3a0
Varun Agrawal 2021-01-05 10:44:26 -05:00
parent 473a6a15ee
commit b7584ce362
2 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/slam/GeneralSFMFactor.h
View File

@ -70,6 +70,7 @@ class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
protected: protected:
Point2 measured_; ///< the 2D measurement Point2 measured_; ///< the 2D measurement
bool verbose_; ///< Flag for print verbosity
public: public:
@ -86,12 +87,17 @@ public:
* @param cameraKey is the index of the camera * @param cameraKey is the index of the camera
* @param landmarkKey is the index of the landmark * @param landmarkKey is the index of the landmark
*/ */
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model,
Base(model, cameraKey, landmarkKey), measured_(measured) {} Key cameraKey, Key landmarkKey, bool verbose = false)
: Base(model, cameraKey, landmarkKey),
measured_(measured),
verbose_(verbose) {}
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor GeneralSFMFactor() : measured_(0.0, 0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 GeneralSFMFactor(const Point2& p) : measured_(p) {}
///< constructor that takes doubles x,y to make a Point2
GeneralSFMFactor(double x, double y) : measured_(x, y) {}
virtual ~GeneralSFMFactor() {} ///< destructor virtual ~GeneralSFMFactor() {} ///< destructor
@ -127,7 +133,9 @@ public:
catch( CheiralityException& e) { catch( CheiralityException& e) {
if (H1) *H1 = JacobianC::Zero(); if (H1) *H1 = JacobianC::Zero();
if (H2) *H2 = JacobianL::Zero(); if (H2) *H2 = JacobianL::Zero();
// TODO warn if verbose output asked for if (verbose_) {
std::cout << e.what() << std::endl;
}
return Z_2x1; return Z_2x1;
} }
} }
@ -149,7 +157,9 @@ public:
H1.setZero(); H1.setZero();
H2.setZero(); H2.setZero();
b.setZero(); b.setZero();
// TODO warn if verbose output asked for if (verbose_) {
std::cout << e.what() << std::endl;
}
} }
// Whiten the system if needed // Whiten the system if needed

3
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
View File

@ -188,7 +188,8 @@ int main(int argc, char** argv) {
smartFactors[j]->addRange(i, range); smartFactors[j]->addRange(i, range);
printf("adding range %g for %d",range,(int)j); printf("adding range %g for %d",range,(int)j);
} catch (const invalid_argument& e) { } catch (const invalid_argument& e) {
printf("warning: omitting duplicate range %g for %d",range,(int)j); printf("warning: omitting duplicate range %g for %d: %s", range,
(int)j, e.what());
} }
cout << endl; cout << endl;
} }