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 {
//***************************************************************************
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";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);

18
gtsam/slam/OrientedPlane3Factor.h
View File

@ -7,16 +7,15 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Key.h>
#include <iostream>
namespace gtsam {
/**
/**
* Factor to measure a planar landmark from a given pose
*/
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
@ -48,7 +47,10 @@ public:
}
/// 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,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
@ -81,10 +83,12 @@ public:
{
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 Vector evaluateError(const OrientedPlane3& plane,