qTransformsPlugin.cpp

//##########################################################################
//#                                                                        #
//#                       CLOUDCOMPARE PLUGIN: qDummy                      #
//#                                                                        #
//#  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; version 2 of the License.               #
//#                                                                        #
//#  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.                          #
//#                                                                        #
//#                             COPYRIGHT: Sören Schwertfeg                #
//#                                                                        #
//##########################################################################

//First: replace all occurrences of 'qDummyPlugin' by your own plugin class name in this file!
#include "qTransformsPlugin.h"

//Qt
#include <QtGui>
#include <QFileDialog>
#include <QInputDialog>


#include "sttl.h"

//Default constructor: should mainly be used to initialize
//actions (pointers) and other members
qTransformsPlugin::qTransformsPlugin(QObject* parent/*=0*/)
	: QObject(parent)
	, m_action(0)
{
}

//This method should enable or disable each plugin action
//depending on the currently selected entities ('selectedEntities').
//For example: if none of the selected entities is a cloud, and your
//plugin deals only with clouds, call 'm_action->setEnabled(false)'
void qTransformsPlugin::onNewSelection(const ccHObject::Container& selectedEntities)
{
	//if (m_action)
	//	m_action->setEnabled(!selectedEntities.empty());
}

//This method returns all 'actions' of your plugin.
//It will be called only once, when plugin is loaded.
void qTransformsPlugin::getActions(QActionGroup& group)
{
	//default action (if it has not been already created, it's the moment to do it)
	if (!m_action)
	{
		//here we use the default plugin name, description and icon,
		//but each action can have its own!
		m_action = new QAction(getName(),this);
		m_action->setToolTip(getDescription());
		m_action->setIcon(getIcon());
		//connect appropriate signal
		connect(m_action, SIGNAL(triggered()), this, SLOT(doAction()));

		m_actionLoadTransform = new QAction("Load Transforms...",this);
		m_actionLoadTransform->setToolTip("Load transforms from a JSON file and apply to the visible point clouds.");
		m_actionLoadTransform->setIcon(getIcon());
		//connect appropriate signal
		connect(m_actionLoadTransform, SIGNAL(triggered()), this, SLOT(loadTransforms()));

		m_actionSaveTransform = new QAction("Save Transforms...",this);
		m_actionSaveTransform->setToolTip("Save transforms to a JSON file.");
		m_actionSaveTransform->setIcon(getIcon());
		//connect appropriate signal
		connect(m_actionSaveTransform, SIGNAL(triggered()), this, SLOT(saveTransforms()));

		m_changeUserAction = new QAction("Change User Name: ",this);
		m_changeUserAction->setToolTip("Change the user name...");
		m_changeUserAction->setIcon(getIcon());
		connect(m_changeUserAction, SIGNAL(triggered()), this, SLOT(changeUser()));
	
		m_changeMethodAction = new QAction("Change Registration Method: ",this);
		m_changeMethodAction->setToolTip("Change the method used for registration...");
		m_changeMethodAction->setIcon(getIcon());
		connect(m_changeMethodAction, SIGNAL(triggered()), this, SLOT(changeMethod()));

		m_changeDetailsAction = new QAction("Change Method Details",this);
		m_changeDetailsAction->setToolTip("Change some details about the registration...");
		m_changeDetailsAction->setIcon(getIcon());
		connect(m_changeDetailsAction, SIGNAL(triggered()), this, SLOT(changeDetails()));

		m_frameNameLength = 8;
		QString txt("Change Frame Name Length: %1");
		txt = txt.arg(m_frameNameLength);

		m_changeFrameNameLengthAction = new QAction(txt,this);
		m_changeFrameNameLengthAction->setToolTip("Change some details about the registration...");
		m_changeFrameNameLengthAction->setIcon(getIcon());
		connect(m_changeFrameNameLengthAction, SIGNAL(triggered()), this, SLOT(changeFrameNameLength()));

	}


	//group.addAction(m_action);
	group.addAction(m_changeUserAction);
	group.addAction(m_changeMethodAction);
	group.addAction(m_changeDetailsAction);
	group.addAction(m_changeFrameNameLengthAction);
	QAction * sep = new QAction(this);
	sep->setSeparator(true);
	group.addAction(sep);
	group.addAction(m_actionLoadTransform);
	group.addAction(m_actionSaveTransform);
}



void qTransformsPlugin::recTravTree(ccHObject * obj, QString spaces){
	QString num("Name: %1 Children %2 PointCloud %3");
	num = num.arg(obj->getName());
	num = num.arg(obj->getChildrenNumber());
	num = num.arg(obj->getClassID() == CC_TYPES::POINT_CLOUD);
	m_app->dispToConsole(spaces+num,ccMainAppInterface::WRN_CONSOLE_MESSAGE); //an error message is displayed in the console AND an error box will pop-up!

	const QVariantMap& meta = obj->metaData();
    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!



	spaces = spaces + QString("  ");
	for(int i=0; i<obj->getChildrenNumber(); ++i){
		recTravTree(obj->getChild(i), spaces);
	}

}

