MIRA
Transformer.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 by
3  * MetraLabs GmbH (MLAB), GERMANY
4  * and
5  * Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
6  * All rights reserved.
7  *
8  * Contact: info@mira-project.org
9  *
10  * Commercial Usage:
11  * Licensees holding valid commercial licenses may use this file in
12  * accordance with the commercial license agreement provided with the
13  * software or, alternatively, in accordance with the terms contained in
14  * a written agreement between you and MLAB or NICR.
15  *
16  * GNU General Public License Usage:
17  * Alternatively, this file may be used under the terms of the GNU
18  * General Public License version 3.0 as published by the Free Software
19  * Foundation and appearing in the file LICENSE.GPL3 included in the
20  * packaging of this file. Please review the following information to
21  * ensure the GNU General Public License version 3.0 requirements will be
22  * met: http://www.gnu.org/copyleft/gpl.html.
23  * Alternatively you may (at your option) use any later version of the GNU
24  * General Public License if such license has been publicly approved by
25  * MLAB and NICR (or its successors, if any).
26  *
27  * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
28  * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
29  * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
30  * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32  * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
33  * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
34  * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
35  * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
36  * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
37  */
38 
48 #ifndef _MIRA_TRANSFORMER_H_
49 #define _MIRA_TRANSFORMER_H_
50 
51 #include <string>
52 #include <list>
53 #include <vector>
54 
55 #ifndef Q_MOC_RUN
56 #include <boost/thread/mutex.hpp>
57 #endif
58 
59 #include <error/Exceptions.h>
60 
61 #include <utils/Foreach.h>
62 
65 
67 
68 namespace mira {
69 
71 
79 {
80 public:
81 
88  static const int MAX_TREE_DEPTH = 1000;
89 
90 public:
91 
94 
95 public:
96 
103  class Chain
104  {
105  public:
106  std::list<AbstractNodePtr> inverse;
107  std::list<AbstractNodePtr> forward;
108 
110  bool empty() const { return inverse.empty() && forward.empty(); }
111  };
112 
113 
114 public:
115 
117  TransformerBase();
118 
120  ~TransformerBase();
121 
122 public:
123 
134  bool addLink(AbstractNodePtr child, AbstractNodePtr parent);
135 
141  void removeLink(AbstractNodePtr child, AbstractNodePtr parent);
142 
143 public:
144 
148  std::list<AbstractNodePtr> getNodes();
149 
154  std::list<std::pair<AbstractNodePtr, AbstractNodePtr>> getLinks();
155 
160  const std::list<AbstractNodePtr> getRootNodes();
161 
162 public:
163 
168  void getTransformChain(AbstractNodePtr target, AbstractNodePtr source,
169  Chain& oChain);
170 
176  bool isTransformAvailable(AbstractNodePtr target, AbstractNodePtr source);
177 
178 public:
179 
184  MIRA_DEFINE_SERIALIZABLE_EXCEPTION(XTransform, XRuntime)
185 
186 protected:
187 
189  void addNode(AbstractNodePtr node);
190 
195  AbstractNodePtr getNode(const std::string& nodeID);
196 
197 
198 private:
199 
207  AbstractNodePtr collectNodesUpwardsToRoot(AbstractNodePtr node,
208  std::list<AbstractNodePtr>& oNodes);
209 
210 protected:
211 
212 
213  typedef std::map<std::string, AbstractNodePtr> IDToNodeMap;
214 
217 
219  boost::mutex mMutex;
220 };
221 
223 
230 {
232 
234  const TransformerBase::Chain& c2) : chain1(c1), chain2(c2) {}
235 
238 };
239 
241 
249 template <typename TNode>
251 {
252  typedef TransformerBase Base;
253 
254 public:
255 
257  typedef TNode Node;
258 
260  typedef Node* NodePtr;
261 
264 
265 public:
266 
273  {
274  Chain chain;
275  getTransformChain(target, source, chain);
276  return TransformDesc(chain);
277  }
278 
287  {
288  Chain chain1, chain2;
289  getTransformChain(target, fixed, chain1);
290  getTransformChain(fixed, source, chain2);
291  return TransformDesc(chain1, chain2);
292  }
293 
302  template<typename Transform, typename Filter>
303  Transform getTransform(const TransformDesc& desc,
304  const Time& targetTime, const Time& sourceTime,
305  Filter&& filter)
306  {
307  if(desc.chain2.empty())
308  return getTransform<Transform>(desc.chain1, targetTime, filter);
309  else
310  return getTransform<Transform>(desc.chain2, sourceTime, filter) *
311  getTransform<Transform>(desc.chain1, targetTime, filter);
312  }
313 
315  template<typename Transform>
316  Transform getTransform(const TransformDesc& desc,
317  const Time& targetTime, const Time& sourceTime)
318  {
319  return getTransform<Transform>(desc, targetTime, sourceTime,
321  }
322 
323 
325  template<typename Transform, typename Filter>
326  Transform getTransform(const TransformDesc& desc, const Time& time,
327  Filter&& filter)
328  {
329  return getTransform<Transform>(desc, time, time, filter);
330  }
331 
333  template<typename Transform>
334  Transform getTransform(const TransformDesc& desc, const Time& time)
335  {
336  return getTransform<Transform>(desc, time,
338  }
339 
347  template<typename Transform, typename Filter>
348  Transform getTransform(NodePtr target, NodePtr source, const Time& time,
349  Filter&& filter)
350  {
351  TransformDesc desc = prepareTransform(target, source);
352  return getTransform<Transform>(desc, time, filter);
353  }
354 
356  template<typename Transform>
357  Transform getTransform(NodePtr target, NodePtr source, const Time& time)
358  {
359  return getTransform<Transform>(target, source, time,
361  }
362 
372  template<typename Transform, typename Filter>
373  Transform getTransform(NodePtr target, const Time& targetTime,
374  NodePtr source, const Time& sourceTime,
375  NodePtr fixed, Filter&& filter)
376  {
377  TransformDesc desc = prepareTransform(target, source, fixed);
378  return getTransform<Transform>(desc, targetTime, sourceTime, filter);
379  }
380 
382  template<typename Transform>
383  Transform getTransform(NodePtr target, const Time& targetTime,
384  NodePtr source, const Time& sourceTime,
385  NodePtr fixed)
386  {
387  return getTransform<Transform>(target, targetTime,
388  source, sourceTime, fixed,
390  }
391 
392 public:
393 
428  template<typename Transform, typename Filter>
429  Transform inferTransform(NodePtr node, NodePtr target, NodePtr source,
430  const Transform& transform,
431  const Time& time, Filter&& filter)
432  {
433  /*
434  CASE 1:
435  p ---- x ---- source
436  | .
437  | ?? .
438  | .
439  v v
440  node --- y --- target
441  */
442 
443  // path from node to target
444  TransformDesc node_target = prepareTransform(target,node);
445 
446  // path from node to source
447  TransformDesc node_source = prepareTransform(source,node);
448 
449  // if the way from node to target goes in reverse direction towards the
450  // parent of "node" (this is the case if "node" is the first one in the
451  // inverse list), then we have case 2 here
452  if(!node_target.chain1.inverse.empty() &&
453  node_target.chain1.inverse.front()==node) {
454  /*
455  CASE 2: (the way to target would be node->p->x->target,
456  via the unknown link (p->node))
457  p ---- x ---- target
458  | ^
459  | ?? .
460  | .
461  v .
462  node --- y --- source
463  */
464 
465  // make sure, that the way to source does not go via the unknown
466  // link (p->node), too. In this case we have a degenerate case and
467  // the user has provided invalid nodes
468  if(!node_source.chain1.inverse.empty() &&
469  node_source.chain1.inverse.front()==node) {
470  MIRA_THROW(TransformerBase::XTransform,
471  "Cannot infer the transformation since all paths from '" <<
472  node->getID() << "' to source '" << source->getID() << "' "
473  "or target '" << target->getID() << "' go via "
474  "the link that should be inferred.");
475  }
476 
477  Transform y = getTransform<Transform>(node_source, time, filter);
478 
479  // remove node from node->target path to get p->target path
480  assert(!node_target.chain1.inverse.empty());
481  assert(node_target.chain1.inverse.front()==node);
482  node_target.chain1.inverse.pop_front();
483 
484  Transform x = getTransform<Transform>(node_target, time, filter);
485 
486  return x*transform.inverse()*y.inverse();
487 
488  } else {
489  /*
490  CASE 1:
491  p ---- x ---- source
492  | .
493  | ?? .
494  | .
495  v v
496  node --- y --- target
497  */
498 
499 
500  Transform y = getTransform<Transform>(node_target, time, filter);
501 
502  // remove node from node->source path to get p->source path
503  assert(!node_source.chain1.inverse.empty());
504  assert(node_source.chain1.inverse.front()==node);
505  node_source.chain1.inverse.pop_front();
506 
507  Transform x = getTransform<Transform>(node_source, time, filter);
508 
509  return x * transform * y.inverse();
510  }
511  }
512 
514  template<typename Transform>
515  Transform inferTransform(NodePtr node, NodePtr target, NodePtr source,
516  const Transform& transform, const Time& time)
517  {
518  return inferTransform<Transform>(node, target, source, transform, time,
520  }
521 
522 public:
523 
528  NodePtr getNode(const std::string& nodeID) {
529  return castNode(Base::getNode(nodeID));
530  }
531 
532 protected:
533 
536  return static_cast<Node*>(node);
537  }
538 
543  template<typename Transform, typename Filter>
544  Transform getTransform(const Chain& chain, const Time& time, Filter&& filter)
545  {
546  Transform finalTransform; // start with identity transform
547 
548  // now concatenate the inverse transforms
549  foreach(AbstractNodePtr n, chain.inverse)
550  finalTransform*=castNode(n)->template getTransform<Transform>(time, filter).inverse();
551 
552  // concatenate the forward transforms (in reverse order because we chained them in reverse order)
554  finalTransform*=castNode(n)->template getTransform<Transform>(time, filter);
555 
556  return finalTransform;
557  }
558 
559 };
560 
562 
572 template <typename T, int D, template <typename T_, int D_> class TTransform = RigidTransform>
573 class Transformer : public GenericTransformer<TransformerNode<TTransform<T, D>>>
574 {
576 public:
577 
580 
582  typedef Node* NodePtr;
583 
584 public:
585 
590  NodePtr newNode(const std::string& nodeID) {
591  NodePtr node(new Node(nodeID));
592  this->addNode(node);
593  return node;
594  }
595 };
596 
598 
603 
608 
609 
614 
619 
621 
622 } // namespace
623 
624 #endif
IDToNodeMap mNodes
maps from ids to nodes that were added to us
Definition: Transformer.h:216
Definition: FrameworkTransformer.h:75
Macro for iterating over all elements in a container.
TransformerBase::Chain chain1
Definition: Transformer.h:236
TransformDesc(const TransformerBase::Chain &c1)
Definition: Transformer.h:231
AbstractTransformerNode base class and TransformerNode reference implementation.
Transform getTransform(NodePtr target, NodePtr source, const Time &time, Filter &&filter)
Computes and returns a certain transformation between the specified target and source node...
Definition: Transformer.h:348
Transform getTransform(const TransformDesc &desc, const Time &targetTime, const Time &sourceTime, Filter &&filter)
Computes and returns a certain transformation that is specified via the TransformDesc.
Definition: Transformer.h:303
Transformer< float, 3, RigidTransformCov > TransformerCov3f
Typedef for a transformer with 3D floating point transforms and covariance.
Definition: Transformer.h:613
NodePtr newNode(const std::string &nodeID)
Creates and adds a new node with the specified id to this transformer.
Definition: Transformer.h:590
NodePtr getNode(const std::string &nodeID)
Returns a pointer to the node with the given ID or nullptr if it does not exist.
Definition: Transformer.h:528
Transform inferTransform(NodePtr node, NodePtr target, NodePtr source, const Transform &transform, const Time &time, Filter &&filter)
Infers a transform by specifying a direct transform between two nodes, which are connected indirectly...
Definition: Transformer.h:429
boost::mutex mMutex
protects the above node map
Definition: Transformer.h:219
A generic transformer base class that can be used with different types of actual transformation nodes...
Definition: Transformer.h:250
Node * NodePtr
The type of a pointer of the actual transformation node.
Definition: Transformer.h:260
1D nearest neighbor interpolator.
Definition: NearestNeighborInterpolator.h:68
specialize cv::DataType for our ImgPixel and inherit from cv::DataType<Vec>
Definition: IOService.h:67
Transform getTransform(const TransformDesc &desc, const Time &targetTime, const Time &sourceTime)
same as above, but using NearestNeighborInterpolator as filter.
Definition: Transformer.h:316
Transform getTransform(const TransformDesc &desc, const Time &time, Filter &&filter)
same as above, but with the same time for source and target transform.
Definition: Transformer.h:326
#define MIRA_DEFINE_SERIALIZABLE_EXCEPTION(Ex, Base)
Macro for easily defining a new serializable exception class.
Definition: Exceptions.h:66
const std::string & getID() const
Returns the id of this node.
Definition: TransformerNode.h:104
AbstractTransformerNode::AbstractNodePtr AbstractNodePtr
Injects the AbstractTransformerNode::AbstractNodePtr type.
Definition: Transformer.h:93
TransformerBase::Chain chain2
Definition: Transformer.h:237
TransformDesc prepareTransform(NodePtr target, NodePtr source, NodePtr fixed)
Prepares the computation of a transformation by computing the path between the &#39;target&#39; and &#39;source&#39; ...
Definition: Transformer.h:286
#define MIRA_THROW(ex, msg)
Macro for throwing an exception.
Definition: Exception.h:82
Transform getTransform(const TransformDesc &desc, const Time &time)
same as above, but using NearestNeighborInterpolator as filter.
Definition: Transformer.h:334
Transformer< double, 2, RigidTransformCov > TransformerCov2d
Typedef for a transformer with 2D double precision transforms and covariance.
Definition: Transformer.h:616
Wrapper class for boost::posix_time::ptime for adding more functionality to it.
Definition: Time.h:418
std::list< AbstractNodePtr > inverse
Definition: Transformer.h:106
bool empty() const
returns true, if the inverse and forward chain are both empty.
Definition: Transformer.h:110
Commonly used exception classes.
Filter that returns the nearest neighbor.
AbstractNodePtr getNode(const std::string &nodeID)
Returns the node with the specified id, or nullptr if such a node does not exist. ...
Transformer< float, 2, RigidTransformCov > TransformerCov2f
Typedef for a transformer with 2D floating point transforms and covariance.
Definition: Transformer.h:611
Represents a chain or path through the transformation tree from a staring node to the target node con...
Definition: Transformer.h:103
Transform inferTransform(NodePtr node, NodePtr target, NodePtr source, const Transform &transform, const Time &time)
same as above, but using NearestNeighborInterpolator as filter
Definition: Transformer.h:515
Transformer< float, 2 > Transformer2f
Typedef for a transformer with 2D floating point transforms.
Definition: Transformer.h:600
Base class for Transformer to decouple base functionality that is type independent from type dependen...
Definition: Transformer.h:78
Transformer< double, 2 > Transformer2d
Typedef for a transformer with 2D double precision transforms.
Definition: Transformer.h:605
Transformer< double, 3, RigidTransformCov > TransformerCov3d
Typedef for a transformer with 3D double precision transforms and covariance.
Definition: Transformer.h:618
Describes a path of transformations through the transformation tree.
Definition: Transformer.h:229
Rigid transformation class.
A full features transformer class based on GenericTransformer.
Definition: Transformer.h:573
Transformer< double, 3 > Transformer3d
Typedef for a transformer with 3D double precision transforms.
Definition: Transformer.h:607
void addNode(AbstractNodePtr node)
An exception that is thrown when errors related to transformations occur.
Transform getTransform(NodePtr target, const Time &targetTime, NodePtr source, const Time &sourceTime, NodePtr fixed)
same as above, but using NearestNeighborInterpolator as filter
Definition: Transformer.h:383
TransformerNode< TTransform< T, D > > Node
the node type, which is a TransformerNode
Definition: Transformer.h:579
Transform getTransform(NodePtr target, NodePtr source, const Time &time)
same as above, but using NearestNeighborInterpolator as filter
Definition: Transformer.h:357
std::list< AbstractNodePtr > forward
Definition: Transformer.h:107
std::map< std::string, AbstractNodePtr > IDToNodeMap
Definition: Transformer.h:213
TransformerBase::AbstractNodePtr AbstractNodePtr
The type of a pointer to the AbstractTransformerNode base class.
Definition: Transformer.h:263
Abstract base class where all other different types of transformation nodes must be derived from...
Definition: TransformerNode.h:88
Node * NodePtr
pointer type to that node
Definition: Transformer.h:582
Transform getTransform(const Chain &chain, const Time &time, Filter &&filter)
Computes the resulting transform by applying all transforms according to the specified transform chai...
Definition: Transformer.h:544
#define MIRA_BASE_EXPORT
This is required because on windows there is a macro defined called ERROR.
Definition: Platform.h:153
TransformDesc(const TransformerBase::Chain &c1, const TransformerBase::Chain &c2)
Definition: Transformer.h:233
#define foreach_reverse(i, c)
The macro can be used similar to foreach, but the containers are traversed in reverse order...
Definition: Foreach.h:110
TNode Node
The type of a transformation node.
Definition: Transformer.h:257
void getTransformChain(AbstractNodePtr target, AbstractNodePtr source, Chain &oChain)
TransformDesc prepareTransform(NodePtr target, NodePtr source)
Prepares the computation of a transformation by computing the path between the target and source node...
Definition: Transformer.h:272
Transformer< float, 3 > Transformer3f
Typedef for a transformer with 3D floating point transforms.
Definition: Transformer.h:602
Transform getTransform(NodePtr target, const Time &targetTime, NodePtr source, const Time &sourceTime, NodePtr fixed, Filter &&filter)
Computes and returns a certain transformation between the specified target and source node via the &#39;f...
Definition: Transformer.h:373
Basic reference implementation of an AbstractTransformerNode that stores the transformation data inte...
Definition: TransformerNode.h:179
static NodePtr castNode(AbstractNodePtr node)
Casts an abstract node pointer to the actual used Node. This is safe.
Definition: Transformer.h:535