init

.gitignore

+/*
+!*.gitignore
+!*.md
+!/src
+!/include
+!/tests
+!CMakeLists.txt

CMakeLists.txt

+cmake_minimum_required(VERSION 3.5.0)
+
+project(JJFrame VERSION 0.1.0 LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTORCC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+set(CMAKE_BUILD_TYPE release)
+add_definitions(-DQT_NO_VERSION_TAGGING)
+
+find_package(Qt5 REQUIRED COMPONENTS Core)
+find_package(Qt5 REQUIRED COMPONENTS Widgets)
+find_package(Qt5 REQUIRED COMPONENTS Gui)
+
+include_directories(
+    ${Qt5Core_INCLUDE_DIRS}
+    ${Qt5Widgets_INCLUDE_DIRS}
+    ${Qt5Gui_INCLUDE_DIRS}
+    ${PROJECT_SOURCE_DIR}/include/
+    ${PROJECT_SOURCE_DIR}/third-party/Eigen/
+    )
+
+link_directories(
+    ${QT_LIBRARY_DIR}
+)
+
+#If your project has custom CMake build configurations, you have to map your custom configuration to either the debug or the release Qt configuration.
+set(CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_RELEASE} -fprofile-arcs -ftest-coverage")
+
+set_target_properties(Qt5::Core PROPERTIES MAP_IMPORTED_CONFIG_COVERAGE "RELEASE")
+set_target_properties(Qt5::Widgets PROPERTIES MAP_IMPORTED_CONFIG_COVERAGE "RELEASE")
+set_target_properties(Qt5::Gui PROPERTIES MAP_IMPORTED_CONFIG_COVERAGE "RELEASE")
+
+add_subdirectory(src)
+#add_subdirectory(third-party)
+add_subdirectory(tests)
+

README.md

+# JJFrame
+
+统一的坐标系管理

include/frame.h

+#pragma once
+
+#include "print.h"
+#include <string>
+#include <Eigen/Core>
+
+namespace jj_universal {
+    class Frame : public Print
+    {
+    public:
+        Frame();        //don't use it
+        Frame(std::string name, Eigen::Matrix4d trans = Eigen::Matrix4d::Identity());
+        ~Frame();
+        friend std::ostream &operator<<(std::ostream& os, const Frame& obj){
+            os << obj.getName();
+            return os;
+        }
+
+        void printSimple() override;
+        void printVerbose() override;
+
+        Eigen::Matrix4d getTransformation() const;
+        void setTransformation(const Eigen::Matrix4d &value);
+
+        std::string getName() const;
+
+    private:
+        std::string m_name;
+        Eigen::Matrix4d m_transformation;
+
+    };
+}

include/frametree.h

+#pragma once
+
+#include "print.h"
+#include "frame.h"
+#include "tree.hh"
+#include <string>
+#include <Eigen/Core>
+#include <unordered_map>
+
+namespace jj_universal {
+    class FrameTree : public Print
+    {
+    public:
+        FrameTree();
+        ~FrameTree();
+        FrameTree(const FrameTree & c);
+        FrameTree copy();
+
+        void printSimple() override;
+        void printVerbose() override;
+
+        bool addFrame(const Frame& newFrame, std::string parent);
+        bool deleteFrame(std::string name);
+        bool addSubtree(FrameTree& subtree, std::string parent);
+        bool copySubtree(std::string rootOfSubtree);
+        Eigen::Matrix4d calculateTransformation(std::string child, std::string parent);
+        bool modifyTransformation(std::string name, Eigen::Matrix4d newTrans);
+    protected:
+
+    private:
+        tree<Frame> m_frameTree;
+        std::unordered_map<std::string, tree<Frame>::iterator_base> m_frameNameToIterMap;
+        bool isFrameNameRegistered(std::string name);
+
+        //for debugging
+        void print_map_by_key();
+
+
+    };
+}

include/print.h

+#pragma once
+
+namespace jj_universal {
+    //abstract class
+    class Print
+    {
+        virtual void printSimple() = 0;
+        virtual void printVerbose() = 0;
+    };
+}
+

include/tree_util.hh

+/* 
+
+	A collection of miscellaneous utilities that operate on the templated 
+	tree.hh class.
+
+
+	Copyright (C) 2001-2009  Kasper Peeters <kasper.peeters@aei.mpg.de>
+
+	(At the moment this only contains a printing utility, thanks to Linda
+	Buisman <linda.buisman@studentmail.newcastle.edu.au>)
+
+   This program is free software: you can redistribute it and/or
+   modify it under the terms of the GNU General Public License as
+   published by the Free Software Foundation, either version 3 of the
+   License, or (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+   General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ 
+*/
+
+#ifndef tree_util_hh_
+#define tree_util_hh_
+
+#include <iostream>
+#include "tree.hh"
+
+namespace kptree {
+
+template<class T>
+void print_tree_bracketed(const tree<T>& t, std::ostream& str=std::cout);
+
+template<class T>
+void print_subtree_bracketed(const tree<T>& t, typename tree<T>::iterator iRoot, 
+									  std::ostream& str=std::cout);
+
+
+
+// Iterate over all roots (the head) and print each one on a new line
+// by calling printSingleRoot.
+
+template<class T>
+void print_tree_bracketed(const tree<T>& t, std::ostream& str) 
+	{
+	int headCount = t.number_of_siblings(t.begin());
+	int headNum = 0;
+	for(typename tree<T>::sibling_iterator iRoots = t.begin(); iRoots != t.end(); ++iRoots, ++headNum) {
+		print_subtree_bracketed(t,iRoots,str);
+		if (headNum != headCount) {
+			str << std::endl;
+			}
+		}
+	}
+
+
+// Print everything under this root in a flat, bracketed structure.
+
+template<class T>
+void print_subtree_bracketed(const tree<T>& t, typename tree<T>::iterator iRoot, std::ostream& str) 
+	{
+	if(t.empty()) return;
+	if (t.number_of_children(iRoot) == 0) {
+		str << *iRoot;	
+		}
+	else {
+		// parent
+		str << *iRoot;
+		str << "(";
+		// child1, ..., childn
+		int siblingCount = t.number_of_siblings(t.begin(iRoot));
+		int siblingNum;
+		typename tree<T>::sibling_iterator iChildren;
+		for (iChildren = t.begin(iRoot), siblingNum = 0; iChildren != t.end(iRoot); ++iChildren, ++siblingNum) {
+			// recursively print child
+			print_subtree_bracketed(t,iChildren,str);
+			// comma after every child except the last one
+			if (siblingNum != siblingCount ) {
+				str << ", ";
+				}
+			}
+		str << ")";
+		}
+	}
+
+}
+
+#endif