void qTransformsPlugin::recTravTree(ccHObject * obj, sttl::Transform * parent){
	sttl::Transform * current = parent;
	if(obj->getClassID() == CC_TYPES::POINT_CLOUD){
		// this is a point cloud - will be a new parent...
		sttl::Transform transform;

		transform.program = "Cloudcompare";
		transform.byUser = m_user.toStdString();
		transform.method = m_method.toStdString();
		transform.details = m_detials.toStdString();

		transform.time = QDateTime::currentDateTime().toString("yyyy.MM.dd hh:mm:ss").toStdString();


		//std::list<Id> history; // ToDo: History filling...

		transform.frameName = obj->getName().toStdString().substr(0,m_frameNameLength);

	    //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);
		current = &m_transforms.back();
	}

	// recursively call the children...
	for(int i=0; i<obj->getChildrenNumber(); ++i){
		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(){
	ccHObject * root = getMainAppInterface()->dbRootObject();

	QString file = QFileDialog::getOpenFileName(0, "SaveTransforms", "", "*.stt", 0 , 0); 

	m_transforms.clear();
	if(!file.isEmpty()){
		sttl::loadList(m_transforms, file.toStdString());
	}

	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(){

	while(m_user.isEmpty()) changeUser();
	while(m_method.isEmpty()) changeMethod();

	ccHObject * root = getMainAppInterface()->dbRootObject();
	recTravTree(root, QString(""));

	m_transforms.clear();
	sttl::Transform rootFrame;
	rootFrame.frameName = "root";
	rootFrame.transform = Eigen::Transform<double,3,Eigen::Affine>::Identity();
	rootFrame.referenceFrameName = "";
	rootFrame.byUser = "Schwerti";

	//m_transforms.push_back(rootFrame);
	recTravTree(root, &rootFrame);

	QString file = QFileDialog::getSaveFileName(0, "SaveTransforms", "", "*.stt", 0 , 0); 

	if(!file.isEmpty()){
		sttl::saveList(m_transforms, file.toStdString());
	}
	
}

void qTransformsPlugin::changeUser(){
	bool ok;
	QString name = QInputDialog::getText(0, "Provide the user Name (currently "+m_user+")", "New user name:",QLineEdit::Normal, m_user, &ok);
	if(ok && !name.isEmpty()) {
		m_user = name;
		m_changeUserAction->setText("Change User Name: "+m_user);
	}
}

void qTransformsPlugin::changeMethod(){
	QStringList list;
	list << sttl::METHOD_MANUAL.c_str() << sttl::METHOD_POINT_PAIRS.c_str() << sttl::METHOD_ICP.c_str();

	bool ok;
	QString method = QInputDialog::getItem(0, "Registration Method", "Method:", list, 0, true, &ok);
	if(ok && !method.isEmpty()) {
		m_method = method;
		m_changeMethodAction->setText("Change Registration Method: "+m_method);
	}
}

void qTransformsPlugin::changeDetails(){
	bool ok;
	QString details = QInputDialog::getText(0, "Change Details", "Details:", QLineEdit::Normal, m_detials, &ok);
	if(ok && !details.isEmpty()) {
		m_detials = details;
	}
}


void qTransformsPlugin::changeFrameNameLength(){
	bool ok;
	int len = QInputDialog::getInt(0, "Provide the frame name length", "New frame name length:", m_frameNameLength, 1, 255, 1, &ok);
	if(ok) {
		m_frameNameLength = len;
		QString txt("Change Frame Name Length: %1");
		txt = txt.arg(m_frameNameLength);
		m_changeFrameNameLengthAction->setText(txt);
	}
}


//This is an example of an action's slot called when the corresponding action
//is triggered (i.e. the corresponding icon or menu entry is clicked in CC's
//main interface). You can access to most of CC components (database,
//3D views, console, etc.) via the 'm_app' attribute (ccMainAppInterface
//object).
void qTransformsPlugin::doAction()
{
	//m_app should have already been initialized by CC when plugin is loaded!
	//(--> pure internal check)
	assert(m_app);
	if (!m_app)
		return;


	//This is how you can output messages
	m_app->dispToConsole("[qTransformsPlugin] Hello world!",ccMainAppInterface::STD_CONSOLE_MESSAGE); //a standard message is displayed in the console
	m_app->dispToConsole("[qTransformsPlugin] Warning: dummy plugin shouldn't be used as is!",ccMainAppInterface::WRN_CONSOLE_MESSAGE); //a warning message is displayed in the console
	//	m_app->dispToConsole("Transforms plugin shouldn't be used as is!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); //an error message is displayed in the console AND an error box will pop-up!


	ccHObject * root = getMainAppInterface()->dbRootObject();
	recTravTree(root, QString(""));

	m_transforms.clear();
	sttl::Transform rootFrame;
	rootFrame.frameName = "root";
	rootFrame.transform = Eigen::Transform<double,3,Eigen::Affine>::Identity();
	rootFrame.referenceFrameName = "";
	rootFrame.byUser = "Schwerti";

	m_transforms.push_back(rootFrame);
	recTravTree(root, &m_transforms.front());


}

//This method should return the plugin icon (it will be used mainly
//if your plugin as several actions in which case CC will create a
//dedicated sub-menu entry with this icon.
QIcon qTransformsPlugin::getIcon() const
{
	//open qDummyPlugin.qrc (text file), update the "prefix" and the
	//icon(s) filename(s). Then save it with the right name (yourPlugin.qrc).
	//(eventually, remove the original qDummyPlugin.qrc file!)
	return QIcon(":/CC/plugin/qTransforms/icon.png");
}