loading and saving needs a tiny little more debugging...

qTransformsPlugin.cpp

@@ -92,6 +92,8 @@ void qTransformsPlugin::recTravTree(ccHObject * obj, QString spaces){
     for(QVariantMap::const_iterator iter = meta.begin(); iter != meta.end(); ++iter) {
   		m_app->dispToConsole(spaces+iter.key()+"  "+iter.value().toString(),ccMainAppInterface::STD_CONSOLE_MESSAGE); //an error message is displayed in the console AND an error box will pop-up!
     }
+    //ccGLMatrix transf;
+    //obj->getAbsoluteGLTransformation(transf);
     const ccGLMatrix& transf = obj->getGLTransformationHistory();
 	m_app->dispToConsole(spaces+"  "+transf.toString(),ccMainAppInterface::WRN_CONSOLE_MESSAGE); //an error message is displayed in the console AND an error box will pop-up!
 
@@ -110,16 +112,20 @@ void qTransformsPlugin::recTravTree(ccHObject * obj, sttl::Transform * parent){
 		// this is a point cloud - will be a new parent...
 		sttl::Transform transform;
 		transform.frameName = obj->getName().toStdString();
+
+	    //ccGLMatrix transf;
+    	//obj->getAbsoluteGLTransformation(transf);
+
 		const ccGLMatrix& transf = obj->getGLTransformationHistory();
 		Eigen::Transform<double,3,Eigen::Affine> globalT;
 		for(int i = 0; i<16; ++i){
 			globalT.data()[i] = transf.data()[i];
 		}
+
 		// ToDo: properly compute the transform of this frame wrt the parent... - I think this is correct...
 		Eigen::Transform<double,3,Eigen::Affine> result = globalT * parent->transform.inverse();
 		transform.transform = result;
 
-
 		transform.referenceFrameName = parent->frameName;
 		transform.generateId();
 		m_transforms.push_back(transform);
@@ -131,6 +137,61 @@ void qTransformsPlugin::recTravTree(ccHObject * obj, sttl::Transform * parent){
 		recTravTree(obj->getChild(i), current);
 	}
 
+}
+
+void qTransformsPlugin::applyFrames(ccHObject * obj, std::list<sttl::Frame> &frames){
+
+	if(obj->getClassID() == CC_TYPES::POINT_CLOUD){
+		std::string name = obj->getName().toStdString();
+		std::list<sttl::Frame>::iterator found = findFrameForData(frames, name);
+		if(found == frames.end()){
+			QString wrn("Warning: did not find frame with name %1 in our transforms!");
+			wrn = wrn.arg(name.c_str());
+			m_app->dispToConsole(wrn,ccMainAppInterface::WRN_CONSOLE_MESSAGE); 
+		}else{
+			if(!found->visited){
+				QString wrn("Warning: found frame %1, but it has no valid pose!");
+				wrn = wrn.arg(name.c_str());
+				m_app->dispToConsole(wrn,ccMainAppInterface::WRN_CONSOLE_MESSAGE); 
+			}else{
+				// all good - apply the transform!
+				ccGLMatrix trans;
+				for(int i = 0; i<16; ++i){
+					trans.data()[i] = found->rootTransform.data()[i];
+				}
+				m_app->dispToConsole("Transformation applied to "+QString(name.c_str())+" : "+trans.toString(),ccMainAppInterface::WRN_CONSOLE_MESSAGE); //an error message is displayed in the console AND an error box will pop-up!
+
+//				ccGLMatrix backToOrigin;
+//    			obj->getAbsoluteGLTransformation(backToOrigin);
+				const ccGLMatrix& bb = obj->getGLTransformationHistory();
+				ccGLMatrix backToOrigin = bb;
+//    			obj->getAbsoluteGLTransformation(backToOrigin);
+				m_app->dispToConsole(QString(name.c_str())+" back : "+backToOrigin.toString(),ccMainAppInterface::WRN_CONSOLE_MESSAGE);
+
+    			backToOrigin = backToOrigin.inverse();
+
+				m_app->dispToConsole(QString(name.c_str())+" back.inv : "+backToOrigin.toString(),ccMainAppInterface::WRN_CONSOLE_MESSAGE);
+
+				m_app->dispToConsole(QString(name.c_str())+" back.inv * trans : "+(backToOrigin * trans).toString(),ccMainAppInterface::WRN_CONSOLE_MESSAGE);
+
+//				obj->resetGLTransformation();
+				obj->setGLTransformation(trans * backToOrigin);
+//				obj->setGLTransformationHistory(backToOrigin * trans);
+				obj->applyGLTransformation_recursive();
+				obj->prepareDisplayForRefresh_recursive();
+				obj->notifyGeometryUpdate();
+			}
+		}
+	}
+
+	for(int i=0; i<obj->getChildrenNumber(); ++i){
+		applyFrames(obj->getChild(i), frames);
+	}
+
+	m_app->refreshAll();
+		
+
+
 }
 
 void qTransformsPlugin::loadTransforms(){
@@ -145,6 +206,13 @@ void qTransformsPlugin::loadTransforms(){
 
 	std::cerr<<"We now have "<<m_transforms.size()<<" transforms! "<<std::endl;
 
+
+	std::list<sttl::Frame> frames;
+
+	generateFrames(m_transforms, frames);
+
+	applyFrames(getMainAppInterface()->dbRootObject(), frames);
+
 }
 
 void qTransformsPlugin::saveTransforms(){
@@ -159,8 +227,8 @@ void qTransformsPlugin::saveTransforms(){
 	rootFrame.referenceFrameName = "";
 	rootFrame.byUser = "Schwerti";
 
-	m_transforms.push_back(rootFrame);
-	recTravTree(root, &m_transforms.front());
+	//m_transforms.push_back(rootFrame);
+	recTravTree(root, &rootFrame);
 
 	QString file = QFileDialog::getSaveFileName(0, "SaveTransforms", "", "*.stt", 0 , 0); 
 

qTransformsPlugin.h

@@ -77,6 +77,9 @@ protected:
 
 	void recTravTree(ccHObject * obj, sttl::Transform * parent);
 
+
+	void applyFrames(ccHObject * obj, std::list<sttl::Frame> &frames);
+
 	std::list<sttl::Transform> m_transforms;
 
 	QAction* m_action;

sttl/sttl.cpp

@@ -209,4 +209,127 @@ bool sttl::loadList(std::list<sttl::Transform> & transfs, const std::string &fil
     return true;
 }
 
+std::list<sttl::Frame>::iterator sttl::findFrameForData(std::list<sttl::Frame> &frames, const std::string & dataName){
+	for(std::list<sttl::Frame>::iterator itr = frames.begin(); itr!= frames.end(); ++itr){
+		if(itr->name.compare(dataName) == 0 ){
+			return itr;
+		}
+	}
+	return frames.end();
+}
+
+std::list<sttl::Frame>::iterator sttl::findFrame(std::list<sttl::Frame> &frames, const std::string & frame){
+	for(std::list<sttl::Frame>::iterator itr = frames.begin(); itr!= frames.end(); ++itr){
+		if(itr->name.compare(frame) == 0 ){
+			return itr;
+		}
+	}
+	return frames.end();
+}
+
+/**
+ * If not already in the frames list add new frames
+ *
+ */
+void addFrames(std::list<sttl::Transform> & transforms, std::list<sttl::Frame> &frames){
+	for(std::list<sttl::Transform>::iterator itr = transforms.begin(); itr != transforms.end(); ++itr){
+		if(findFrame(frames, itr->frameName) == frames.end() ){
+			sttl::Frame frame;
+			frame.name = itr->frameName;
+			frames.push_back(frame);
+		}
+		if(findFrame(frames, itr->referenceFrameName) == frames.end() ){
+			sttl::Frame frame;
+			frame.name = itr->referenceFrameName;
+			frames.push_back(frame);
+		}
+	}
+}
+
+std::list<sttl::Transform>::iterator findTransform(std::list<std::list<sttl::Transform>::iterator> & transfs, const sttl::Transform::Id &id){
+	for(std::list<std::list<sttl::Transform>::iterator>::iterator itr = transfs.begin(); itr!=transfs.end(); ++itr){
+		if((*itr)->id == id) return *itr;
+	}
+	return std::list<sttl::Transform>::iterator();
+}
+
+/**
+ * Add those transforms to their respective frames (but not two with the same id)
+ * Assumes that the frame exists!
+ *
+ */
+void addTransforms(std::list<sttl::Transform> & transforms, std::list<sttl::Frame> &frames){
+	for(std::list<sttl::Transform>::iterator itr = transforms.begin(); itr != transforms.end(); ++itr){
+		std::list<sttl::Frame>::iterator frame = findFrame(frames, itr->frameName);
+		std::list<sttl::Frame>::iterator reference = findFrame(frames, itr->referenceFrameName);
+
+		if(findTransform(frame->parents, itr->id) == std::list<sttl::Transform>::iterator()){
+			// no transform found - add it!
+			frame->parents.push_back(itr);
+		}
+		if(findTransform(reference->children, itr->id) == std::list<sttl::Transform>::iterator()){
+			// no transform found - add it!
+			reference->children.push_back(itr);
+		}
+	}
+}
+
+/** sets the visited boolean in all frames to false
+ */
+void resetVisited(std::list<sttl::Frame> &frames){
+	for(std::list<sttl::Frame>::iterator itr = frames.begin(); itr!= frames.end(); ++itr){
+		itr->visited = false;
+	}
+}
+
+void calculateCoordinates(std::list<sttl::Frame> &frames, std::list<std::list<sttl::Transform>::iterator> &wavefront){
+
+	while(!wavefront.empty()){
+		std::list<sttl::Transform>::iterator trans = wavefront.front();
+		wavefront.pop_front();
+		std::list<sttl::Frame>::iterator frame = findFrame(frames, trans->frameName);
+		std::list<sttl::Frame>::iterator reference = findFrame(frames, trans->referenceFrameName);
+		if(frame->visited) continue;
+		if(!reference->visited){
+			std::cerr<<"Reference not visited - should not happen!!!"<<std::endl;
+			continue;
+		}
+		frame->rootTransform = reference->rootTransform * trans->transform;
+		frame->visited = true;
+		wavefront.insert(wavefront.end(), frame->children.begin(), frame->children.end());
+	}
+
+}
+
+bool sttl::generateFrames(std::list<sttl::Transform> & transforms, std::list<sttl::Frame> &frames){
+
+	cerr<<"frames size before: "<<frames.size()<<endl;
+	addFrames(transforms, frames);
+	cerr<<"frames size after: "<<frames.size()<<endl;
+	cerr<<"transforms size : "<<transforms.size()<<endl;
+
+	addTransforms(transforms, frames);
+
+	resetVisited(frames);
+
+	std::list<sttl::Frame>::iterator root = findFrame(frames, "root");
+
+	if(root == frames.end()){
+		cerr<<"Root frame not found!"<<endl;
+		return false;
+	}
+
+	cerr<<" Root parents (should be 0): "<<root->parents.size()<<"; root children: "<<root->children.size()<<std::endl;
+
+	std::list<std::list<Transform>::iterator> wavefront = root->children;
+
+	root->rootTransform = Eigen::Transform<double,3,Eigen::Affine>::Identity();
+	root->visited = true;
+
+	calculateCoordinates(frames, wavefront);
+
+	return true;
+}
+
+
 

sttl/sttl.h

@@ -75,10 +75,28 @@ static const std::string METHOD_MANUAL = "Manual";
 static const std::string METHOD_ICP = "ICP"; 
 static const std::string METHOD_G2O = "g2o"; 
 
+class Frame{
+public:
+	std::string name;
+	std::string dataNames;
+
+	std::list<std::list<sttl::Transform>::iterator> children;
+	std::list<std::list<sttl::Transform>::iterator> parents;
+
+	Eigen::Transform<double,3,Eigen::Affine> rootTransform; //< the transfrom of this frame wrt root
+
+	bool visited;
+};
+
+
+bool generateFrames(std::list<sttl::Transform> & transforms, std::list<Frame> &frames);
+
+std::list<sttl::Frame>::iterator findFrame(std::list<sttl::Frame> &frames, const std::string & frame);
+std::list<sttl::Frame>::iterator findFrameForData(std::list<sttl::Frame> &frames, const std::string & dataName);
 
+bool saveList(const std::list<sttl::Transform> & transforms, const std::string &file);
+bool loadList(std::list<sttl::Transform> & transforms, const std::string &file);
 
-bool saveList(const std::list<sttl::Transform> & frames, const std::string &file);
-bool loadList(std::list<sttl::Transform> & frames, const std::string &file);
 
 
 }