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

Added missing print, made print conform to base class

release/4.3a0
dellaert 2015-02-17 00:12:21 +01:00
parent 9b766e3e71
commit bcd6a09c14
2 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -13,15 +13,22 @@ using namespace std;
namespace gtsam { namespace gtsam {
//*************************************************************************** //***************************************************************************
void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "OrientedPlane3Factor Factor on " << landmarkSymbol_ << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
void OrientedPlane3DirectionPrior::print(const string& s) const { //***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "Prior Factor on " << landmarkKey_ << "\n"; cout << "Prior Factor on " << landmarkKey_ << "\n";
measured_p_.print("Measured Plane"); measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
//*************************************************************************** //***************************************************************************
bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
double tol) const { double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);

18
gtsam/slam/OrientedPlane3Factor.h
View File

@ -7,16 +7,15 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/OrientedPlane3.h> #include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <iostream> #include <iostream>
namespace gtsam { namespace gtsam {
/** /**
* Factor to measure a planar landmark from a given pose * Factor to measure a planar landmark from a given pose
*/ */
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> { class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
@ -48,7 +47,10 @@ public:
} }
/// print /// print
void print(const std::string& s="PlaneFactor") const; virtual void print(const std::string& s = "OrientedPlane3Factor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// evaluateError
virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const boost::optional<Matrix&> H2 = boost::none) const
@ -81,10 +83,12 @@ public:
{ {
measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3)); measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3));
} }
/// print
void print(const std::string& s) const;
/** equals */ /// print
virtual void print(const std::string& s = "OrientedPlane3DirectionPrior",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
virtual Vector evaluateError(const OrientedPlane3& plane, virtual Vector evaluateError(const OrientedPlane3& plane,