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 
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 
169  Chain& oChain);
170 
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:74
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
void removeLink(AbstractNodePtr child, AbstractNodePtr parent)
Removes an existing link between the specified child node and the given parent node.
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
TransformerBase()
The default construtor.
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:78
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
~TransformerBase()
The destructor.
Describes a path of transformations through the transformation tree.
Definition: Transformer.h:229
Rigid transformation class.
const std::list< AbstractNodePtr > getRootNodes()
Returns a list of all nodes, that have no ancestor, e.g.
A full features transformer class based on GenericTransformer.
Definition: Transformer.h:573
bool addLink(AbstractNodePtr child, AbstractNodePtr parent)
Adds a link between the specified child and its parent.
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
bool isTransformAvailable(AbstractNodePtr target, AbstractNodePtr source)
Checks if a transformation between &#39;target&#39; and &#39;source&#39; node is available:
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
TransformDesc(const TransformerBase::Chain &c1, const TransformerBase::Chain &c2)
Definition: Transformer.h:233
std::list< AbstractNodePtr > getNodes()
Returns all nodes that are known by this transformer.
#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
static const int MAX_TREE_DEPTH
The max.
Definition: Transformer.h:88
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
std::list< std::pair< AbstractNodePtr, AbstractNodePtr > > getLinks()
Returns all links (as parent,child-pairs) that are known by this transformer.