gtsam  4.1.0
gtsam
TransformBtwRobotsUnaryFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
17 #pragma once
18 
22 #include <gtsam/base/Testable.h>
23 #include <gtsam/base/Lie.h>
24 
25 #include <ostream>
26 
27 namespace gtsam {
28 
34  template<class VALUE>
35  class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ?
36 
37  public:
38 
39  typedef VALUE T;
40 
41  private:
42 
45 
46  gtsam::Key key_;
47 
48  VALUE measured_;
50  gtsam::Values valA_; // given values for robot A map\trajectory
51  gtsam::Values valB_; // given values for robot B map\trajectory
52  gtsam::Key keyA_; // key of robot A to which the measurement refers
53  gtsam::Key keyB_; // key of robot B to which the measurement refers
54 
55  SharedGaussian model_;
56 
58  GTSAM_CONCEPT_LIE_TYPE(T)
59  GTSAM_CONCEPT_TESTABLE_TYPE(T)
60 
61  public:
62 
63  // shorthand for a smart pointer to a factor
64  typedef typename boost::shared_ptr<TransformBtwRobotsUnaryFactor> shared_ptr;
65 
68 
70  TransformBtwRobotsUnaryFactor(Key key, const VALUE& measured, Key keyA, Key keyB,
71  const gtsam::Values& valA, const gtsam::Values& valB,
72  const SharedGaussian& model) :
73  Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB),
74  model_(model){
75 
76  setValAValB(valA, valB);
77 
78  }
79 
80  virtual ~TransformBtwRobotsUnaryFactor() {}
81 
82 
84  gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::make_shared<This>(*this); }
85 
86 
90  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
91  std::cout << s << "TransformBtwRobotsUnaryFactor("
92  << keyFormatter(key_) << ")\n";
93  std::cout << "MR between factor keys: "
94  << keyFormatter(keyA_) << ","
95  << keyFormatter(keyB_) << "\n";
96  measured_.print(" measured: ");
97  model_->print(" noise model: ");
98  // Base::print(s, keyFormatter);
99  }
100 
102  bool equals(const NonlinearFactor& f, double tol=1e-9) const override {
103  const This *t = dynamic_cast<const This*> (&f);
104 
105  if(t && Base::equals(f))
106  return key_ == t->key_ && measured_.equals(t->measured_);
107  else
108  return false;
109  }
110 
113  /* ************************************************************************* */
114  void setValAValB(const gtsam::Values& valA, const gtsam::Values& valB){
115  if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
116  throw("something is wrong!");
117 
118  // TODO: make sure the two keys belong to different robots
119 
120  if (valA.exists(keyA_)){
121  valA_ = valA;
122  valB_ = valB;
123  }
124  else {
125  valA_ = valB;
126  valB_ = valA;
127  }
128  }
129 
130  /* ************************************************************************* */
131  double error(const gtsam::Values& x) const override {
132  return whitenedError(x).squaredNorm();
133  }
134 
135  /* ************************************************************************* */
141  /* This version of linearize recalculates the noise model each time */
142  boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const override {
143  // Only linearize if the factor is active
144  if (!this->active(x))
145  return boost::shared_ptr<gtsam::JacobianFactor>();
146 
147  //std::cout<<"About to linearize"<<std::endl;
148  gtsam::Matrix A1;
149  std::vector<gtsam::Matrix> A(this->size());
150  gtsam::Vector b = -whitenedError(x, A);
151  A1 = A[0];
152 
154  new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size())));
155  }
156 
157 
158  /* ************************************************************************* */
159  gtsam::Vector whitenedError(const gtsam::Values& x,
160  boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
161 
162  T orgA_T_currA = valA_.at<T>(keyA_);
163  T orgB_T_currB = valB_.at<T>(keyB_);
164  T orgA_T_orgB = x.at<T>(key_);
165 
166  T currA_T_currB_pred;
167  if (H) {
168  Matrix H_compose, H_between1;
169  T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, boost::none);
170  currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, boost::none, H_between1);
171  (*H)[0] = H_compose * H_between1;
172  }
173  else {
174  T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
175  currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
176  }
177 
178  const T& currA_T_currB_msr = measured_;
179  Vector error = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
180 
181  if (H)
182  model_->WhitenSystem(*H, error);
183  else
184  model_->whitenInPlace(error);
185 
186  return error;
187  }
188 
189 
190  /* ************************************************************************* */
191  gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
192 
193  T orgA_T_currA = valA_.at<T>(keyA_);
194  T orgB_T_currB = valB_.at<T>(keyB_);
195  T orgA_T_orgB = x.at<T>(key_);
196 
197  T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
198  T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
199 
200  T currA_T_currB_msr = measured_;
201  return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
202  }
203 
204  /* ************************************************************************* */
205 
206  size_t dim() const override {
207  return model_->R().rows() + model_->R().cols();
208  }
209 
210  private:
211 
214  template<class ARCHIVE>
215  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
216  ar & boost::serialization::make_nvp("NonlinearFactor",
217  boost::serialization::base_object<Base>(*this));
218  //ar & BOOST_SERIALIZATION_NVP(measured_);
219  }
220  }; // \class TransformBtwRobotsUnaryFactor
221 
223  template<class VALUE>
225  public Testable<TransformBtwRobotsUnaryFactor<VALUE> > {
226  };
227 
228 }
gtsam::TransformBtwRobotsUnaryFactor::access
friend class boost::serialization::access
Serialization function.
Definition: TransformBtwRobotsUnaryFactor.h:213
gtsam::TransformBtwRobotsUnaryFactor::error
double error(const gtsam::Values &x) const override
Calculate the error of the factor This is typically equal to log-likelihood, e.g.
Definition: TransformBtwRobotsUnaryFactor.h:131
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::TransformBtwRobotsUnaryFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: TransformBtwRobotsUnaryFactor.h:90
Testable.h
Concept check for values that can be used in unit tests.
gtsam::TransformBtwRobotsUnaryFactor
Definition: TransformBtwRobotsUnaryFactor.h:35
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:608
gtsam::TransformBtwRobotsUnaryFactor::TransformBtwRobotsUnaryFactor
TransformBtwRobotsUnaryFactor(Key key, const VALUE &measured, Key keyA, Key keyB, const gtsam::Values &valA, const gtsam::Values &valB, const SharedGaussian &model)
Constructor.
Definition: TransformBtwRobotsUnaryFactor.h:70
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
BetweenFactor.h
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::TransformBtwRobotsUnaryFactor::linearize
boost::shared_ptr< gtsam::GaussianFactor > linearize(const gtsam::Values &x) const override
Linearize a non-linearFactorN to get a gtsam::GaussianFactor, Hence .
Definition: TransformBtwRobotsUnaryFactor.h:142
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::TransformBtwRobotsUnaryFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Clone.
Definition: TransformBtwRobotsUnaryFactor.h:84
NonlinearFactor.h
Non-linear factor base classes.
gtsam::TransformBtwRobotsUnaryFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: TransformBtwRobotsUnaryFactor.h:206
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
gtsam::Factor::size
size_t size() const
Definition: Factor.h:129
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::TransformBtwRobotsUnaryFactor::shared_ptr
boost::shared_ptr< TransformBtwRobotsUnaryFactor > shared_ptr
concept check by type
Definition: TransformBtwRobotsUnaryFactor.h:64
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::TransformBtwRobotsUnaryFactor::TransformBtwRobotsUnaryFactor
TransformBtwRobotsUnaryFactor()
default constructor - only use for serialization
Definition: TransformBtwRobotsUnaryFactor.h:67
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition: Values.cpp:104
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::TransformBtwRobotsUnaryFactor::setValAValB
void setValAValB(const gtsam::Values &valA, const gtsam::Values &valB)
implement functions needed to derive from Factor
Definition: TransformBtwRobotsUnaryFactor.h:114
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::TransformBtwRobotsUnaryFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition: TransformBtwRobotsUnaryFactor.h:102
Lie.h
Base class and basic functions for Lie types.
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition: NonlinearFactor.cpp:36