src/CMakeLists.txt

+set(JJFRAME_HEADERS
+    ${PROJECT_SOURCE_DIR}/include/frame.h
+    ${PROJECT_SOURCE_DIR}/include/frametree.h
+    ${PROJECT_SOURCE_DIR}/include/tree.hh
+    ${PROJECT_SOURCE_DIR}/include/tree_util.hh
+)
+
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include/
+)
+
+set (JJFRAME_SOURCES
+    frame.cpp
+    frametree.cpp
+)
+
+add_library(JJFrame ${JJFRAME_HEADERS} ${JJFRAME_SOURCES})
+
+target_link_libraries(JJFrame
+
+)
+
+
+
+
+

src/frame.cpp

+#include "frame.h"
+
+#include <iostream>
+
+using namespace jj_universal;
+
+Frame::Frame()
+{
+    m_name = "world";
+    m_transformation = Eigen::Matrix4d::Identity();
+}
+
+Frame::Frame(std::string name, Eigen::Matrix4d trans)
+{
+    m_name = name;
+    m_transformation = trans;
+}
+
+Frame::~Frame()
+{
+
+}
+
+void Frame::printSimple()
+{
+    std::cout<<"name: " << m_name<<std::endl;
+}
+
+void Frame::printVerbose()
+{
+    std::cout<<"address: " <<this<<std::endl;
+    std::cout<<"name: " << m_name<<std::endl;
+    std::cout<<"trans: " <<std::endl;
+    std::cout<<m_transformation<<std::endl;
+}
+
+Eigen::Matrix4d Frame::getTransformation() const
+{
+    return m_transformation;
+}
+
+void Frame::setTransformation(const Eigen::Matrix4d &value)
+{
+    m_transformation = value;
+}
+
+std::string Frame::getName() const
+{
+    return m_name;
+}

