Permalink
Browse files

Base classes of the FrameStore implementation.

Feel free to review the stuff and tell me what I did wrong.
Test files are next on my list.
Missing from the commit is also the actual FrameStore class, which needs
a bit more polish, before I add it.

The story so far:
- ATrans: Affine transform class
  * as the matrix is not necessarily a rotation matrix, the 
    accessor is probably named wrong
  * tries to be "compatible" with other matrix classes,
    but is probalby missing some features in that respect
- TreeNode: Generic tree implementation for n-iary trees
  * could be probably used in other places, where trees
    with varying number of childs are required, so feel free
    to take a look if you'd like to generalize it further
  * it's pretty much optimized for child->parent lookup
    the other way round is slow, due to the use of
    single-linked, un-ordered list.
- Frame: Named affine transform
  * Is probably missing some helper methods to beautify
    rpy assignements etc
  * The RAPID folks would most likely want to see a vector<string>
    extras be added
  * The typical use case would probably benefit from the ability
    to cache single-rotation-axis updates
- FrameTreeNode: Typedef and helpers for a tree of frame-transforms

I also added doxygen documenation and added the Geometry module to the
Doxyfile. Please revert, if Geometry is not intended to be documented
by default.
  • Loading branch information...
1 parent b407717 commit da6aac5731661ecb97db497c73de2defa21dd6ff Hans Utz committed Nov 30, 2009
View
@@ -82,7 +82,7 @@ WARN_LOGFILE =
#---------------------------------------------------------------------------
# configuration options related to the input files
#---------------------------------------------------------------------------
-INPUT = vw vw/Core vw/Image vw/Math vw/FileIO vw/Camera vw/HDR vw/Mosaic vw/Cartography vw/GPU
+INPUT = vw vw/Core vw/Image vw/Math vw/FileIO vw/Camera vw/HDR vw/Mosaic vw/Cartography vw/GPU vw/Geometry
FILE_PATTERNS = *.h
RECURSIVE = NO
EXCLUDE =
View
@@ -17,5 +17,9 @@
#include <vw/Geometry/Sphere.h>
#include <vw/Geometry/SpatialTree.h>
#include <vw/Geometry/PointListIO.h>
+#include <vw/Geometry/ATrans.h>
+#include <vw/Geometry/Frame.h>
+#include <vw/Geometry/TreeNode.h>
+#include <vw/Geometry/FrameTreeNode.h>
#endif // __VW_GEOMETRY_H__
@@ -0,0 +1,164 @@
+// -*- c++ -*-
+#ifndef vw_geometry_ATrans_h
+#define vw_geometry_ATrans_h
+
+#include "vw/Math/Vector.h"
+#include "vw/Math/Matrix.h"
+
+namespace vw
+{
+namespace geometry
+{
+ template<class ElemT, int SizeN>
+ class ATrans;
+
+ //! Helper function for inverting an affine transform.
+ template<class ElemT, int SizeN>
+ ATrans<ElemT, SizeN> inverse(ATrans<ElemT, SizeN> const& atrans);
+
+ //! Output stream operator for affine transformation class.
+ template<class ElemT, int SizeN>
+ std::ostream& operator<< (std::ostream& ostr, ATrans<ElemT, SizeN> const& rhs);
+
+
+ //! Class representing an affine transform.
+ /**
+ * Affine transforms of dimension N consist of a NxN matrix and an
+ * vector of size N. An affine transform of dimension N can be
+ * expressed as a matrix of dimension (N+1)x(N+1) with the last row
+ * consisting of [0, 0, ..., 1]. While the later allows for a more
+ * compact representation, the former is computationally more
+ * efficient.
+ *
+ * This class represents affine transforms internally as matrix plus
+ * vector. The user-visible part tries hard to follow the
+ * (N+1)x(N+1) matrix notation.
+ *
+ * The primary customers of this class are the Frame &
+ * FrameStore classes. The affine transformation in this case holds
+ * a position-vector and rotation matrix.
+ *
+ */
+ template<class ElemT = double, int SizeN = 3>
+ class ATrans
+ {
+ public:
+ //! Vector type (translation part)
+ typedef vw::math::Vector<ElemT, SizeN> VectorT;
+ //! Matrix type (rotation part)
+ typedef vw::math::Matrix<ElemT, SizeN, SizeN> MatrixT;
+
+ //! Default constructor
+ ATrans() {}
+ //! Initializing constructor
+ ATrans(VectorT const& p, MatrixT const& r) : m_p(p), m_r(r) {}
+ //! Initialize from transformation matrix
+ ATrans(vw::math::Matrix<ElemT, SizeN + 1, SizeN + 1> const& m);
+ //! Initialize from transformation matrix
+ ATrans(vw::math::Matrix<ElemT, 0, 0> const& m);
+
+
+ //! Const accessor of the translation vector.
+ VectorT const& translation() const throw() { return m_p; }
+ //! Accessor of the translation vector.
+ VectorT& translation() throw() { return m_p; }
+
+ //! Const accessor of the (rotation) matrix.
+ MatrixT const& rotation() const throw() { return m_r; }
+ //! Accessor of the (rotation) matrix.
+ MatrixT& rotation() throw() { return m_r; }
+
+ //! Matrix N+1 style multiplication
+ ATrans operator*(ATrans const& rhs) const;
+ //! In-place matrix N+1 style multiplication
+ void operator*=(ATrans const& rhs);
+ //! Position vecotr multiplication
+ VectorT operator*(VectorT const& rhs) const;
+
+ //! Conversion to matrix N+1
+ operator vw::math::Matrix<ElemT, SizeN + 1, SizeN + 1>();
+
+ protected:
+ //! Position vector of the affine transform.
+ VectorT m_p;
+ //! (Rotation) matrix of the affine transform.
+ MatrixT m_r;
+ };
+
+ template<class ElemT, int SizeN>
+ inline
+ ATrans<ElemT, SizeN> inverse(ATrans<ElemT, SizeN> const& atrans)
+ {
+ typename ATrans<ElemT, SizeN>::MatrixT mI = inverse(atrans.rotation());
+ return ATrans<ElemT, SizeN>(mI * atrans.translation(), mI);
+ }
+
+ template<class ElemT, int SizeN>
+ std::ostream& operator<< (std::ostream& ostr, ATrans<ElemT, SizeN> const& rhs)
+ {
+ ostr << "xATrans(" << rhs.translation() << "), (" << rhs.rotation() << ")";
+ return ostr;
+ }
+
+
+ template<class ElemT, int SizeN>
+ inline
+ ATrans<ElemT, SizeN>::ATrans(vw::math::Matrix<ElemT, SizeN + 1, SizeN + 1> const& m) :
+ m_p(m(0, 3), m(1,3), m(2, 3))
+ {
+ for (int i = 0; i < SizeN; ++i) {
+ for (int j = 0; j < SizeN; ++j) {
+ m_r(i, j) = m(i, j);
+ }
+ }
+ }
+
+ template<class ElemT, int SizeN>
+ inline
+ ATrans<ElemT, SizeN>::ATrans(vw::math::Matrix<ElemT, 0,0> const& m) :
+ m_p(m(0, 3), m(1,3), m(2, 3))
+ {
+ for (int i = 0; i < SizeN; ++i) {
+ for (int j = 0; j < SizeN; ++j) {
+ m_r(i, j) = m(i, j);
+ }
+ }
+ }
+
+ template<class ElemT, int SizeN>
+ inline
+ ATrans<ElemT, SizeN>
+ ATrans<ElemT, SizeN>::operator*(ATrans const& rhs) const {
+ return ATrans(m_r * rhs.m_p + m_p, m_r * rhs.m_r);
+ }
+
+ template<class ElemT, int SizeN>
+ inline
+ void
+ ATrans<ElemT, SizeN>::operator*=(ATrans const& rhs) {
+ *this = *this * rhs;
+ }
+
+ template<class ElemT, int SizeN>
+ inline
+ typename ATrans<ElemT, SizeN>::VectorT
+ ATrans<ElemT, SizeN>::operator*(VectorT const& p) const {
+ VectorT v = m_r * p;
+ return v + p;
+ }
+
+ template<class ElemT, int SizeN>
+ inline
+ ATrans<ElemT, SizeN>::operator vw::math::Matrix<ElemT, SizeN+1, SizeN+1>()
+ {
+ vw::math::Matrix<ElemT, SizeN+1, SizeN+1> m;
+ return m;
+ }
+}
+
+ typedef geometry::ATrans<double, 3> ATrans3;
+ typedef geometry::ATrans<float, 3> ATrans3f;
+}
+
+#endif // vw_geometry_ATrans_h
+
@@ -0,0 +1,50 @@
+// -*- C++ -*-
+#ifndef vw_Math_Frame_h
+#define vw_Math_Frame_h
+
+#include "ATrans.h"
+
+#include <string>
+
+namespace vw
+{
+namespace geometry
+{
+
+ /** Class representing a named coordinate transform.
+ *
+ * This class is mostly used in combination with the TreeNode
+ * class to create a frame tree.
+ */
+ class Frame
+ {
+ public:
+ typedef vw::ATrans3 Location;
+
+ /// Default constructor.
+ Frame() {}
+ /// Initializing constructor.
+ Frame(std::string const& name, Location const& loc = identity_matrix<4>()) :
+ m_name(name),
+ m_loc(loc)
+ {}
+
+ /// @{ Accessor methods
+
+ /// Access name field.
+ std::string const& name() const throw() { return m_name; }
+ /// Set name field.
+ void set_name(std::string const& name) { m_name = name; }
+ /// Access location field.
+ Location const& location() const throw() { return m_loc; }
+ /// Set location field.
+ void set_location(Location const& loc) { m_loc = loc; }
+ /// @}
+
+ protected:
+ std::string m_name;
+ Location m_loc;
+ };
+}
+}
+#endif // vw_Math_Frame_h
@@ -0,0 +1,109 @@
+#include "FrameTreeNode.h"
+
+#include <algorithm>
+
+namespace vw
+{
+namespace geometry
+{
+ using namespace std;
+
+ Frame::Location
+ get_location(FrameTreeNode const * wrtFrame, FrameTreeNode const * source)
+ {
+ Frame::Location loc = vw::identity_matrix(4);
+
+ if (source != NULL) {
+
+ if (wrtFrame == NULL)
+ return source->data().location();
+
+ if (wrtFrame != source) {
+
+ FrameTreeNode const * ancestor = wrtFrame->last_common_ancestor(source);
+
+ if (ancestor != NULL) {
+ // from the origin frame to the ancestor
+ {
+ FrameTreeNode::NodeVector const& nodes = wrtFrame->ancestry(true);
+ FrameTreeNode::NodeVector::const_iterator iter =
+ std::find(nodes.begin(), nodes.end(), ancestor);
+
+ assert (iter != nodes.end());
+
+ ++iter;
+ if (iter != nodes.end()) {
+ for (; iter != nodes.end(); ++iter) {
+ loc *= (*iter)->data().location();
+ }
+ loc = inverse(loc);
+ }
+ }
+
+
+ // from the last common ancestor to the wrt frame
+ {
+ FrameTreeNode::NodeVector const& nodes = source->ancestry(true);
+ FrameTreeNode::NodeVector::const_iterator iter =
+ std::find(nodes.begin(), nodes.end(), ancestor);
+
+ assert (iter != nodes.end());
+
+ for (; iter != nodes.end(); ++iter) {
+ loc *= (*iter)->data().location();
+ }
+ }
+ }
+ }
+ }
+
+ return loc;
+ }
+
+ namespace
+ {
+ struct FtnLess
+ {
+ bool operator () (FrameTreeNode const * lhs, FrameTreeNode const * rhs)
+ {
+ return lhs->data().name() < rhs->data().name();
+ }
+ };
+ }
+
+ void
+ merge_frame_trees(FrameTreeNode * target_tree, FrameTreeNode * source_tree)
+ {
+ if (target_tree->data().name() != source_tree->data().name())
+ return;
+
+ FrameTreeNode::NodeVector src_children = target_tree->children();
+
+ if (src_children.size() > 0) {
+ FrameTreeNode::NodeVector tgt_children = source_tree->children();
+
+ FtnLess less;
+ sort(src_children.begin(), src_children.end(), less);
+ sort(tgt_children.begin(), tgt_children.end(), less);
+
+ FrameTreeNode::NodeVector::const_iterator first, last = src_children.end();
+ FrameTreeNode::NodeVector::const_iterator tgt_iter = tgt_children.end();
+ for (first = src_children.begin(); first != last; ++first) {
+ while (tgt_iter != tgt_children.end() &&
+ (*first)->data().name() > (*tgt_iter)->data().name()) {
+ ++tgt_iter;
+ }
+
+ if (tgt_iter == tgt_children.end() ||
+ (*first)->data().name() < (*tgt_iter)->data().name()) {
+ (*first)->set_parent(target_tree);
+ }
+ else if ((*first)->data().name() == (*tgt_iter)->data().name()) {
+ merge_frame_trees(*tgt_iter, *first);
+ }
+ }
+
+ }
+ }
+}
+}
Oops, something went wrong.

0 comments on commit da6aac5

Please sign in to comment.