iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
TransformManager.H
Go to the documentation of this file.
1 /*! @file
2  @author Unknown
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 #ifndef INCLUDE_NRT_ROBOTICS_TRANSFORMMANAGER_H
35 #define INCLUDE_NRT_ROBOTICS_TRANSFORMMANAGER_H
36 
40 #include <deque>
41 #include <unordered_map>
42 #include <sstream>
43 
44 namespace nrt
45 {
46  class TransformManagerNode
47  {
48  public:
49  typedef std::string _Key;
50  typedef std::shared_ptr<TransformManagerNode> _Ptr;
51  // typedef std::unordered_map<_Key, _Ptr> _NodeMap; not used
52  typedef Eigen::Affine3d _Transform;
53 
54  TransformManagerNode( _Key const & name, _Ptr const & parent = NULL, _Transform const & transform = _Transform() )
55  :
56  itsName( name ), itsParent( parent ), itsTransform( transform )
57  {
58  }
59 
60  public:
61  _Key itsName;
62  _Ptr itsParent;
63  // _NodeMap itsChildren; not used
64  _Transform itsTransform;
65  };
66 
67  class TransformManager
68  {
69  public:
70  typedef TransformManagerNode _Node;
71  typedef _Node::_Key _Key;
72  typedef std::unordered_map<_Key, _Node::_Ptr> _FramePtrMap;
73  typedef _Node::_Transform _Transform;
74  typedef TransformMessage _TransformMessage;
75  typedef TransformLookupMessage _TransformLookup;
76 
77  enum Mode
78  {
79  //! Only a single root, named "world", is allowed
80  /*! - The world frame cannot be parented by any other frame
81  * - Only child frames can be added
82  * - The parent of the first frame added must be the world frame
83  * - The parent of all subsequent frames must be the world frame or another existing frame */
84  SINGLE_TREE = 0,
85  //! Frames can be added in any order and multiple trees are allowed
86  MULTIPLE_TREES = 1
87  };
88 
89  TransformManager( int const & mode = Mode::SINGLE_TREE )
90  :
91  itsMode( mode )
92  {
93  if( itsMode == Mode::SINGLE_TREE ) itsFrames[_Key("world")] = _Node::_Ptr( new _Node( "world", NULL, _Transform::Identity() ) );
94  }
95 
96  void setMode( int const & mode )
97  {
98  if( mode < 0 || mode > 1 ) return;
99  itsMode = mode;
100  }
101 
102  _TransformMessage::unique_ptr lookupTransform( _Key const & fromFrameId, _Key const & toFrameId )
103  {
104  auto const frame1_it = itsFrames.find( fromFrameId );
105  auto const frame2_it = itsFrames.find( toFrameId );
106 
107  if( frame1_it == itsFrames.end() ) throw nrt::exception::InvalidTransform( fromFrameId, toFrameId, fromFrameId + " does not exist." );
108  if( frame2_it == itsFrames.end() ) throw nrt::exception::InvalidTransform( fromFrameId, toFrameId, toFrameId + " does not exist." );
109 
110  _TransformMessage result;
111  result.fromFrameId = fromFrameId;
112  result.toFrameId = toFrameId;
113 
114  if( fromFrameId == toFrameId ) return std::unique_ptr<_TransformMessage>(new _TransformMessage(result));
115 
116  typedef std::deque<_Node::_Ptr> _NodePtrSet;
117 
118  _NodePtrSet visitedFrames1, visitedFrames2;
119 
120  _Node::_Ptr frame1 = itsFrames[fromFrameId];
121  _Node::_Ptr frame2 = itsFrames[toFrameId];
122 
123  visitedFrames1.push_front( frame1 );
124  visitedFrames2.push_front( frame2 );
125 
126  // Transform transform1 = frame1->itsTransform;
127  // Transform transform2 = frame2->itsTransform;
128 
129  _Node::_Ptr commonParent( NULL );
130 
131  //std::stringstream path1, path2;
132  Eigen::Affine3d path1 = Eigen::Affine3d::Identity();
133  Eigen::Affine3d path2 = Eigen::Affine3d::Identity();
134 
135  // continue until we find a common parent
136  while( !commonParent )
137  {
138  // only step up frame1's tree if frame1 has a parent
139  if( frame1->itsParent )
140  {
141  frame1 = frame1->itsParent;
142  // add the new parent to the list of visited frames
143  visitedFrames1.push_front( frame1 );
144  }
145 
146  // only step up frame2's tree if frame2 has a parent
147  if( frame2->itsParent )
148  {
149  frame2 = frame2->itsParent;
150  // add the new parent to the list of visited frames
151  visitedFrames2.push_front( frame2 );
152  }
153 
154  // for debugging purposees only; print out visitedFrames
155  /*std::stringstream visitedFrames1ss;
156  for( auto const & frame : visitedFrames1 )
157  {
158  visitedFrames1ss << frame->itsTransform << " | ";
159  }
160  NRT_DEBUG( "visitedFrames1: " << visitedFrames1ss.str() );
161 
162  std::stringstream visitedFrames2ss;
163  for( auto const & frame : visitedFrames2 )
164  {
165  visitedFrames2ss << frame->itsTransform << " | ";
166  }
167  NRT_DEBUG( "visitedFrames2: " << visitedFrames2ss.str() );
168  NRT_DEBUG( "----------" );*/
169 
170  // loop through visitedFrames1 and search for the current frame in visitedFrames2
171  for( auto const & visitedFrame1 : visitedFrames1 )
172  {
173  auto match = std::find( visitedFrames2.begin(), visitedFrames2.end(), visitedFrame1 );
174 
175  // if a frame was found, that frame must be the common parent of the original frame1 and frame2
176  if( match != visitedFrames2.end() )
177  {
178  commonParent = *match;
179  break;
180  }
181  }
182 
183  /*if( !commonParent )
184  {
185  if( frame1->itsParent ) path1 << " * " << frame1->itsTransform;
186  if( frame2->itsParent ) path2 << " * " << frame2->itsTransform;
187  }*/
188 
189  // transform1 *= frame1->itsTransform;
190  // transform2 *= frame2->itsTransform;
191  }
192 
193  // multiply out the transforms starting at the first visited frame and ending at the common parent frame
194  // rbegin() will necessarily be the bottom of the tree
195  // traversing up the tree necessarily involves reversing the stored transform
196  for( auto frame = visitedFrames1.rbegin(); frame != visitedFrames1.rend(); ++frame )
197  {
198  if( *frame == commonParent ) break;
199  path1 = path1 * (*frame)->itsTransform.inverse();
200  //path1 << " * " << (*frame)->itsTransform;
201  }
202 
203  // multiply out the transforms starting at the first visited frame and ending at the common parent frame
204  for( auto frame = visitedFrames2.rbegin(); frame != visitedFrames2.rend(); ++frame )
205  {
206  if( *frame == commonParent ) break;
207  path2 = path2 * (*frame)->itsTransform.inverse();
208  //path2 << " * " << (*frame)->itsTransform;
209  }
210 
211  // result_transform ( transform1 * transform2 );
212 
213  // NRT_INFO( "Found transform: ( " << path1.str() << " ) * ( " << path2.str() << " )" );
214  result.transform = path1 * path2.inverse();
215 
216  if( !commonParent ) NRT_WARNING( "commonParent is null!" );
217 
218  return std::unique_ptr<_TransformMessage>(new _TransformMessage(result) );
219 
220  }
221 
222  _TransformMessage::unique_ptr lookupTransform( _TransformLookup::const_ptr const & lookupMessage )
223  {
224  return lookupTransform( lookupMessage->fromFrameId, lookupMessage->toFrameId );
225  }
226 
227  void updateTransform( _TransformMessage::const_ptr const & transformMessage )
228  {
229  NRT_INFO( "Attempting to update transform " << transformMessage->fromFrameId << " -> " << transformMessage->toFrameId);
230 
231  auto child = itsFrames.find( transformMessage->toFrameId );
232  auto parent = itsFrames.find( transformMessage->fromFrameId );
233 
234  // if the parent frame doesn't exist already
235  if( parent == itsFrames.end() )
236  {
237  // if we're allowing multiple trees, create the new parent frame
238  if( itsMode == Mode::MULTIPLE_TREES )
239  {
240  NRT_DEBUG( "No parent found with name " << transformMessage->fromFrameId << "; creating a new one" );
241 
242  // add the new parent frame to the list of frames
243  //itsFrames[transformMessage->fromFrameId] = _Node::_Ptr( new _Node( NULL, _Transform( "--- " + transformMessage->fromFrameId + " ---" ) ) );
244  itsFrames[transformMessage->fromFrameId] = _Node::_Ptr( new _Node( transformMessage->fromFrameId, NULL, _Transform::Identity() ) );
245 
246  // get an iterator to parent to use when creating the new child frame
247  parent = itsFrames.find( transformMessage->fromFrameId );
248  }
249  // otherwise, we can't add this frame, so we're done
250  else
251  {
252  NRT_WARNING( "No parent found with name " << transformMessage->fromFrameId << "; multiple trees not allowed" );
253  return;
254  }
255  }
256  // otherwise we've found the parent frame
257  else NRT_DEBUG( "Parent frame " << transformMessage->fromFrameId << " found" );
258 
259  // by this point, parent is guaranteed to be an entry in itsFrames (or we would have bailed already); now we need to check the child frame
260 
261  // if the child frame doesn't exist already create it
262  if( child == itsFrames.end() )
263  {
264  NRT_DEBUG( "No child found with name " << transformMessage->toFrameId << "; creating it" );
265 
266  // add the new child frame to the list of frames
267  //itsFrames[transformMessage->toFrameId] = _Node::_Ptr( new _Node( parent->second, _Transform( transformMessage->toFrameId + " <-- " + transformMessage->fromFrameId ) ) );
268  itsFrames[transformMessage->toFrameId] = _Node::_Ptr( new _Node( transformMessage->toFrameId, parent->second, transformMessage->transform ) );
269  NRT_DEBUG( "Adding new child frame: " << *transformMessage );
270  }
271  // if the child frame exists already and its parent frame matches the given parent frame, simply update it
272  else if( child->second->itsParent == parent->second )
273  {
274  NRT_DEBUG( "Child frame " << transformMessage->toFrameId << " found; updating transform" );
275  child->second->itsTransform = transformMessage->transform;
276  NRT_DEBUG( "Updating frame: " << *transformMessage );
277  }
278  // if the child frame exists with no parent, then update its parent
279  else if( child->second->itsParent == NULL )
280  {
281  NRT_DEBUG( "Child frame " << transformMessage->toFrameId << " found with no parent; updating parent and transform" );
282  child->second->itsParent = parent->second;
283  child->second->itsTransform = transformMessage->transform;
284  NRT_DEBUG( "Updating frame: " << *transformMessage );
285  }
286  // if the child frame exists but its parent frame does not match the given parent frame, we're done; multiple parents are not allowed under any circumstances
287  else
288  {
289  NRT_WARNING( "Frame " << transformMessage->toFrameId << " already has parent " << child->second->itsParent->itsName );
290  }
291  }
292 
293  protected:
294  _FramePtrMap itsFrames;
295  int itsMode;
296  };
297 }
298 
299 #endif // INCLUDE_NRT_ROBOTICS_TRANSFORMMANAGER_H