src/frametree.cpp

+#include "frametree.h"
+#include "frame.h"
+#include "tree_util.hh"
+
+#include <iostream>
+#include <Eigen/Eigen>
+#include <type_traits>
+
+using namespace jj_universal;
+
+FrameTree::FrameTree()
+{
+    auto top = m_frameTree.insert(m_frameTree.begin(), Frame("root"));
+    m_frameNameToIterMap.emplace((*top).getName(), top);
+//    auto iter111 = m_frameTree.append_child(top, Frame("111"));
+//    m_frameTree.append_child(iter111, Frame("333"));
+//    m_frameTree.append_child(iter111, Frame("444"));
+//    m_frameTree.append_child(top, Frame("555"));
+//    auto target = m_frameTree.append_child(iter111, Frame("666"));
+//    auto path = m_frameTree.path_from_iterator(target, top);
+//    for(auto& p: path)
+//       std::cerr << p << "/";
+//    std::cerr << std::endl;
+//    auto find = m_frameTree.iterator_from_path(path, top);
+//    std::cerr << *find << std::endl;
+    addFrame(Frame("111"), "root");
+    addFrame(Frame("222"), "111");
+    addFrame(Frame("333"), "root");
+    addFrame(Frame("444"), "root");
+    addFrame(Frame("555"), "111");
+    addFrame(Frame("666"), "333");
+    addFrame(Frame("777"), "222");
+//    printSimple();
+//    std::cout<<"////"<<std::endl;
+//    deleteFrame("111");
+//    std::cout<<"////"<<std::endl;
+//    calculateTransformation("777", "555");
+    FrameTree ff = *this;
+    ff.deleteFrame("111");
+    ff.printSimple();
+    std::cout<<".............."<<std::endl;
+    printSimple();
+
+
+}
+
+FrameTree::~FrameTree()
+{
+
+}
+
+FrameTree::FrameTree(const FrameTree & c)
+{
+    auto top = m_frameTree.insert(m_frameTree.begin(), Frame("root"));
+    m_frameNameToIterMap.emplace((*top).getName(), top);
+    for(auto parentIter = c.m_frameTree.begin(); parentIter != c.m_frameTree.end(); parentIter++)
+    {
+        tree<Frame>::sibling_iterator childIter = parentIter;
+        for(auto child : childIter)
+        {
+            addFrame(child, parentIter->getName());
+        }
+    }
+
+}
+
+void FrameTree::printSimple()
+{
+    tree<Frame>::iterator loc = m_frameTree.begin();
+    while(loc!=m_frameTree.end())
+    {
+      for(int i=0; i<m_frameTree.depth(loc); ++i)
+         std::cout << " |";
+      std::cout << (*loc).getName() << std::endl;
+      ++loc;
+    }
+
+}
+
+void FrameTree::printVerbose()
+{
+
+}
+
+
+bool FrameTree::addFrame(const Frame& newFrame, std::string parent)
+{
+    //TODO: find parent failed, should give error info
+    tree<Frame>::iterator_base parentIter = m_frameNameToIterMap[parent];
+    auto currentIter = m_frameTree.append_child(parentIter, newFrame);
+    m_frameNameToIterMap.emplace(newFrame.getName(), currentIter);
+    return true;
+}
+
+bool FrameTree::deleteFrame(std::string name)
+{
+
+    tree<Frame>::iterator targetIter = m_frameNameToIterMap[name];
+    m_frameNameToIterMap.erase(m_frameNameToIterMap.find((*targetIter).getName()));
+    tree<Frame>::iterator iter = targetIter.begin();
+    while(iter!=targetIter.end())
+    {
+      m_frameNameToIterMap.erase(m_frameNameToIterMap.find((*iter).getName()));
+      ++iter;
+    }
+    m_frameTree.erase(targetIter);
+    return true;
+}
+
+bool FrameTree::addSubtree(FrameTree& subtree, std::string parent)
+{
+    auto parentIter = m_frameNameToIterMap[parent];
+    m_frameTree.insert_subtree(parentIter, subtree.m_frameTree.begin());
+//    m_frameNameToIterMap.emplace(subtree.m_frameNameToIterMap);
+}
+
+Eigen::Matrix4d FrameTree::calculateTransformation(std::string child, std::string parent)
+{
+   std::vector<int> pathToChild = m_frameTree.path_from_iterator(m_frameNameToIterMap[child], m_frameTree.begin());
+   std::vector<int> pathToParent = m_frameTree.path_from_iterator(m_frameNameToIterMap[parent], m_frameTree.begin());
+//   std::cout<<"********child***********"<<std::endl;
+//   for(auto child: pathToChild)
+//   {
+//       std::cout<<child;
+//   }
+//   std::cout<<std::endl;
+//   std::cout<<"********parent***********"<<std::endl;
+//   for(auto parent: pathToParent)
+//   {
+//       std::cout<<parent;
+//   }
+//   std::cout<<std::endl;
+    Eigen::Matrix4d childTrans = Eigen::Matrix4d::Identity();
+    while(pathToChild.size()>0)
+    {
+        auto frameIter = m_frameTree.iterator_from_path(pathToChild, m_frameTree.begin());
+        childTrans = frameIter->getTransformation() * childTrans;
+        pathToChild.pop_back();
+    }
+    std::cout<<"childTrans"<<std::endl;
+    std::cout<<childTrans<<std::endl;
+    Eigen::Matrix4d parentTrans = Eigen::Matrix4d::Identity();
+    while(pathToParent.size()>0)
+    {
+        auto frameIter = m_frameTree.iterator_from_path(pathToParent, m_frameTree.begin());
+        parentTrans = frameIter->getTransformation() * parentTrans;
+        pathToParent.pop_back();
+    }
+    std::cout<<"parentTrans"<<std::endl;
+    std::cout<<parentTrans<<std::endl;
+    return parentTrans.inverse() * childTrans;
+
+}
+
+bool FrameTree::modifyTransformation(std::string name, Eigen::Matrix4d newTrans)
+{
+    m_frameNameToIterMap[name]->setTransformation(newTrans);
+    return true;
+}
+
+bool FrameTree::isFrameNameRegistered(std::string name)
+{
+    if(m_frameNameToIterMap.count(name))
+    {
+        //TODO: notify the frame has been registered
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void FrameTree::print_map_by_key()
+{
+    for(auto key: m_frameNameToIterMap)
+    {
+        std::cout<<key.second->getName()<<std::endl;
+    }
+}

tests/CMakeLists.txt

+
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include/
+    /home/bohan/Qt5.12.3/5.12.3/gcc_64/lib
+    ${CMAKE_CURRENT_BINARY_DIR}
+
+    )
+
+set(FRAME_TEST1_SOURCE
+
+    test1.cpp
+
+)
+
+add_executable(test1
+
+    ${FRAME_TEST1_SOURCE}
+    )
+
+
+
+target_link_libraries(test1
+    Qt5::Core
+    Qt5::Widgets
+    JJFrame)
+
+set(FRAME_TEST2_SOURCE
+    test2.cpp
+)
+add_executable(test2
+    ${FRAME_TEST2_SOURCE}
+    )
+
+
+
+target_link_libraries(test2
+    Qt5::Core
+    Qt5::Widgets
+    JJFrame)
+

tests/test1.cpp

+#include <iostream>
+#include "frame.h"
+
+
+int main() {
+    jj_universal::Frame f("root");
+    f.printSimple();
+    f.printVerbose();
+}
+
+

tests/test2.cpp

+#include <iostream>
+#include "frame.h"
+#include "frametree.h"
+
+
+int main() {
+    jj_universal::FrameTree ft;
+    ft.printSimple();
+}
+
+