diff --git a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp
index 19968523..18f96b41 100644
--- a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp
+++ b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp
@@ -16,6 +16,9 @@
 #include <metaroom_xml_parser/load_utilities.h>
 #include <strands_sweep_registration/RobotContainer.h>
 
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+
 typedef pcl::PointXYZRGB PointType;
 typedef pcl::PointCloud<pcl::PointXYZRGB> Cloud;
 typedef typename Cloud::Ptr CloudPtr;
@@ -23,11 +26,314 @@ using namespace std;
 
 typedef actionlib::SimpleActionServer<calibrate_sweeps::CalibrateSweepsAction> Server;
 
+std::string runCalibration(int min_num_sweeps = 1, int max_num_sweeps = 100000, std::string sweep_location = "", std::string save_location = "",
+						   bool newParams = true, float fx = 534.191590, float fy = 534.016892, float cx = 315.622746, float cy = 238.568515){
+	if (sweep_location=="")
+	{
+		passwd* pw = getpwuid(getuid());
+		std::string path(pw->pw_dir);
+
+		path+="/.semanticMap/";
+		sweep_location = path;
+	} else {
+		sweep_location+="/";
+	}
+	if ( ! boost::filesystem::exists( sweep_location ) )
+	{
+		ROS_ERROR_STREAM("Could not find folder where to load sweeps from "+sweep_location);
+		//as->setAborted(res,"Could not find folder where to load sweeps from "+sweep_location);
+		return "";
+	}
+
+	//SweepParameters complete_sweep_parameters (-160, 20, 160, -30, 30, 30);
+	SweepParameters complete_sweep_parameters	(-160, 20, 160, -30, 30,  30);
+
+	SweepParameters medium_sweep_parameters		(-160, 20, 160, -30, -30, -30);
+	SweepParameters short_sweep_parameters		(-160, 40, 160, -30, -30, -30);
+	SweepParameters shortest_sweep_parameters	(-160, 60, 140, -30, -30, -30);
+
+	SweepParameters medium_sweep_parameters2	(-160, 20, 160, -30, 30, -30);
+	SweepParameters short_sweep_parameters2		(-160, 40, 160, -30, 30, -30);
+	SweepParameters shortest_sweep_parameters2	(-160, 60, 140, -30, 30, -30);
+
+	ROS_INFO_STREAM("complete_sweep_parameters paramters " << complete_sweep_parameters);
+	ROS_INFO_STREAM("medium_sweep_parameters   paramters " << medium_sweep_parameters);
+	ROS_INFO_STREAM("short_sweep_parameters    paramters " << short_sweep_parameters);
+	ROS_INFO_STREAM("shortest_sweep_parameters paramters " << shortest_sweep_parameters);
+
+
+	ROS_INFO_STREAM("Calibrating using sweeps with paramters "<<complete_sweep_parameters);
+
+	ROS_INFO_STREAM("Sweeps will be read from "<<sweep_location);
+
+	std::string save_folder;
+	// default location
+	passwd* pw = getpwuid(getuid());
+	std::string path(pw->pw_dir);
+
+	path+="/.ros/semanticMap/";
+	save_folder = path;
+	if ( ! boost::filesystem::exists( save_folder ) )
+	{
+		if (!boost::filesystem::create_directory(save_folder))
+		{
+			ROS_ERROR_STREAM("Could not create folder where to save calibration data "+save_folder);
+			 //as->setAborted(res,"Could not create folder where to save calibration data "+save_folder);
+			 return "";
+		}
+	}
+	ROS_INFO_STREAM("Calibration data will be saved at: "<<save_folder);
+
+	// Load sweeps
+	vector<string> matchingObservations = semantic_map_load_utilties::getSweepXmls<PointType>(sweep_location);
+
+	if (matchingObservations.size() < min_num_sweeps)
+	{
+		ROS_ERROR_STREAM("Not enough sweeps to perform calibration "<<matchingObservations.size());
+		 //as->setAborted(res,"Not enough sweeps to perform calibration "+matchingObservations.size());
+		 return "";
+	}
+
+	std::string saveLocation = save_location;
+	if (saveLocation == "")
+	{
+		saveLocation = sweep_location; // save in the same folder
+	} else {
+		saveLocation+="/";
+		if ( ! boost::filesystem::exists( saveLocation ) )
+		{
+			if (!boost::filesystem::create_directory(saveLocation))
+			{
+				ROS_ERROR_STREAM("Could not create folder where to save calibration data "+saveLocation);
+				 //as->setAborted(res,"Could not create folder where to save calibration data "+saveLocation);
+				 return "";
+			}
+		}
+	}
+	ROS_INFO_STREAM("The registered sweeps will be saved at: "<<saveLocation);
+
+	sort(matchingObservations.begin(), matchingObservations.end());
+	reverse(matchingObservations.begin(), matchingObservations.end());
+
+	// Initialize calibration class
+	unsigned int gx = 17;
+	unsigned int todox = 17;
+	unsigned int gy = 3;
+	unsigned int todoy = 3;
+	RobotContainer * rc = new RobotContainer(gx,todox,gy,todoy);
+
+
+//    	534.191590 0.000000 315.622746
+//		0.000000 534.016892 238.568515
+//0.000000 0.000000 1.000000
+//printf("%f %f %f %f\n",534.191590, 534.016892,315.622746, 238.568515);
+//	rc->initializeCamera(534.191590, 534.016892,315.622746, 238.568515, 640, 480);
+
+
+
+	if(newParams){
+		//printf("%f %f %f %f\n",fx,fy,cx,cy);
+		rc->initializeCamera(fx,fy,cx,cy, 640, 480);
+	}else{
+		// initialize camera parameters from the sweep
+		if (matchingObservations.size()){
+			SemanticRoom<PointType> aRoom = SemanticRoomXMLParser<PointType>::loadRoomFromXML(matchingObservations.front(),true);
+			if (aRoom.getIntermediateCloudCameraParameters().size()){
+				image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0];
+				rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height);
+			} else {
+				// no camera parameters saved with the sweep -> initialize optimizer with default parameters
+				rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480);
+			}
+		}else{
+			exit(0);
+		}
+	}
+
+	CloudPtr dummy_cloud (new Cloud());
+	dummy_cloud->width = 640;
+	dummy_cloud->height = 480;
+	dummy_cloud->points.resize(dummy_cloud->width*dummy_cloud->height);
+
+//exit(0);
+
+//	SweepParameters complete_sweep_parameters	(-160, 20, 160, -30, 30,  30);
+//	SweepParameters medium_sweep_parameters		(-160, 20, 160, -30, 30, -30);
+//	SweepParameters short_sweep_parameters		(-160, 40, 160, -30, 30, -30);
+//	SweepParameters shortest_sweep_parameters	(-160, 60, 140, -30, 30, -30);
+
+	for (size_t i=0; i<max_num_sweeps && i<matchingObservations.size(); i++)
+	{
+		printf("sweep: %i\n",i);
+		// check if sweep parameters correspond
+
+		unsigned found = matchingObservations[i].find_last_of("/");
+		std::string base_path = matchingObservations[i].substr(0,found+1);
+		RegistrationFeatures reg(false);
+		//reg.saveOrbFeatures<PointType>(aRoom,base_path);
+
+		std::vector<semantic_map_registration_features::RegistrationFeatures> features = semantic_map_registration_features::loadRegistrationFeaturesFromSingleSweep(matchingObservations[i], false);
+		if (features.size() == 0){
+			SemanticRoom<PointType> aRoom = SemanticRoomXMLParser<PointType>::loadRoomFromXML(matchingObservations[i],true);
+			reg.saveOrbFeatures<PointType>(aRoom,base_path);
+		}
+		rc->addToTrainingORBFeatures(matchingObservations[i]);
+
+/*
+		if		 (aRoom.m_SweepParameters == complete_sweep_parameters){
+			printf("complete_sweep_parameters\n");
+
+			if (features.size() == 0){reg.saveOrbFeatures<PointType>(aRoom,base_path);}
+			rc->addToTrainingORBFeatures(matchingObservations[i]);
+
+		}else if (aRoom.m_SweepParameters == medium_sweep_parameters   || aRoom.m_SweepParameters == medium_sweep_parameters2 ){
+			printf("medium_sweep_parameters\n");
+
+			for(unsigned int j = 0; j < before.size(); j++){
+				after.push_back(before[j]);
+			}
+
+			while(after.size() < 51){after.push_back(dummy_cloud);}
+			aRoom.setIntermediateClouds(after);
+
+			if (features.size() == 0){reg.saveOrbFeatures<PointType>(aRoom,base_path);}
+			rc->addToTrainingORBFeatures(matchingObservations[i]);
+
+		}else if (aRoom.m_SweepParameters == short_sweep_parameters    || aRoom.m_SweepParameters == short_sweep_parameters2 ){
+			printf("short_sweep_parameters\n");
+		}else if (aRoom.m_SweepParameters == shortest_sweep_parameters || aRoom.m_SweepParameters == shortest_sweep_parameters2 ){
+			printf("shortest_sweep_parameters\n");
+		}else{
+			printf("sweep has no known type\n");
+			std::cout <<"sweep parameters not correct: " << aRoom.m_SweepParameters << std::endl << std::endl << std::endl << std::endl;
+		}
+*/
+
+//		if (aRoom.m_SweepParameters != complete_sweep_parameters){
+//			ROS_INFO_STREAM("Skipping "<<matchingObservations[i]<<" sweep parameters not correct: "<<aRoom.m_SweepParameters<<" Required parameters "<<complete_sweep_parameters);
+//			continue; // not a match
+//		}
+
+//		// check if the orb features have already been computed
+
+		//
+	}
+	// perform calibration
+	std::vector<Eigen::Matrix4f> cameraPoses = rc->train();
+
+	std::vector<tf::StampedTransform> registeredPoses;
+
+	for (auto eigenPose : cameraPoses)
+	{
+		tf::StampedTransform tfStamped;
+		tfStamped.frame_id_ = "temp";
+		tfStamped.child_frame_id_ = "temp";
+		tf::Transform tfTr;
+		const Eigen::Affine3d eigenTr(eigenPose.cast<double>());
+		tf::transformEigenToTF(eigenTr, tfTr);
+		tfStamped.setOrigin(tfTr.getOrigin());
+		tfStamped.setBasis(tfTr.getBasis());
+		registeredPoses.push_back(tfStamped);
+	}
+	std::string registeredPosesFile = semantic_map_registration_transforms::saveRegistrationTransforms(registeredPoses);
+	registeredPoses.clear();
+	registeredPoses = semantic_map_registration_transforms::loadRegistrationTransforms(registeredPosesFile);
+	ROS_INFO_STREAM("Calibration poses saved at: "<<registeredPosesFile);
+
+	std::vector<tf::StampedTransform> complete_registeredPoses = registeredPoses;
+
+	std::vector<tf::StampedTransform> medium_registeredPoses;
+	for(unsigned int i = 0; i < 17; i+=1){medium_registeredPoses.push_back(registeredPoses[i]);}
+
+	std::vector<tf::StampedTransform> short_registeredPoses;
+	for(unsigned int i = 0; i < 17; i+=2){short_registeredPoses.push_back(registeredPoses[i]);}
+
+	std::vector<tf::StampedTransform> shortest_registeredPoses;
+	for(unsigned int i = 0; i < 17; i+=3){shortest_registeredPoses.push_back(registeredPoses[i]);}
+
+	std::string sweepParametersFile = semantic_map_registration_transforms::saveSweepParameters(complete_sweep_parameters);
+	ROS_INFO_STREAM("Calibration sweep parameters saved at: "<<sweepParametersFile);
+
+	double*** rawPoses = rc->poses;
+	unsigned int x,y;
+	std::string rawPosesFile = semantic_map_registration_transforms::saveRegistrationTransforms(rawPoses, rc->todox,rc->todoy);
+	ROS_INFO_STREAM("Raw calibration data saved at: "<<rawPosesFile);
+
+
+	// correct used sweeps with the new transforms and camera parameters
+	// create corrected cam params
+	sensor_msgs::CameraInfo camInfo;
+	camInfo.P = {rc->camera->fx, 0.0, rc->camera->cx, 0.0, 0.0, rc->camera->fy, rc->camera->cy, 0.0,0.0, 0.0, 1.0,0.0};
+	camInfo.D = {0,0,0,0,0};
+	image_geometry::PinholeCameraModel aCameraModel;
+	aCameraModel.fromCameraInfo(camInfo);
+
+	std::string camParamsFile = semantic_map_registration_transforms::saveCameraParameters(aCameraModel);
+	ROS_INFO_STREAM("Camera parameters saved at: "<<camParamsFile);
+
+
+
+	// update sweeps with new poses and new camera parameters
+
+	SemanticRoomXMLParser<PointType> reg_parser(saveLocation);
+
+	for (auto usedObs : matchingObservations){
+		SemanticRoom<PointType> aRoom = SemanticRoomXMLParser<PointType>::loadRoomFromXML(usedObs,true);
+		auto origTransforms = aRoom.getIntermediateCloudTransforms();
+		aRoom.clearIntermediateCloudRegisteredTransforms();
+		aRoom.clearIntermediateCloudCameraParametersCorrected();
+
+		std::vector<tf::StampedTransform> corresponding_registeredPoses;
+		corresponding_registeredPoses = semantic_map_registration_transforms::loadCorrespondingRegistrationTransforms(aRoom.m_SweepParameters);
+		ROS_INFO_STREAM("Corresponing registered poses "<<corresponding_registeredPoses.size()<<" original transforms "<<origTransforms.size());
+		if (corresponding_registeredPoses.size() != origTransforms.size()){
+			ROS_ERROR_STREAM("Cannot use registered poses to correct sweep: "<<usedObs);
+			continue;
+		}
+
+		for (size_t i=0; i<origTransforms.size(); i++)
+		{
+			tf::StampedTransform transform = origTransforms[i];
+//            transform.setOrigin(registeredPoses[i].getOrigin());
+//            transform.setBasis(registeredPoses[i].getBasis());
+			transform.setOrigin(corresponding_registeredPoses[i].getOrigin());
+			transform.setBasis(corresponding_registeredPoses[i].getBasis());
+			aRoom.addIntermediateCloudCameraParametersCorrected(aCameraModel);
+			aRoom.addIntermediateRoomCloudRegisteredTransform(transform);
+		}
+		semantic_map_room_utilities::reprojectIntermediateCloudsUsingCorrectedParams<PointType>(aRoom);
+		semantic_map_room_utilities::rebuildRegisteredCloud<PointType>(aRoom);
+		// transform to global frame of reference
+		tf::StampedTransform origin = origTransforms[0];
+//		CloudPtr completeCloud = aRoom.getCompleteRoomCloud();
+//		pcl_ros::transformPointCloud(*completeCloud, *completeCloud,origin);
+//		aRoom.setCompleteRoomCloud(completeCloud);
+		string room_path = reg_parser.saveRoomAsXML(aRoom,"room.xml",true);
+		ROS_INFO_STREAM("..done");
+		// recompute ORB features
+//		unsigned found = room_path.find_last_of("/");
+//		std::string base_path = room_path.substr(0,found+1);
+//		RegistrationFeatures reg(false);
+//		reg.saveOrbFeatures<PointType>(aRoom,base_path);
+	}
+	delete rc;
+	return saveLocation;
+}
+
 void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server* as)
 {
     ROS_INFO_STREAM("Received calibrate message. Min/max sweeps: "<<goal->min_num_sweeps<<" "<<goal->max_num_sweeps);
     calibrate_sweeps::CalibrateSweepsResult res;
-
+	res.calibration_file = runCalibration(goal->min_num_sweeps, goal->max_num_sweeps, goal->sweep_location, goal->save_location);//registeredPosesFile;
+
+	if (res.calibration_file.compare("") != 0){
+		as->setAborted(res,"");
+	}else{
+		as->setSucceeded(res,"Done");
+	}
+/*
+	ROS_INFO_STREAM("Received calibrate message. Min/max sweeps: "<<goal->min_num_sweeps<<" "<<goal->max_num_sweeps);
+	calibrate_sweeps::CalibrateSweepsResult res;
     SweepParameters complete_sweep_parameters (-160, 20, 160, -30, 30, 30);
     ROS_INFO_STREAM("Calibrating using sweeps with paramters "<<complete_sweep_parameters);
 
@@ -91,7 +397,7 @@ void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server*
             if (!boost::filesystem::create_directory(saveLocation))
             {
                 ROS_ERROR_STREAM("Could not create folder where to save calibration data "+saveLocation);
-                 as->setAborted(res,"Could not create folder where to save calibration data "+saveLocation);
+				 as->setAborted(res,"Could not create folder where to /media/johane/SSDstorage/final_tsc_semantic_maps/semantic_maps//20160808/patrol_run_110/room_0/save calibration data "+saveLocation);
                  return;
             }
         }
@@ -107,18 +413,26 @@ void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server*
     unsigned int gy = 3;
     unsigned int todoy = 3;
     RobotContainer * rc = new RobotContainer(gx,todox,gy,todoy);
-
+    
+    
+//    	534.191590 0.000000 315.622746
+//		0.000000 534.016892 238.568515
+//0.000000 0.000000 1.000000
+printf("%f %f %f %f\n",534.191590, 534.016892,315.622746, 238.568515);
+	rc->initializeCamera(534.191590, 534.016892,315.622746, 238.568515, 640, 480);
+//exit(0);
     // initialize camera parameters from the sweep
-    if (matchingObservations.size()){
-        SemanticRoom<PointType> aRoom = SemanticRoomXMLParser<PointType>::loadRoomFromXML(matchingObservations[0],true);
-        if (aRoom.getIntermediateCloudCameraParameters().size()){
-            image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0];
-            rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height);
-        } else {
-            // no camera parameters saved with the sweep -> initialize optimizer with default parameters
-            rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480);
-        }
-    }
+
+//    if (matchingObservations.size()){
+//        SemanticRoom<PointType> aRoom = SemanticRoomXMLParser<PointType>::loadRoomFromXML(matchingObservations[0],true);
+//        if (aRoom.getIntermediateCloudCameraParameters().size()){
+//            image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0];
+//            rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height);
+//        } else {
+//            // no camera parameters saved with the sweep -> initialize optimizer with default parameters
+//            rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480);
+//        }
+//    }
 
 
     for (size_t i=0; i<goal->max_num_sweeps && i<matchingObservations.size(); i++)
@@ -172,7 +486,7 @@ void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server*
     unsigned int x,y;
     std::string rawPosesFile = semantic_map_registration_transforms::saveRegistrationTransforms(rawPoses, rc->todox,rc->todoy);
     ROS_INFO_STREAM("Raw calibration data saved at: "<<rawPosesFile);
-    res.calibration_file = registeredPosesFile;
+
 
     // correct used sweeps with the new transforms and camera parameters
     // create corrected cam params
@@ -232,8 +546,14 @@ void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server*
 
     delete rc;
 
+	res.calibration_file = registeredPosesFile;
 
     as->setSucceeded(res,"Done");
+	*/
+}
+
+void runCallback(const std_msgs::String::ConstPtr& msg){
+	runCalibration();
 }
 
 int main(int argc, char** argv)
@@ -243,6 +563,70 @@ int main(int argc, char** argv)
   Server server(n, "calibrate_sweeps", boost::bind(&execute, _1, &server), false);
   ROS_INFO_STREAM("Calibrate sweep action server initialized");
   server.start();
+  ros::Subscriber sub = n.subscribe("calibrate_sweeps_action_server/run", 1000, runCallback);
+
+  bool runCalib = false;
+  std::vector<std::string> recalibPaths;
+  //std::string recalibPath = std::string(getenv ("HOME"))+"/.semanticMap/";
+  bool loadParams = false;
+  float fx = 535;
+  float fy = 535;
+  float cx = 0.5*(640.0-1);
+  float cy = 0.5*(480.0-1);
+
+  int max_sweeps = 100;
+
+  int inputstate = -1;
+  for(unsigned int i = 1; i < argc; i++){
+	  if(std::string(argv[i]).compare("-run") == 0){
+		  runCalib = true;
+		  inputstate = 0;
+	  }else if(std::string(argv[i]).compare("-fx") == 0){
+		  inputstate = 1;
+	  }else if(std::string(argv[i]).compare("-fy") == 0){
+		  inputstate = 2;
+	  }else if(std::string(argv[i]).compare("-cx") == 0){
+		  inputstate = 3;
+	  }else if(std::string(argv[i]).compare("-cy") == 0){
+		  inputstate = 4;
+	  }else if(std::string(argv[i]).compare("-max_sweeps") == 0){
+		  inputstate = 5;
+	  }else if(std::string(argv[i]).compare("-loadParams") == 0){
+		  loadParams = true;
+	  }else if(inputstate == 0){
+		  recalibPaths.push_back(std::string(argv[i]));
+	  }else if(inputstate == 1){
+		  loadParams = true;
+		  fx = atof(argv[i]);
+	  }else if(inputstate == 2){
+		  loadParams = true;
+		  fy = atof(argv[i]);
+	  }else if(inputstate == 3){
+		  loadParams = true;
+		  cx = atof(argv[i]);
+	  }else if(inputstate == 4){
+		  loadParams = true;
+		  cy = atof(argv[i]);
+	  }else if(inputstate == 5){
+		  max_sweeps = atoi(argv[i]);
+	  }
+  }
+
+  if(runCalib){
+	if(recalibPaths.size() == 0){
+		recalibPaths.push_back(std::string(getenv ("HOME"))+"/.semanticMap/");
+	}
+	for(unsigned int i = 0; i < recalibPaths.size(); i++){
+		if(loadParams){
+			runCalibration(1,max_sweeps,recalibPaths[i], "",loadParams,fx,fy,cx,cy);
+		}else{
+			runCalibration(1,max_sweeps,recalibPaths[i], "");
+		}
+		//bool newParams = true, float fx = 534.191590, float fy = 534.016892, float cx = 315.622746, float cy = 238.568515
+	}
+	exit(0);
+  }
+
   ros::spin();
   return 0;
 }
diff --git a/cloud_merge/include/cloud_merge/cloud_merge_node.h b/cloud_merge/include/cloud_merge/cloud_merge_node.h
index 6ee77aef..d91aa5e3 100644
--- a/cloud_merge/include/cloud_merge/cloud_merge_node.h
+++ b/cloud_merge/include/cloud_merge/cloud_merge_node.h
@@ -434,7 +434,7 @@ void CloudMergeNode<PointType>::controlCallback(const std_msgs::String& controlS
     }
 
     if (controlString.data == "end_sweep")
-    {
+	{
         ROS_INFO_STREAM("Pan tilt sweep stopped");
         m_bAquisitionPhase = false;
 
@@ -483,8 +483,8 @@ void CloudMergeNode<PointType>::controlCallback(const std_msgs::String& controlS
             std::string roomXMLPath = parser.saveRoomAsXML(aSemanticRoom);
             ROS_INFO_STREAM("Saved semantic room");
 
-            if (m_bRegisterAndCorrectSweep)
-            {
+			if (m_bRegisterAndCorrectSweep)
+			{
                 // load precalibrated camera poses
 //                std::vector<tf::StampedTransform> regTransforms = semantic_map_registration_transforms::loadRegistrationTransforms("default", true);
                std::vector<tf::StampedTransform> corresponding_registeredPoses;
@@ -541,7 +541,7 @@ void CloudMergeNode<PointType>::controlCallback(const std_msgs::String& controlS
                     reg.saveOrbFeatures<PointType>(aSemanticRoom,base_path);
 
                 }
-            }
+			}
 
             // Pulbish room observation
             semantic_map::RoomObservation obs_msg;
diff --git a/dynamic_object_retrieval/benchmark/include/object_3d_benchmark/benchmark_visualization.h b/dynamic_object_retrieval/benchmark/include/object_3d_benchmark/benchmark_visualization.h
index c16e9136..dce92ab6 100644
--- a/dynamic_object_retrieval/benchmark/include/object_3d_benchmark/benchmark_visualization.h
+++ b/dynamic_object_retrieval/benchmark/include/object_3d_benchmark/benchmark_visualization.h
@@ -16,8 +16,8 @@ using CloudT = pcl::PointCloud<PointT>;
 namespace benchmark_retrieval {
 
 void put_text(cv::Mat& image, const std::string& text);
-cv::Mat make_image(std::vector<CloudT::Ptr>& results,  const Eigen::Matrix4f& room_transform,
-                   std::vector<boost::filesystem::path>& sweep_paths, const std::vector<std::string>& optional_text);
+std::pair<cv::Mat, std::vector<cv::Mat> > make_image(std::vector<CloudT::Ptr>& results,  const Eigen::Matrix4f& room_transform,
+                                                     std::vector<boost::filesystem::path>& sweep_paths, const std::vector<std::string>& optional_text);
 cv::Mat add_query_image(cv::Mat& results, cv::Mat& query_image, const std::string& query_label);
 
 // this will alter the look of the point clouds, but for the better I think
@@ -25,6 +25,9 @@ cv::Mat add_query_image(cv::Mat& results, cv::Mat& query_image, const std::strin
 cv::Mat make_visualization_image(cv::Mat& query_image, const std::string& query_label, std::vector<CloudT::Ptr>& clouds,
                                  std::vector<boost::filesystem::path>& sweep_paths, const std::vector<std::string>& optional_text,
                                  const Eigen::Matrix4f& T);
+cv::Mat make_visualization_image(cv::Mat& query_image, const std::string& query_label, std::vector<CloudT::Ptr>& clouds,
+                                 std::vector<boost::filesystem::path>& sweep_paths, const std::vector<std::string>& optional_text,
+                                 const Eigen::Matrix4f& T, std::vector<cv::Mat>& individual_images);
 cv::Mat make_visualization_image(CloudT::Ptr& query_cloud, cv::Mat& query_mask, const boost::filesystem::path& query_path,
                                  const Eigen::Matrix3f& K, const Eigen::Matrix4f& query_transform,
                                  const std::string& query_label, std::vector<CloudT::Ptr>& clouds,
diff --git a/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp b/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp
index bf8ad6e9..04f30240 100644
--- a/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp
+++ b/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp
@@ -28,11 +28,23 @@ cv::Mat make_visualization_image(cv::Mat& query_image, const string& query_label
             p.getVector4fMap() = T*p.getVector4fMap();
         }
     }*/
-    cv::Mat result_image = make_image(clouds, T, sweep_paths, optional_text);
+    cv::Mat result_image;
+    vector<cv::Mat> individual_images;
+    tie(result_image, individual_images) = make_image(clouds, T, sweep_paths, optional_text);
     return add_query_image(result_image, query_image, query_label);
     //return result_image;
 }
 
+cv::Mat make_visualization_image(cv::Mat& query_image, const string& query_label, vector<CloudT::Ptr>& clouds,
+                                 vector<boost::filesystem::path>& sweep_paths, const vector<string>& optional_text,
+                                 const Eigen::Matrix4f& T, vector<cv::Mat>& individual_images)
+{
+    cv::Mat result_image;
+    tie(result_image, individual_images) = make_image(clouds, T, sweep_paths, optional_text);
+    individual_images.insert(individual_images.begin(), query_image.clone());
+    return add_query_image(result_image, query_image, query_label);
+}
+
 cv::Mat make_visualization_image(CloudT::Ptr& query_cloud, cv::Mat& query_mask, const boost::filesystem::path& query_path,
                                  const Eigen::Matrix3f& K, const Eigen::Matrix4f& query_transform,
                                  const string& query_label, vector<CloudT::Ptr>& clouds,
@@ -70,7 +82,9 @@ cv::Mat make_visualization_image(CloudT::Ptr& query_cloud, cv::Mat& query_mask,
     cv::Mat inverted_mask;
     cv::bitwise_not(query_mask, inverted_mask);
     image.setTo(cv::Scalar(255, 255, 255), inverted_mask);
-    cv::Mat result_image = make_image(clouds, T, sweep_paths, optional_text);
+    cv::Mat result_image;
+    vector<cv::Mat> individual_images;
+    tie(result_image, individual_images) = make_image(clouds, T, sweep_paths, optional_text);
     return add_query_image(result_image, image, query_label);
     //return result_image;
 }
@@ -244,15 +258,16 @@ cv::Mat render_image(CloudT::Ptr& cloud, const Eigen::Matrix4f& T, const Eigen::
 }
 
 // the question is if we actually need the paths for this?
-cv::Mat make_image(std::vector<CloudT::Ptr>& results, const Eigen::Matrix4f& room_transform,
-                   vector<boost::filesystem::path>& sweep_paths, const std::vector<std::string>& optional_text)
+pair<cv::Mat, vector<cv::Mat> > make_image(std::vector<CloudT::Ptr>& results, const Eigen::Matrix4f& room_transform,
+                                           vector<boost::filesystem::path>& sweep_paths, const std::vector<std::string>& optional_text)
 {
-    pair<int, int> sizes = make_pair(1, 10);//get_similar_sizes(results.size());
+    pair<int, int> sizes = make_pair(1, std::min(10, int(results.size())));//get_similar_sizes(results.size());
 
     int width = 200;
     int height = 200;
 
     cv::Mat visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_8UC3);
+    vector<cv::Mat> individual_images;
 
     int counter = 0;
     for (CloudT::Ptr& cloud : results) {
@@ -327,6 +342,7 @@ cv::Mat make_image(std::vector<CloudT::Ptr>& results, const Eigen::Matrix4f& roo
         int offset_height = counter / sizes.second;
         int offset_width = counter % sizes.second;
         sub_image.copyTo(visualization(cv::Rect(offset_width*width, offset_height*height, width, height)));
+        individual_images.push_back(sub_image);
 
         //cv::imshow("test", visualization);
         //cv::waitKey();
@@ -338,7 +354,7 @@ cv::Mat make_image(std::vector<CloudT::Ptr>& results, const Eigen::Matrix4f& roo
         visualization = cv::Mat::zeros(height*2, width*5, CV_8UC3);
     }
 
-    return visualization;
+    return make_pair(visualization, individual_images);
 }
 
 } // namespace benchmark_retrieval
diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/CMakeLists.txt b/dynamic_object_retrieval/dynamic_object_retrieval/CMakeLists.txt
index 7516f794..53d84523 100644
--- a/dynamic_object_retrieval/dynamic_object_retrieval/CMakeLists.txt
+++ b/dynamic_object_retrieval/dynamic_object_retrieval/CMakeLists.txt
@@ -16,6 +16,9 @@ add_definitions(-std=c++11 -O3)
 # Show where to find the find package scripts
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
 
+set(stopwatch_dir ${CMAKE_CURRENT_SOURCE_DIR}/stopwatch)
+set(stopwatch_include_dir ${stopwatch_dir}/src)
+
 # Define the locations of the k_means_tree project
 # If using catkin, including it using catkin instead
 if (catkin_FOUND)
@@ -23,6 +26,7 @@ if (catkin_FOUND)
                         metaroom_xml_parser object_manager k_means_tree convex_segmentation)
     set(ROS_LIBRARIES ${catkin_LIBRARIES})
     include_directories(${catkin_INCLUDE_DIRS})
+    #set(stopwatch_library_dir ${CATKIN_DEVEL_PREFIX}/stopwatch/build)
 else()
     set(k_means_tree_dir ${CMAKE_CURRENT_SOURCE_DIR}/../k_means_tree)
     set(k_means_tree_library_dir ${k_means_tree_dir}/build)
@@ -57,24 +61,23 @@ else()
 
     # Define the locations of cereal serialization header files
     set(cereal_include_dir ${CMAKE_CURRENT_SOURCE_DIR}/../k_means_tree/cereal/include)
+
+    # Only compile the stopwatch project if it's not with catkin
     include_directories(${cereal_include_dir})
+    set(stopwatch_library_dir ${stopwatch_dir}/build)
+    # Compile the stopwatch project, needed for this
+    ExternalProject_Add(stopwatch_project
+      DOWNLOAD_COMMAND ""
+      SOURCE_DIR ${stopwatch_include_dir}
+      BINARY_DIR ${stopwatch_library_dir}
+      BUILD_COMMAND ${CMAKE_MAKE_PROGRAM}
+      INSTALL_COMMAND ""
+    )
 endif()
 
 # Eh, we need to somehow build this maybe?
-set(stopwatch_dir ${CMAKE_CURRENT_SOURCE_DIR}/stopwatch)
-set(stopwatch_include_dir ${stopwatch_dir}/src)
-set(stopwatch_library_dir ${stopwatch_dir}/build)
 include_directories(${stopwatch_include_dir})
 
-# Compile the stopwatch project, needed for this
-ExternalProject_Add(stopwatch_project
-  DOWNLOAD_COMMAND ""
-  SOURCE_DIR ${stopwatch_include_dir}
-  BINARY_DIR ${stopwatch_library_dir}
-  BUILD_COMMAND ${CMAKE_MAKE_PROGRAM}
-  INSTALL_COMMAND ""
-)
-
 # Find PCL
 find_package(PCL 1.6 REQUIRED COMPONENTS common io search visualization surface kdtree features surface segmentation octree filter keypoints)
 include_directories(${PCL_INCLUDE_DIRS})
diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h
index 6ddfe10d..2ee734dd 100644
--- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h
+++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h
@@ -17,8 +17,15 @@
 #include <boost/filesystem.hpp>
 #include <pcl/io/pcd_io.h>
 #include <dynamic_object_retrieval/definitions.h>
+#include <mongodb_store/message_store.h>
 
 #define WITH_SURFEL_NORMALS 1
+#define WITH_UNIVERSAL_URIS 1
+
+#if WITH_UNIVERSAL_URIS
+#include <quasimodo_msgs/fused_world_state_object.h>
+#include <pcl_ros/point_cloud.h>
+#endif
 
 using PointT = pcl::PointXYZRGB;
 using CloudT = pcl::PointCloud<PointT>;
@@ -39,19 +46,167 @@ struct path_result<grouped_vocabulary_tree<HistT, 8> > {
     using type = std::vector<boost::filesystem::path>;
 };
 
+enum uri_kind {
+    metaroom,
+    mongodb,
+    all
+};
+
+
+
+// this should have some switch for what to keep
+// if kind = uri_kind::mongodb, every index will be returned but we will only keep the first (what number) which are valid
+// alternatively, maybe return empty paths for everything else? sounds good, or rather, let's still return the files
+// but somehow we need to know how many to load... let's hardcode for now
 template <typename IndexT>
-std::vector<boost::filesystem::path> get_retrieved_paths(const std::vector<IndexT>& scores, const vocabulary_summary& summary)
+std::vector<boost::filesystem::path> get_retrieved_paths(const std::vector<IndexT>& scores, const vocabulary_summary& summary,
+                                                         bool verbose = false, uri_kind kind = uri_kind::all)
 {
+    if (verbose) {
+        std::cout << "Entering get_retrieved_paths" << std::endl;
+    }
+
+    // OK, this is where we add the new type of segment_uris
+#if WITH_UNIVERSAL_URIS
+
+    ros::NodeHandle n("/dynamic_retrieval");
+    size_t offset = summary.nbr_noise_segments;
+    segment_uris uris;
+    std::cout << "Loading URI:s file: " << (boost::filesystem::path(summary.noise_data_path) / "vocabulary" / "segment_uris.json").string() << std::endl;
+    uris.load(boost::filesystem::path(summary.noise_data_path) / "vocabulary");
+    mongodb_store::MessageStoreProxy* message_store = NULL;
+
+    for (IndexT s : scores) {
+        if (s.index >= offset) {
+            std::cout << "1. Index: " << s.index << " is larger than: " << offset << std::endl;
+            std::cout << "URI loading does not support noise+annotated data structure!" << std::endl;
+            std::cout << "Got URI: " << uris.uris[s.index] << std::endl;
+            exit(-1);
+        }
+        std::string uri = uris.uris[s.index];
+        if (verbose) {
+            std::cout << "Got URI: " << uri << " at index: " << s.index << std::endl;
+        }
+        if (uri.compare(0, 10, "mongodb://") == 0) {
+            std::string db_uri = uri.substr(10, uri.size()-10);
+            std::vector<std::string> strs;
+            boost::split(strs, db_uri, boost::is_any_of("/"));
+            std::cout << "Initializing message story proxy with database: " << strs[0] << ", collection: " << strs[1] << std::endl;
+            message_store = new mongodb_store::MessageStoreProxy(n, strs[1], strs[0]);
+            break;
+        }
+    }
+
+    std::vector<boost::filesystem::path> retrieved_paths;
+    size_t loaded_mongodb_results = 0;
+    for (IndexT s : scores) {
+        // should we write to some temporary files and return the paths to those?
+        // e.g. in ~/.ros/quasimodo
+        // the question is also if we should write a dummy surfel_map.pcd containing the cloud and a separate object.pcd with only points
+        // another possibility would be to actually just write surfel_map.pcd, that should be enough?
+        // the big problem is that we won't able to get any of the info in room.xml
+        if (s.index >= offset) {
+            std::cout << "2. Index: " << s.index << " is larger than: " << offset << std::endl;
+            std::cout << "URI loading does not support noise+annotated data structure!" << std::endl;
+            std::cout << "Got URI: " << uris.uris[s.index] << std::endl;
+            exit(-1);
+        }
+        std::string uri = uris.uris[s.index];
+        if (uri.compare(0, 7, "file://") == 0) {
+            retrieved_paths.push_back(boost::filesystem::path(uri.substr(7, uri.size()-7)));
+        }
+        else if (uri.compare(0, 10, "mongodb://") == 0) {
+            // don't delete anything here, instead have a cache based on the database id:s
+            // use boost split to split separate the uri into database / collection / id
+
+            if (kind == uri_kind::mongodb && loaded_mongodb_results > 20) { // TODO: hard coded for now, let's revisit
+                retrieved_paths.push_back(boost::filesystem::path());
+                continue;
+            }
+
+            std::string db_uri = uri.substr(10, uri.size()-10);
+            std::vector<std::string> strs;
+            boost::split(strs, db_uri, boost::is_any_of("/"));
+
+            // how do we get the home directory correctly?
+            boost::filesystem::path temp_path = boost::filesystem::absolute(boost::filesystem::path("quasimodo/temp") / strs[2] / "surfel_map.pcd");
+
+            if (kind == uri_kind::all && boost::filesystem::exists(temp_path.parent_path())) {
+                retrieved_paths.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd");
+                ++loaded_mongodb_results;
+                continue;
+            }
+
+            if (!boost::filesystem::exists(temp_path.parent_path())) {
+                boost::filesystem::create_directories(temp_path.parent_path());
+                boost::filesystem::create_directory(temp_path.parent_path() / "convex_segments");
+            }
+
+            boost::shared_ptr<quasimodo_msgs::fused_world_state_object> message;
+            mongo::BSONObj bson_message;
+            std::tie(message, bson_message) = message_store->queryID<quasimodo_msgs::fused_world_state_object>(strs[2]);
+
+            if (message->transforms.empty()) {
+                retrieved_paths.push_back(boost::filesystem::path());
+                continue;
+            }
+
+            SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT);
+            pcl::fromROSMsg(message->surfel_cloud, *surfel_cloud);
+            pcl::io::savePCDFileBinary(temp_path.string(), *surfel_cloud);
+            pcl::io::savePCDFileBinary((temp_path.parent_path() / "convex_segments" / "segment.pcd").string(), *surfel_cloud);
+
+            if (kind == uri_kind::mongodb && !message->removed_at.empty()) {
+                retrieved_paths.push_back(boost::filesystem::path());
+            }
+            else {
+                retrieved_paths.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd");
+                ++loaded_mongodb_results;
+            }
+
+            // [0] - database, [1] - collection, [2] - mongodb object id
+
+        }
+        else {
+            std::cout << "Loading from segment URI problem: URI not well formed!" << std::endl;
+            exit(-1);
+        }
+    }
+
+    if (message_store != NULL) { // redundant but clearer
+        delete message_store;
+    }
+
+    if (verbose) {
+        std::cout << "Exiting get_retrieved_paths" << std::endl;
+    }
+
+    return retrieved_paths;
+#else
+
     // here we'll make use of the info saved in the data_summaries
     // maybe we should cache these as well as they might get big?
     data_summary noise_summary;
+    if (verbose) {
+        std::cout << "Loading noise data summary..." << std::endl;
+    }
     noise_summary.load(boost::filesystem::path(summary.noise_data_path));
     data_summary annotated_summary;
+    if (verbose) {
+        std::cout << "Loading annotated data summary..." << std::endl;
+    }
     annotated_summary.load(boost::filesystem::path(summary.annotated_data_path));
 
+    if (verbose) {
+        std::cout << "Number of results: " << scores.size() << std::endl;
+        std::cout << "Number of noise segments: " << noise_summary.index_convex_segment_paths.size() << std::endl;
+        std::cout << "Number of annotated segments: " << noise_summary.index_convex_segment_paths.size() << std::endl;
+    }
+
     std::vector<boost::filesystem::path> retrieved_paths;
     size_t offset = summary.nbr_noise_segments;
     for (IndexT s : scores) {
+        std::cout << "Segment vocabulary index: " << s.index << std::endl;
         // TODO: vt index is not correct for grouped_vocabulary
         if (s.index < offset) {
             retrieved_paths.push_back(boost::filesystem::path(noise_summary.index_convex_segment_paths[s.index]));
@@ -61,26 +216,203 @@ std::vector<boost::filesystem::path> get_retrieved_paths(const std::vector<Index
         }
     }
 
+    if (verbose) {
+        std::cout << "Exiting get_retrieved_paths" << std::endl;
+    }
+
     return retrieved_paths;
+#endif
 }
 
 //std::vector<boost::filesystem::path> get_retrieved_paths(const std::vector<index_score>& scores, const vocabulary_summary& summary);
 std::vector<std::pair<boost::filesystem::path, vocabulary_tree<HistT, 8>::result_type> >
-get_retrieved_path_scores(const std::vector<vocabulary_tree<HistT, 8>::result_type>& scores, const vocabulary_summary& summary)
+get_retrieved_path_scores(const std::vector<vocabulary_tree<HistT, 8>::result_type>& scores, const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all)
 {
-    std::vector<boost::filesystem::path> paths = get_retrieved_paths(scores, summary);
+    if (verbose) {
+        std::cout << "Entering get_retrieved_path_scores" << std::endl;
+    }
+
+    std::vector<boost::filesystem::path> paths = get_retrieved_paths(scores, summary, verbose, kind);
     std::vector<std::pair<boost::filesystem::path, vocabulary_tree<HistT, 8>::result_type> > path_scores;
     for (auto tup : dynamic_object_retrieval::zip(paths, scores)) {
         path_scores.push_back(std::make_pair(boost::get<0>(tup), boost::get<1>(tup)));
     }
+
+    if (verbose) {
+        std::cout << "Exiting get_retrieved_path_scores" << std::endl;
+    }
+
     return path_scores;
 }
 
 std::vector<std::pair<std::vector<boost::filesystem::path>, grouped_vocabulary_tree<HistT, 8>::result_type> >
-get_retrieved_path_scores(const std::vector<grouped_vocabulary_tree<HistT, 8>::result_type>& scores, const vocabulary_summary& summary)
+get_retrieved_path_scores(const std::vector<grouped_vocabulary_tree<HistT, 8>::result_type>& scores, const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all)
 {
     // OK, now we need to get the paths for all of the stuff within the scans instead, so basically
     // we need to get a subsegment_sweep__path_iterator and add all of the path corresponding to groups
+
+    if (verbose) {
+        std::cout << "Entering get_retrieved_path_scores" << std::endl;
+    }
+
+#if WITH_UNIVERSAL_URIS
+
+    using IndexT = grouped_vocabulary_tree<HistT, 8>::result_type;
+
+    std::vector<std::pair<std::vector<boost::filesystem::path>, grouped_vocabulary_tree<HistT, 8>::result_type> > path_scores;
+
+    ros::NodeHandle n("/dynamic_retrieval");
+    size_t offset = summary.nbr_noise_segments;
+    segment_uris uris;
+    std::cout << "Loading URI:s file: " << (boost::filesystem::path(summary.noise_data_path) / "vocabulary" / "segment_uris.json").string() << std::endl;
+    uris.load(boost::filesystem::path(summary.noise_data_path) / "vocabulary");
+    mongodb_store::MessageStoreProxy* message_store = NULL;
+
+    for (IndexT s : scores) {
+        int index = s.subgroup_global_indices[0];
+        if (index >= offset) {
+            std::cout << "1. Index: " << index << " is larger than: " << offset << std::endl;
+            std::cout << "URI loading does not support noise+annotated data structure!" << std::endl;
+            std::cout << "Got URI: " << uris.uris[index] << std::endl;
+            exit(-1);
+        }
+        std::string uri = uris.uris[index];
+        if (verbose) {
+            std::cout << "Got URI: " << uri << " at index: " << index << std::endl;
+        }
+        if (uri.compare(0, 10, "mongodb://") == 0) {
+            std::string db_uri = uri.substr(10, uri.size()-10);
+            std::vector<std::string> strs;
+            boost::split(strs, db_uri, boost::is_any_of("/"));
+            std::cout << "Initializing message story proxy with database: " << strs[0] << ", collection: " << strs[1] << std::endl;
+            message_store = new mongodb_store::MessageStoreProxy(n, strs[1], strs[0]);
+            break;
+        }
+    }
+
+    int temp = 0;
+    size_t loaded_mongodb_results = 0;
+    for (const grouped_vocabulary_tree<HistT, 8>::result_type& score : scores) {
+        int index = score.subgroup_global_indices[0];
+
+        std::cout << "Iteration nbr: " << temp << std::endl;
+        std::cout << "Scores size: " << scores.size() << std::endl;
+        std::cout << "Subsegments in group: " << score.subgroup_group_indices.size() << std::endl;
+        ++temp;
+        path_scores.push_back(std::make_pair(std::vector<boost::filesystem::path>(), score));
+
+        // find the uri of the first segment to determine if mongodb result or metaroom result
+        std::string uri = uris.uris[index];
+        if (uri.compare(0, 7, "file://") == 0) {
+            //retrieved_paths.push_back(boost::filesystem::path(uri.substr(7, uri.size()-7)));
+
+            int sweep_id = score.group_index;
+            std::cout << "Getting sweep xml for group: " << sweep_id << std::endl;
+            std::cout << "With subsegment: " << score.subgroup_index << std::endl;
+            boost::filesystem::path sweep_path = dynamic_object_retrieval::get_sweep_xml(sweep_id, summary);
+            std::cout << "Getting a subsegment iterator for sweep: " << sweep_path.string() << std::endl;
+            if (summary.subsegment_type == "subsegment" || summary.subsegment_type == "supervoxel") {
+                dynamic_object_retrieval::sweep_subsegment_keypoint_map subsegments(sweep_path); // this is a problem
+                std::cout << "Finished getting a subsegment iterator for sweep: " << sweep_path.string() << std::endl;
+                int counter = 0;
+                for (const boost::filesystem::path& path : subsegments) {
+                    if (std::find(score.subgroup_group_indices.begin(), score.subgroup_group_indices.end(), counter) != score.subgroup_group_indices.end()) {
+                        path_scores.back().first.push_back(path);
+                    }
+                    ++counter;
+                }
+            }
+            else if (summary.subsegment_type == "convex_segment") {
+                dynamic_object_retrieval::sweep_convex_segment_map subsegments(sweep_path); // this is a problem
+                std::cout << "Finished getting a subsegment iterator for sweep: " << sweep_path.string() << std::endl;
+                int counter = 0;
+                for (const boost::filesystem::path& path : subsegments) {
+                    if (std::find(score.subgroup_group_indices.begin(), score.subgroup_group_indices.end(), counter) != score.subgroup_group_indices.end()) {
+                        path_scores.back().first.push_back(path);
+                    }
+                    ++counter;
+                }
+            }
+            else {
+                std::cout << summary.subsegment_type << " not a valid subsegment type..." << std::endl;
+                exit(-1);
+            }
+
+            std::cout << "Finished getting sweep xml for group: " << sweep_id << std::endl;
+            std::cout << "Got " << path_scores.back().first.size() << " subsegments..." << std::endl;
+        }
+        else if (uri.compare(0, 10, "mongodb://") == 0) {
+            // don't delete anything here, instead have a cache based on the database id:s
+            // use boost split to split separate the uri into database / collection / id
+
+            if (kind == uri_kind::mongodb && loaded_mongodb_results > 20) { // TODO: hard coded for now, let's revisit
+                path_scores.back().first.push_back(boost::filesystem::path());
+                continue;
+            }
+
+            std::string db_uri = uri.substr(10, uri.size()-10);
+            std::vector<std::string> strs;
+            boost::split(strs, db_uri, boost::is_any_of("/"));
+
+            // how do we get the home directory correctly?
+            boost::filesystem::path temp_path = boost::filesystem::absolute(boost::filesystem::path("quasimodo/temp") / strs[2] / "surfel_map.pcd");
+
+            if (kind == uri_kind::all && boost::filesystem::exists(temp_path.parent_path())) {
+                path_scores.back().first.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd");
+                ++loaded_mongodb_results;
+                continue;
+            }
+
+            if (!boost::filesystem::exists(temp_path.parent_path())) {
+                boost::filesystem::create_directories(temp_path.parent_path());
+                boost::filesystem::create_directory(temp_path.parent_path() / "convex_segments");
+            }
+
+            boost::shared_ptr<quasimodo_msgs::fused_world_state_object> message;
+            mongo::BSONObj bson_message;
+            std::tie(message, bson_message) = message_store->queryID<quasimodo_msgs::fused_world_state_object>(strs[2]);
+
+            if (message->transforms.empty()) {
+                path_scores.back().first.push_back(boost::filesystem::path());
+                continue;
+            }
+
+            SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT);
+            pcl::fromROSMsg(message->surfel_cloud, *surfel_cloud);
+            pcl::io::savePCDFileBinary(temp_path.string(), *surfel_cloud);
+            pcl::io::savePCDFileBinary((temp_path.parent_path() / "convex_segments" / "segment.pcd").string(), *surfel_cloud);
+
+            if (kind == uri_kind::mongodb && !message->removed_at.empty()) {
+                path_scores.back().first.push_back(boost::filesystem::path());
+            }
+            else {
+                path_scores.back().first.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd");
+                ++loaded_mongodb_results;
+            }
+
+            // [0] - database, [1] - collection, [2] - mongodb object id
+
+        }
+        else {
+            std::cout << "Loading from segment URI problem: URI not well formed!" << std::endl;
+            exit(-1);
+        }
+
+
+    }
+
+    if (message_store != NULL) { // redundant but clearer
+        delete message_store;
+    }
+
+    if (verbose) {
+        std::cout << "Exiting get_retrieved_path_scores" << std::endl;
+    }
+
+    return path_scores;
+
+#else
+
     std::vector<std::pair<std::vector<boost::filesystem::path>, grouped_vocabulary_tree<HistT, 8>::result_type> > path_scores;
 
     int temp = 0;
@@ -127,17 +459,30 @@ get_retrieved_path_scores(const std::vector<grouped_vocabulary_tree<HistT, 8>::r
         ++temp;
     }
 
+    if (verbose) {
+        std::cout << "Exiting get_retrieved_path_scores" << std::endl;
+    }
+
     return path_scores;
+
+#endif
 }
 
 template <typename VocabularyT>
 std::vector<std::pair<typename path_result<VocabularyT>::type, typename VocabularyT::result_type> >
 query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, VocabularyT& vt,
                  const boost::filesystem::path& vocabulary_path,
-                 const vocabulary_summary& summary)
+                 const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all)
 {
+    if (verbose) {
+        std::cout << "Entering query_vocabulary ..." << std::endl;
+    }
+
     // we need to cache the vt if we are to do this multiple times
     if (vt.empty()) {
+        if (verbose) {
+            std::cout << "No vocabulary loaded, reading vocabulary..." << std::endl;
+        }
         load_vocabulary(vt, vocabulary_path);
         vt.set_min_match_depth(3);
         vt.compute_normalizing_constants();
@@ -145,9 +490,35 @@ query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, VocabularyT& vt,
 
     // add some common methods in vt for querying, really only one!
     std::vector<typename VocabularyT::result_type> scores;
-    vt.query_vocabulary(scores, features, nbr_query);
+    if (kind == uri_kind::mongodb) {
+        vt.query_vocabulary(scores, features, 0); // return all results, remove some later
+    }
+    else {
+        vt.query_vocabulary(scores, features, nbr_query);
+    }
+
+    // maybe this would be the place to remove the rotten eggs, considering scores is passed as const
+    using result_type = std::pair<typename path_result<VocabularyT>::type, typename VocabularyT::result_type>;
+    std::vector<std::pair<typename path_result<VocabularyT>::type, typename VocabularyT::result_type> > results = get_retrieved_path_scores(scores, summary, verbose, kind);
+    results.erase(std::remove_if(results.begin(), results.end(), [&kind](const result_type& r) {
+        if (r.first.empty()) {
+            return true;
+        }
+        boost::filesystem::path segment_path = r.first;
+        std::string name = segment_path.stem().string();
+        if (kind == uri_kind::mongodb && name != "segment") { // for the mongodb results
+            return true;
+        }
+        return false;
+    }), results.end());
+    results.resize(std::min(nbr_query, results.size()));
+
+    if (verbose) {
+        std::cout << "Exiting query_vocabulary with " << results.size() << " nbr results ..." << std::endl;
+    }
 
-    return get_retrieved_path_scores(scores, summary);
+
+    return results;
 }
 
 // OK, the solution here is to turn the groups into the path scores
@@ -155,7 +526,7 @@ template <>
 std::vector<std::pair<path_result<grouped_vocabulary_tree<HistT, 8> >::type, typename grouped_vocabulary_tree<HistT, 8>::result_type> >
 query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, grouped_vocabulary_tree<HistT, 8>& vt,
                  const boost::filesystem::path& vocabulary_path,
-                 const vocabulary_summary& summary)
+                 const vocabulary_summary& summary, bool verbose, uri_kind kind)
 {
     // we need to cache the vt if we are to do this multiple times
     if (vt.empty()) {
@@ -166,9 +537,30 @@ query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, grouped_vocabulary
 
     // add some common methods in vt for querying, really only one!
     std::vector<typename grouped_vocabulary_tree<HistT, 8>::result_type> scores;
-    vt.query_vocabulary(scores, features, nbr_query);
+    if (kind == uri_kind::mongodb) {
+        vt.query_vocabulary(scores, features, 300); // TODO: make this a constant!
+    }
+    else {
+        vt.query_vocabulary(scores, features, nbr_query);
+    }
+
+    using path_type = path_result<grouped_vocabulary_tree<HistT, 8> >::type;
+    using result_type = std::pair<path_type, grouped_vocabulary_tree<HistT, 8>::result_type>;
+    std::vector<result_type> results = get_retrieved_path_scores(scores, summary, verbose, kind);
+    results.erase(std::remove_if(results.begin(), results.end(), [&kind](const result_type& r) {
+        if (r.first.empty()) {
+            return true;
+        }
+        boost::filesystem::path segment_path = r.first[0];
+        std::string name = segment_path.stem().string();
+        if (kind == uri_kind::mongodb && name != "segment") { // for the mongodb results
+            return true;
+        }
+        return false;
+    }), results.end());
+    results.resize(std::min(nbr_query, results.size()));
 
-    return get_retrieved_path_scores(scores, summary);
+    return results;
 }
 
 void insert_index_score(std::vector<std::pair<int, double> >& weighted_indices, const vocabulary_tree<HistT, 8>::result_type& index, float score)
@@ -480,7 +872,7 @@ std::pair<std::vector<std::pair<typename path_result<VocabularyT>::type, typenam
 std::vector<std::pair<typename path_result<VocabularyT>::type, typename VocabularyT::result_type> > >
 query_reweight_vocabulary(VocabularyT& vt, HistCloudT::Ptr& query_features,
                           size_t nbr_query, const boost::filesystem::path& vocabulary_path,
-                          const vocabulary_summary& summary)
+                          const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all)
 {
     using result_type = std::vector<std::pair<typename path_result<VocabularyT>::type, typename VocabularyT::result_type> >;
 
@@ -495,7 +887,7 @@ query_reweight_vocabulary(VocabularyT& vt, HistCloudT::Ptr& query_features,
     std::cout << "Querying vocabulary..." << std::endl;
     TICK("query_vocabulary");
 
-    result_type retrieved_paths = query_vocabulary(query_features, nbr_query, vt, vocabulary_path, summary);
+    result_type retrieved_paths = query_vocabulary(query_features, nbr_query, vt, vocabulary_path, summary, verbose, kind);
     TOCK("query_vocabulary");
 
     return make_pair(retrieved_paths, result_type());
diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h
index f60ac110..93624c28 100644
--- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h
+++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h
@@ -85,6 +85,8 @@ struct vocabulary_summary {
     size_t max_training_features;
     size_t max_append_features;
 
+    std::string last_updated; // string for checking last update, used to know when to reload
+
     void load(const boost::filesystem::path& data_path)
     {
         std::ifstream in((data_path / boost::filesystem::path("vocabulary_summary.json")).string());
@@ -133,7 +135,8 @@ struct vocabulary_summary {
                 cereal::make_nvp("vocabulary_tree_size", vocabulary_tree_size),
                 cereal::make_nvp("min_segment_features", min_segment_features),
                 cereal::make_nvp("max_training_features", max_training_features),
-                cereal::make_nvp("max_append_features", max_append_features));
+                cereal::make_nvp("max_append_features", max_append_features),
+                cereal::make_nvp("last_updated", last_updated));
     }
 
     vocabulary_summary() : vocabulary_type("standard"), subsegment_type(""), noise_data_path(""), annotated_data_path(""), nbr_noise_segments(0), nbr_annotated_segments(0),
@@ -235,6 +238,77 @@ struct sweep_summary {
     sweep_summary() : nbr_segments(0), segment_indices(), segment_annotations() {}
 };
 
+struct segment_uris {
+
+    //std::string noise_data_path;
+    std::vector<std::string> uris;
+
+    void load(const boost::filesystem::path& data_path)
+    {
+        std::ifstream in((data_path / boost::filesystem::path("segment_uris.json")).string());
+        {
+            cereal::JSONInputArchive archive_i(in);
+            archive_i(*this);
+        }
+        /*
+        if (!boost::filesystem::path(noise_data_path).is_absolute()) {
+            noise_data_path = boost::filesystem::canonical(noise_data_path, data_path.parent_path()).string();
+        }
+        */
+
+        // we can actually assume that 0 is a file since the first ones are used to train the vocabulary
+        if (!uris.empty() && !boost::filesystem::path(uris[0].substr(7, uris[0].size()-7)).is_absolute()) {
+            for (std::string& segment_path : uris) {
+                //segment_path = boost::filesystem::canonical(segment_path, data_path).string();
+                //HMMM, this will start with a slash?
+                if (segment_path.compare(0, 7, "file://") == 0) {
+                    segment_path = std::string("file://") + boost::filesystem::absolute(segment_path.substr(7, segment_path.size()-7), data_path.parent_path()).string();
+                }
+            }
+        }
+    }
+
+    void save(const boost::filesystem::path& data_path)
+    {
+        // we should just have some way of checking if the paths are childs of data_path, in that case replace with relative path
+        // but this is in general not true for this data_path, which is the vocabulary path!
+        // so, instead check if the data paths are children of the parent of data_path, i.e. if they are siblings
+        /*
+        if (is_sub_dir(boost::filesystem::path(noise_data_path), data_path.parent_path())) {
+            noise_data_path = make_relative(boost::filesystem::path(noise_data_path), data_path.parent_path()).string();
+        }
+        */
+
+        // we can actually assume that 0 is a file since the first ones are used to train the vocabulary
+        if (!uris.empty() && is_sub_dir(boost::filesystem::path(uris[0].substr(7, uris[0].size()-7)), data_path.parent_path())) {
+            for (std::string& segment_path : uris) {
+                if (segment_path.compare(0, 7, "file://") == 0) {
+                    segment_path = std::string("file://") + make_relative(boost::filesystem::path(segment_path.substr(7, segment_path.size()-7)), data_path.parent_path()).string();
+                }
+            }
+        }
+
+        std::ofstream out((data_path / boost::filesystem::path("segment_uris.json")).string());
+        {
+            cereal::JSONOutputArchive archive_o(out);
+            archive_o(*this);
+        }
+    }
+
+    template <class Archive>
+    void serialize(Archive& archive)
+    {
+//        archive(cereal::make_nvp("noise_data_path", noise_data_path),
+//                cereal::make_nvp("uris", uris));
+        archive(cereal::make_nvp("uris", uris));
+    }
+
+    segment_uris() : uris() //noise_data_path(""), uris()
+    {
+
+    }
+};
+
 } // namespace dynamic_object_retrieval
 
 #endif // SUMMARY_TYPES_H
diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/eigen_cereal/eigen_cereal.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/eigen_cereal/eigen_cereal.h
index c00f039b..8330b6ab 100644
--- a/dynamic_object_retrieval/dynamic_object_retrieval/include/eigen_cereal/eigen_cereal.h
+++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/eigen_cereal/eigen_cereal.h
@@ -3,11 +3,14 @@
 
 #include <cereal/cereal.hpp>
 #include <cereal/archives/binary.hpp>
-#include <Eigen/Dense>
+#include <cereal/types/vector.hpp>
+#include <eigen3/Eigen/Dense>
 #include <fstream>
 
 namespace cereal
 {
+
+// for binary serialization
 template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> inline
 typename std::enable_if<traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value, void>::type
 save(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> const & m)
@@ -30,6 +33,49 @@ load(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _Max
     m.resize(rows, cols);
     ar(binary_data(m.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
 }
+
+// for json serialization
+template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> inline
+typename std::enable_if<!traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value, void>::type
+save(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> const & m)
+{
+    int32_t rows = m.rows();
+    int32_t cols = m.cols();
+    ar(rows);
+    ar(cols);
+
+    std::vector<std::vector<_Scalar> > vec(rows);
+    for (int i = 0; i < rows; i++) {
+        vec[i].resize(cols);
+        for (int j = 0; j < cols; j++) {
+            vec[i][j] = m(i, j);
+        }
+    }
+
+    ar(vec);
+}
+
+template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> inline
+typename std::enable_if<!traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value, void>::type
+load(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> & m)
+{
+    int32_t rows;
+    int32_t cols;
+    ar(rows);
+    ar(cols);
+    std::vector<std::vector<_Scalar> > vec;
+    ar(vec);
+
+    m.resize(rows, cols);
+
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < cols; j++) {
+            m(i, j) = vec[i][j];
+        }
+    }
+
+}
+
 }
 
 #endif // EIGEN_CEREAL_H
diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/src/extract_surfel_features.cpp b/dynamic_object_retrieval/dynamic_object_retrieval/src/extract_surfel_features.cpp
index 9f6c2799..31d947a8 100644
--- a/dynamic_object_retrieval/dynamic_object_retrieval/src/extract_surfel_features.cpp
+++ b/dynamic_object_retrieval/dynamic_object_retrieval/src/extract_surfel_features.cpp
@@ -144,8 +144,6 @@ void compute_features(HistCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT:
     for (size_t i = 0; i < pfhrgb_cloud.size(); ++i) {
         std::copy(pfhrgb_cloud.at(i).histogram, pfhrgb_cloud.at(i).histogram+N, features->at(i).histogram);
     }
-
-    std::cout << "Number of features: " << pfhrgb_cloud.size() << std::endl;
 }
 
 NormalCloudT::Ptr compute_surfel_normals(SurfelCloudT::Ptr& surfel_cloud, CloudT::Ptr& segment)
diff --git a/dynamic_object_retrieval/k_means_tree/impl/grouped_vocabulary_tree.hpp b/dynamic_object_retrieval/k_means_tree/impl/grouped_vocabulary_tree.hpp
index 2ab39dfb..223fc910 100644
--- a/dynamic_object_retrieval/k_means_tree/impl/grouped_vocabulary_tree.hpp
+++ b/dynamic_object_retrieval/k_means_tree/impl/grouped_vocabulary_tree.hpp
@@ -73,8 +73,15 @@ void grouped_vocabulary_tree<Point, K>::query_vocabulary(vector<result_type>& up
 
         vector<int> selected_indices;
         // get<1>(scores[i])) is actually the index within the group!
-        double score = super::compute_min_combined_dist(selected_indices, query_cloud, vectors, adjacencies,
-                                                        mapping, inverse_mapping, scores[i].subgroup_index);
+        double score;
+        if (adjacencies.empty()) {
+            score = scores[i].score;
+            selected_indices.push_back(0);
+        }
+        else {
+            score = super::compute_min_combined_dist(selected_indices, query_cloud, vectors, adjacencies,
+                                                     mapping, inverse_mapping, scores[i].subgroup_index);
+        }
         //double score = scores[i].score;
         //selected_indices.push_back(scores[i].subgroup_index);
         updated_scores.push_back(result_type(float(score), scores[i].group_index, selected_indices[0]));
@@ -105,6 +112,9 @@ void grouped_vocabulary_tree<Point, K>::query_vocabulary(vector<result_type>& up
     vector<pair<result_type, size_t> > index_scores;
     int current_index = -1;
     for (size_t i = 0; i < updated_scores.size(); ++i) {
+        if (updated_scores[i].group_index == 100000) {
+            continue;
+        }
         if (updated_scores[i].group_index != current_index) {
             current_index = updated_scores[i].group_index;
             std::sort(index_scores.begin(), index_scores.end(), [](const pair<result_type, size_t>& s1, const pair<result_type, size_t>& s2)
@@ -281,6 +291,10 @@ template <typename Point, size_t K>
 void grouped_vocabulary_tree<Point, K>::load_cached_vocabulary_vectors_for_group(vector<vocabulary_vector>& vectors,
                                                                                  set<pair<int, int> >& adjacencies, int i)
 {
+    if (i == 100000) { // this is hardcoded atm, but segments that don't have adjacencies should use this index
+        return;
+    }
+
     boost::filesystem::path cache_path = boost::filesystem::path(save_state_path) / "vocabulary_vectors";
 
     stringstream ss;
diff --git a/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp b/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp
index 84f21f71..dd162a8c 100644
--- a/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp
+++ b/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp
@@ -299,6 +299,7 @@ void vocabulary_tree<Point, K>::source_freqs_for_node(std::map<int, int>& source
     }
 }
 
+// we should make some const here to make clear what happens
 template <typename Point, size_t K>
 double vocabulary_tree<Point, K>::compute_min_combined_dist(vector<int>& included_indices, CloudPtrT& cloud, vector<vocabulary_vector>& smaller_freqs,
                                                             set<pair<int, int> >& adjacencies, map<node*, int>& mapping, map<int, node*>& inverse_mapping, int hint) // TODO: const
@@ -838,3 +839,19 @@ void vocabulary_tree<Point, K>::load(Archive& archive)
     cout << "Finished loading vocabulary_tree" << endl;
 }
 */
+
+template <typename Point, size_t K>
+double vocabulary_tree<Point, K>::compute_distance(const std::map<node*, double>& vec1, double norm1,
+                                                   const std::map<node*, double>& vec2, double norm2) const
+{
+    double score = 0.0;
+    for (const pair<node* const, double>& n : vec1) {
+        if (vec2.count(n.first) == 0) {
+            continue;
+        }
+        // actually we don't need to check the depth of the node since that is already handled by compute_query_vector: GREAT!
+        score += std::min(n.second, vec2.at(n.first));
+    }
+    score = 1.0 - score/std::max(norm1, norm2);
+    return score;
+}
diff --git a/dynamic_object_retrieval/k_means_tree/include/vocabulary_tree/vocabulary_tree.h b/dynamic_object_retrieval/k_means_tree/include/vocabulary_tree/vocabulary_tree.h
index 4a2564e9..5d3a7f1e 100644
--- a/dynamic_object_retrieval/k_means_tree/include/vocabulary_tree/vocabulary_tree.h
+++ b/dynamic_object_retrieval/k_means_tree/include/vocabulary_tree/vocabulary_tree.h
@@ -76,7 +76,6 @@ class vocabulary_tree : public k_means_tree<Point, K, inverted_file> {
 
     double pexp(const double v) const;
     double proot(const double v) const;
-    double compute_query_vector(std::map<node*, double>& query_id_freqs, CloudPtrT& query_cloud);
     void compute_query_vector(std::map<node*, int>& query_id_freqs, CloudPtrT& query_cloud);
     double compute_query_vector(std::map<node*, std::pair<double, int> >& query_id_freqs, CloudPtrT& query_cloud);
     void source_freqs_for_node(std::map<int, int>& source_id_freqs, node* n) const;
@@ -129,6 +128,9 @@ class vocabulary_tree : public k_means_tree<Point, K, inverted_file> {
     void compute_query_index_vector(std::map<int, int>& query_index_freqs, CloudPtrT& query_cloud, std::map<node*, int>& mapping);
     vocabulary_vector compute_query_index_vector(CloudPtrT& query_cloud, std::map<node *, int> &mapping);
 
+    double compute_query_vector(std::map<node*, double>& query_id_freqs, CloudPtrT& query_cloud);
+    double compute_distance(const std::map<node*, double>& vec1, double norm1, const std::map<node*, double>& vec2, double norm2) const;
+
     /*
     template <class Archive> void save(Archive& archive) const;
     template <class Archive> void load(Archive& archive);
diff --git a/learn_objects_action/launch/learn_objects_dependencies.launch b/learn_objects_action/launch/learn_objects_dependencies.launch
index 72d4dade..e6b067bc 100644
--- a/learn_objects_action/launch/learn_objects_dependencies.launch
+++ b/learn_objects_action/launch/learn_objects_dependencies.launch
@@ -5,19 +5,25 @@
   <arg name="user" default="" />
   <arg name="camera_topic" default="/head_xtion/depth_registered" />
   <arg name="model_path" default="/home/strands/models/" />
+  <!-- 
+  <arg name="planning_method" default="InfoGain" />
+  <arg name="planning_method" default="ral16" />
+  -->
   <arg name="planning_method" default="ral16" />
   <arg name="object_view_map_topic" default="/waypoint_map" />
   <arg name="record_run" default="false" />
 
   <machine name="$(arg machine)" address="$(arg machine)" env-loader="$(optenv ROS_ENV_LOADER )" user="$(arg user)" default="true" />
 
+	<node machine="$(arg machine)" pkg="quasimodo_brain" type="metaroom_additional_view_processing" name="metaroom_additional_view_processing" output="screen" respawn="true"/>
+
     <node machine="$(arg machine)" pkg="object_view_generator" type="view_points_service.py" name="object_view_generator" output="screen" respawn="true">
 	<param name="object_view_map_topic" value="$(arg object_view_map_topic)"/>
     </node>
 
     <node machine="$(arg machine)" pkg="ptu_follow_frame" type="ptu_follow.py" name="ptu_follow_frame" output="screen" respawn="true"/>
 
-    <node machine="$(arg machine)" pkg="semantic_map_to_2d" type="semantic_map_2d_server" name="semantic_map_2d_server" output="screen" respawn="true"/>
+	<node machine="$(arg machine)" pkg="semantic_map_to_2d" type="semantic_map_2d_server" name="semantic_map_2d_server" output="screen" respawn="true"/>
 
     <node machine="$(arg machine)" pkg="static_transform_manager" type="static_tf_services.py" name="static_transforms_manager" output="screen" respawn="true"/>
 
diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py
index bd349195..2b33cbde 100644
--- a/learn_objects_action/src/learn_objects_action/metric_sweep.py
+++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py
@@ -127,7 +127,9 @@ def __init__(self, rois_file, debug_mode, debug_services):
 
 
     def execute(self, userdata):
+        print "SelectCluster::execute"
         try:
+            print "LINE::1"
             # Load the waypoint to soma from file
             if self._rois_file != "NONE":
                 with open(self._rois_file, "r") as f:
@@ -135,10 +137,13 @@ def execute(self, userdata):
                 soma_region = somas[userdata.action_goal.waypoint]
             else:
                 soma_region = ""
-
+                
+            print "LINE::1.5"
             #clusters = self._get_clusters(userdata['waypoint'])
             clusters = self._get_clusters(userdata.action_goal.waypoint)
 
+            print "LINE::2"
+            
             if self._debug_mode:
                 available=[str(s) for s in range(len(clusters.object_id))]
                 self._debug_services.set_status("SELECT CLUSTER: ["+",".join(available)+"]")
@@ -146,6 +151,8 @@ def execute(self, userdata):
             else:
                 select="auto"
 
+            print "LINE::3"
+            
             if select=="PREEMPT" or select=="none":
                 return 'none'
             elif select=="auto":
@@ -169,21 +176,41 @@ def execute(self, userdata):
             else:
                 ID=int(select)
 
+
+            print "LINE:4"
             rospy.logwarn( "Getting cluster: %s"%clusters.object_id[ID])
             #one_cluster = self._get_specific_cluster(userdata['waypoint'], clusters.object_id[ID])
+            
             one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint,
                                                      clusters.object_id[ID])
             userdata['dynamic_object']=one_cluster
             userdata.dynamic_object_centroid = clusters.centroids[ID]
             userdata.dynamic_object_points = clusters.objects[ID]
             userdata.dynamic_object_id = clusters.object_id[ID]
-            world = World()
-            userdata['object'] = world.create_object()
-            userdata['object'].add_identification('ObjectLearnerID', ObjectIdentification({'NEW':1}))
+            print "LINE:5"
+            #world = World()
+            print "LINE:6"
+            #userdata['object'] = world.create_object()
+            print "LINE:7"
+            #userdata['object'].add_identification('ObjectLearnerID', ObjectIdentification({'NEW':1}))
+            print "LINE:8"
             print clusters.object_id[ID]
+            print "LINE:9"
             print "="*20
             print clusters.centroids[ID]
+            print "LINE:10"
             print "="*20
+            
+            #print userdata.dynamic_object_centroid
+            #print "LINE:10"
+            #print "="*20
+            
+                        
+            #print userdata
+            #print "LINE:10"
+            #print "="*20
+            
+            print "LINE:11"
             return "selected"
         except Exception, e:
             rospy.logwarn("Failed to select a cluster!!!!")
diff --git a/learn_objects_action/src/learn_objects_action/ptu_track.py b/learn_objects_action/src/learn_objects_action/ptu_track.py
index 147aa901..915e845d 100644
--- a/learn_objects_action/src/learn_objects_action/ptu_track.py
+++ b/learn_objects_action/src/learn_objects_action/ptu_track.py
@@ -33,28 +33,37 @@ def __init__(self, debug_mode, debug_services):
 
 
     def execute(self, userdata):
+        print "start TurnPTUToObject::execute"
         try:
+            print "LINE 1"
             if self._debug_mode:
                 self._debug_services.set_status("Turn PTU to cluster?")
                 proceed = self._debug_services.get_proceed(("OK",))
                 if proceed == "PREEMPT":
                     return 'preempted'
             # start transformation
+            print "LINE 2"
             self._jnts.position=[math.radians(userdata.dynamic_object.pan_angle),
                                  math.radians(userdata.dynamic_object.tilt_angle)]
+            print "LINE 3"
             goal = PtuGotoGoal()
+            print "LINE 4"
             goal.pan = userdata.dynamic_object.pan_angle
             goal.tilt = userdata.dynamic_object.tilt_angle
             goal.pan_vel = 30
             goal.tilt_vel = 30
+            print "LINE 5"
             rospy.loginfo("SET PTU: pan: %f tilt: %f", goal.pan, goal.tilt)
             self.ptu_client.send_goal(goal)
             self.ptu_client.wait_for_result()
             rospy.loginfo("Reached ptu goal")
 
+            print "LINE 6"
             print "Capturing a new shot of that object before tracking."
             # So uggly force get through publisher queue
             for i in range(30):
+            	print "LINE "
+            	print i
                 userdata.dynamic_object.object_cloud= rospy.wait_for_message("/head_xtion/depth_registered/points", PointCloud2)
                 #  is already there
             print "ok."
diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp
index e637365c..08709e28 100644
--- a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp
+++ b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp
@@ -404,7 +404,7 @@ std::vector<DynamicObjectData<PointType>> loadAllDynamicObjectsFromSingleSweep(s
     if (verbose){
         std::cout<<"Found "<<objectFiles.size()<<" dynamic objects."<<std::endl;
     }
-
+//printf("%s::%i",__FILE__,__LINE__);
     for (auto objectFile : objectFiles){
         if (verbose){
             std::cout<<"Now parsing "<<objectFile.toStdString()<<std::endl;
diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/simple_dynamic_object_parser.h b/metaroom_xml_parser/include/metaroom_xml_parser/simple_dynamic_object_parser.h
index 4d4724fd..28b3c27a 100644
--- a/metaroom_xml_parser/include/metaroom_xml_parser/simple_dynamic_object_parser.h
+++ b/metaroom_xml_parser/include/metaroom_xml_parser/simple_dynamic_object_parser.h
@@ -116,7 +116,7 @@ class SimpleDynamicObjectParser {
                     }
                     if (attributes.hasAttribute("roomRunNumber"))
                     {
-                        toRet.m_roomRunNumber = attributes.value("roomRunNumber").toString().toInt();
+						toRet.m_roomRunNumber = atoi(attributes.value("roomRunNumber").toString().toStdString().c_str());//.toInt();
                     } else {
                         std::cerr<<"Object xml node does not have roomRunNumber attribute."<<std::endl; // leaving blank
                     }
@@ -142,7 +142,7 @@ class SimpleDynamicObjectParser {
 
                     if (attributes.hasAttribute("additionalViews"))
                     {
-                        int additionalViews = attributes.value("additionalViews").toString().toInt();
+						int additionalViews = atoi(attributes.value("additionalViews").toString().toStdString().c_str());//.toInt();
                         if (load_cloud)
                         {
                             // load clouds
diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h b/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h
index b977b060..91564460 100644
--- a/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h
+++ b/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h
@@ -253,7 +253,8 @@ friend class SimpleDynamicObjectParser;
                 }
                 if (xmlReader->name() == "RoomRunNumber")
                 {
-                    aRoom.roomRunNumber = xmlReader->readElementText().toInt();
+					QString tmp = xmlReader->readElementText();
+                    aRoom.roomRunNumber = atoi(tmp.toStdString().c_str());//xmlReader->readElementText().toInt();
                 }
 
                 if (xmlReader->name() == "RoomLogStartTime")
@@ -325,7 +326,10 @@ friend class SimpleDynamicObjectParser;
                     QStringList transformSlist = transformS.split(' ');
                     for (size_t i=0; i<16;i++)
                     {
-                        transform(i)=transformSlist[i].toDouble();
+                        //QString tmp = xmlReader->readElementText();
+                        //aRoom.roomRunNumber = atoi(tmp.toStdString().c_str());//xmlReader->readElementText().toInt();
+                        transform(i)=atof(transformSlist[i].toStdString().c_str());
+                        //transform(i)=transformSlist[i].toDouble();
                     }
                     aRoom.roomTransform = transform;
                 }
@@ -382,12 +386,13 @@ friend class SimpleDynamicObjectParser;
             {
                 if (xmlReader.name() == "sec")
                 {
-                    int sec = xmlReader.readElementText().toInt();
+
+                    int sec = atoi(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toInt();
                     tfmsg.header.stamp.sec = sec;
                 }
                 if (xmlReader.name() == "nsec")
                 {
-                    int nsec = xmlReader.readElementText().toInt();
+                    int nsec = atoi(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toInt();
                     tfmsg.header.stamp.nsec = nsec;
                 }
                 if (xmlReader.name() == "FrameId")
@@ -410,12 +415,12 @@ friend class SimpleDynamicObjectParser;
                 }
                 if (xmlReader.name() == "w")
                 {
-                    double w = xmlReader.readElementText().toDouble();
+                    double w = atof(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toDouble();
                     tfmsg.transform.rotation.w = w;
                 }
                 if (xmlReader.name() == "x")
                 {
-                    double x = xmlReader.readElementText().toDouble();
+                    double x = atof(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toDouble();
                     if (intermediateParentNode == "Rotation")
                     {
                         tfmsg.transform.rotation.x = x;
@@ -427,7 +432,7 @@ friend class SimpleDynamicObjectParser;
                 }
                 if (xmlReader.name() == "y")
                 {
-                    double y = xmlReader.readElementText().toDouble();
+                    double y = atof(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toDouble();
                     if (intermediateParentNode == "Rotation")
                     {
                         tfmsg.transform.rotation.y = y;
@@ -439,7 +444,7 @@ friend class SimpleDynamicObjectParser;
                 }
                 if (xmlReader.name() == "z")
                 {
-                    double z = xmlReader.readElementText().toDouble();
+                    double z = atof(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toDouble();
                     if (intermediateParentNode == "Rotation")
                     {
                         tfmsg.transform.rotation.z = z;
@@ -470,7 +475,7 @@ friend class SimpleDynamicObjectParser;
                     if (paramAttributes.hasAttribute("height"))
                     {
                         QString val = paramAttributes.value("height").toString();
-                        camInfo.height = val.toInt();
+                        camInfo.height = atoi(val.toStdString().c_str());//val.toInt();
 
                     } else {
                         ROS_ERROR("RoomIntermediateCameraParameters xml node does not have the height attribute. Cannot construct camera parameters object.");
@@ -480,7 +485,7 @@ friend class SimpleDynamicObjectParser;
                     if (paramAttributes.hasAttribute("width"))
                     {
                         QString val = paramAttributes.value("width").toString();
-                        camInfo.width = val.toInt();
+                        camInfo.width = atoi(val.toStdString().c_str());//val.toInt();
 
                     } else {
                         ROS_ERROR("RoomIntermediateCameraParameters xml node does not have the width attribute. Cannot construct camera parameters object.");
@@ -518,7 +523,7 @@ friend class SimpleDynamicObjectParser;
                         } else {
                             for (size_t j=0; j< val_list.size();j++)
                             {
-                                camInfo.K[j] = val_list[j].toDouble();
+                                camInfo.K[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble();
                             }
                         }
                     } else {
@@ -537,7 +542,7 @@ friend class SimpleDynamicObjectParser;
                         } else {
                             for (size_t j=0; j< val_list.size();j++)
                             {
-                                camInfo.D.push_back(val_list[j].toDouble());
+                                camInfo.D.push_back(atof(val_list[j].toStdString().c_str()));//val_list[j].toDouble());
                             }
                         }
                     } else {
@@ -556,7 +561,7 @@ friend class SimpleDynamicObjectParser;
                         } else {
                             for (size_t j=0; j< val_list.size();j++)
                             {
-                                camInfo.R[j] = val_list[j].toDouble();
+                                camInfo.R[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble();
                             }
                         }
                     } else {
@@ -575,7 +580,7 @@ friend class SimpleDynamicObjectParser;
                         } else {
                             for (size_t j=0; j< val_list.size();j++)
                             {
-                                camInfo.P[j] = val_list[j].toDouble();
+                                camInfo.P[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble();
                             }
                         }
                     } else {
@@ -636,13 +641,13 @@ friend class SimpleDynamicObjectParser;
         // read in number of depth and rgb images
         int numRGB, numDepth;
         if (attributes.hasAttribute("RGB_Images"))
-        {
-            numRGB = attributes.value("RGB_Images").toString().toInt();
+        {//atoi(val.toStdString().c_str());//
+            numRGB = atoi(attributes.value("RGB_Images").toString().toStdString().c_str());//.toInt();
         }
 
         if (attributes.hasAttribute("Depth_Images"))
         {
-            numDepth = attributes.value("Depth_Images").toString().toInt();
+            numDepth = atoi(attributes.value("Depth_Images").toString().toStdString().c_str());//.toInt();
         }
 
         // read in the images
@@ -723,7 +728,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("height"))
             {
                 QString val = paramAttributes.value("height").toString();
-                camInfo.height = val.toInt();
+                camInfo.height = atoi(val.toStdString().c_str());//val.toInt();
 
             } else {
                 ROS_ERROR("%s xml node does not have the height attribute. Cannot construct camera parameters object.",xmlReader->name().toString().toStdString().c_str());
@@ -733,7 +738,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("width"))
             {
                 QString val = paramAttributes.value("width").toString();
-                camInfo.width = val.toInt();
+                camInfo.width = atoi(val.toStdString().c_str());//val.toInt();
 
             } else {
                 ROS_ERROR("%s xml node does not have the width attribute. Cannot construct camera parameters object.",xmlReader->name().toString().toStdString().c_str());
@@ -771,7 +776,7 @@ friend class SimpleDynamicObjectParser;
                 } else {
                     for (size_t j=0; j< val_list.size();j++)
                     {
-                        camInfo.K[j] = val_list[j].toDouble();
+                        camInfo.K[j] = atof(val_list[j].toStdString().c_str());//toDouble();
                     }
                 }
             } else {
@@ -790,7 +795,7 @@ friend class SimpleDynamicObjectParser;
                 } else {
                     for (size_t j=0; j< val_list.size();j++)
                     {
-                        camInfo.D.push_back(val_list[j].toDouble());
+                        camInfo.D.push_back(atof(val_list[j].toStdString().c_str()));//.toDouble());
                     }
                 }
             } else {
@@ -809,7 +814,7 @@ friend class SimpleDynamicObjectParser;
                 } else {
                     for (size_t j=0; j< val_list.size();j++)
                     {
-                        camInfo.R[j] = val_list[j].toDouble();
+                        camInfo.R[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble();
                     }
                 }
             } else {
@@ -828,7 +833,7 @@ friend class SimpleDynamicObjectParser;
                 } else {
                     for (size_t j=0; j< val_list.size();j++)
                     {
-                        camInfo.P[j] = val_list[j].toDouble();
+                        camInfo.P[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble();
                     }
                 }
             } else {
@@ -869,7 +874,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Stamp_sec"))
             {
                 QString val = paramAttributes.value("Stamp_sec").toString();
-                tfmsg.header.stamp.sec = val.toInt();
+                tfmsg.header.stamp.sec = atoi(val.toStdString().c_str());//val.toInt();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Stamp_sec attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
@@ -878,7 +883,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Stamp_nsec"))
             {
                 QString val = paramAttributes.value("Stamp_nsec").toString();
-                tfmsg.header.stamp.nsec = val.toInt();
+                tfmsg.header.stamp.nsec = atoi(val.toStdString().c_str());//val.toInt();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Stamp_nsec attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
@@ -905,7 +910,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Trans_x"))
             {
                 QString val = paramAttributes.value("Trans_x").toString();
-                tfmsg.transform.translation.x = val.toDouble();
+                tfmsg.transform.translation.x = atof(val.toStdString().c_str());//val.toDouble();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Trans_x attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
@@ -914,7 +919,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Trans_y"))
             {
                 QString val = paramAttributes.value("Trans_y").toString();
-                tfmsg.transform.translation.y = val.toDouble();
+                tfmsg.transform.translation.y = atof(val.toStdString().c_str());//val.toDouble();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Trans_y attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
@@ -923,7 +928,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Trans_z"))
             {
                 QString val = paramAttributes.value("Trans_z").toString();
-                tfmsg.transform.translation.z = val.toDouble();
+                tfmsg.transform.translation.z = atof(val.toStdString().c_str());//val.toDouble();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Trans_z attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
@@ -932,7 +937,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Rot_w"))
             {
                 QString val = paramAttributes.value("Rot_w").toString();
-                tfmsg.transform.rotation.w = val.toDouble();
+                tfmsg.transform.rotation.w = atof(val.toStdString().c_str());//val.toDouble();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Rot_w attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
@@ -941,7 +946,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Rot_x"))
             {
                 QString val = paramAttributes.value("Rot_x").toString();
-                tfmsg.transform.rotation.x = val.toDouble();
+                tfmsg.transform.rotation.x = atof(val.toStdString().c_str());//val.toDouble();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Rot_x attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
@@ -950,7 +955,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Rot_y"))
             {
                 QString val = paramAttributes.value("Rot_y").toString();
-                tfmsg.transform.rotation.y = val.toDouble();
+                tfmsg.transform.rotation.y = atof(val.toStdString().c_str());//val.toDouble();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Rot_y attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
@@ -959,7 +964,7 @@ friend class SimpleDynamicObjectParser;
             if (paramAttributes.hasAttribute("Rot_z"))
             {
                 QString val = paramAttributes.value("Rot_z").toString();
-                tfmsg.transform.rotation.z = val.toDouble();
+                tfmsg.transform.rotation.z = atof(val.toStdString().c_str());//val.toDouble();
 
             } else {
                 ROS_ERROR("%s xml node does not have the Rot_z attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str());
diff --git a/metaroom_xml_parser/src/simple_summary_parser.cpp b/metaroom_xml_parser/src/simple_summary_parser.cpp
index 5e936f08..85362525 100644
--- a/metaroom_xml_parser/src/simple_summary_parser.cpp
+++ b/metaroom_xml_parser/src/simple_summary_parser.cpp
@@ -67,6 +67,9 @@ bool SimpleSummaryParser::createSummaryXML(std::string rootFolder, bool verbose)
     xmlWriter->writeEndDocument();
 
     ROS_INFO_STREAM("Done looking for observations. Found "<<m_vAllRooms.size()<<" observations.");
+    
+    delete xmlWriter;
+    xmlWriter = 0;
 
     return true;
 }
@@ -116,6 +119,9 @@ void SimpleSummaryParser::saveSemanticRooms(QXmlStreamWriter* xmlWriter, QString
         }
 
         file.close();
+        
+        delete xmlReader;
+    	xmlReader = 0;
     }
 
 
diff --git a/object_manager/CMakeLists.txt b/object_manager/CMakeLists.txt
index 21e492a5..aebe50b2 100644
--- a/object_manager/CMakeLists.txt
+++ b/object_manager/CMakeLists.txt
@@ -4,7 +4,7 @@ project(object_manager)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs geometry_msgs pcl_ros message_generation qt_build cv_bridge semantic_map metaroom_xml_parser convex_segmentation k_means_tree observation_registration_services)
+find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs geometry_msgs object_manager_msgs pcl_ros message_generation qt_build cv_bridge semantic_map metaroom_xml_parser convex_segmentation k_means_tree observation_registration_services)
 
 set(CMAKE_CXX_FLAGS "-O4 -fPIC -std=c++0x -fpermissive ${CMAKE_CXX_FLAGS}")
 
@@ -25,27 +25,6 @@ endif()
 
 rosbuild_prepare_qt4(QtCore QtXml)
 
-add_service_files(
-   FILES
-   DynamicObjectsService.srv
-   GetDynamicObjectService.srv
-   DynamicObjectComputeMaskService.srv
-   ProcessDynamicObjectService.srv
-)
-
-add_message_files(
-  FILES
-  DynamicObjectTracks.msg
-  DynamicObjectTrackingData.msg
-)
-
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-  sensor_msgs
-  geometry_msgs
-)
-
 catkin_package(
    INCLUDE_DIRS include
    LIBRARIES dynamic_object
@@ -76,13 +55,13 @@ include_directories(include
 add_library(dynamic_object ${HDRS} ${SRCS})
 
 add_executable(object_manager_node   ${HDRS} ${SRCS} include/object_manager/object_manager.h src/object_manager.cpp src/object_manager_main.cpp)
-add_dependencies(object_manager_node object_manager_node_generate_messages_cpp)
+add_dependencies(object_manager_node quasimodo_msgs_generate_messages_cpp object_manager_node_generate_messages_cpp)
 
 add_executable(load_objects_from_mongo ${HDRS} ${SRCS} src/load_objects_from_mongo.cpp)
-add_dependencies(load_objects_from_mongo load_objects_from_mongo_generate_messages_cpp)
+add_dependencies(load_objects_from_mongo quasimodo_msgs_generate_messages_cpp load_objects_from_mongo_generate_messages_cpp)
 
 add_executable(dynamic_object_compute_mask_server ${HDRS} ${SRCS} src/dynamic_object_compute_mask_server.cpp)
-add_dependencies(dynamic_object_compute_mask_server object_manager_generate_messages_cpp observation_registration_services_generate_messages_cpp)
+add_dependencies(dynamic_object_compute_mask_server quasimodo_msgs_generate_messages_cpp object_manager_generate_messages_cpp observation_registration_services_generate_messages_cpp)
 
  target_link_libraries(dynamic_object
    ${catkin_LIBRARIES}
diff --git a/object_manager/include/object_manager/dynamic_object.h b/object_manager/include/object_manager/dynamic_object.h
index c0a1a9f2..a8c396e7 100644
--- a/object_manager/include/object_manager/dynamic_object.h
+++ b/object_manager/include/object_manager/dynamic_object.h
@@ -44,11 +44,12 @@ class DynamicObject  {
         }
     };
 
-   DynamicObject(bool verbose = false);
+	DynamicObject(bool verbose = false);
    ~DynamicObject();
 
    void setTime(boost::posix_time::ptime);
    void setCloud(CloudPtr);
+   void setLabel(std::string label_);
 
    std::vector<tf::StampedTransform> getAdditionalViewTransforms();
    void addAdditionalViewTransform(tf::StampedTransform);
diff --git a/object_manager/include/object_manager/object_manager.h b/object_manager/include/object_manager/object_manager.h
index cd96d074..1a486e5e 100644
--- a/object_manager/include/object_manager/object_manager.h
+++ b/object_manager/include/object_manager/object_manager.h
@@ -20,19 +20,19 @@
 #include <sensor_msgs/image_encodings.h>
 
 // Services
-#include <object_manager/DynamicObjectsService.h>
-#include <object_manager/GetDynamicObjectService.h>
-#include <object_manager/ProcessDynamicObjectService.h>
+#include <object_manager_msgs/DynamicObjectsService.h>
+#include <object_manager_msgs/GetDynamicObjectService.h>
+#include <object_manager_msgs/ProcessDynamicObjectService.h>
 
 // Registration service
 #include <observation_registration_services/ObjectAdditionalViewRegistrationService.h>
 
 // Additional view mask service
-#include <object_manager/DynamicObjectComputeMaskService.h>
+#include <object_manager_msgs/DynamicObjectComputeMaskService.h>
 
 // Custom messages
-#include <object_manager/DynamicObjectTracks.h>
-#include <object_manager/DynamicObjectTrackingData.h>
+#include <object_manager_msgs/DynamicObjectTracks.h>
+#include <object_manager_msgs/DynamicObjectTrackingData.h>
 
 // PCL includes
 #include <pcl_ros/point_cloud.h>
@@ -69,14 +69,14 @@ class ObjectManager {
     typedef typename Cloud::Ptr CloudPtr;
     typedef semantic_map_load_utilties::DynamicObjectData<PointType> ObjectData;
 
-    typedef typename object_manager::DynamicObjectsService::Request DynamicObjectsServiceRequest;
-    typedef typename object_manager::DynamicObjectsService::Response DynamicObjectsServiceResponse;
+    typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest;
+    typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse;
 
-    typedef typename object_manager::GetDynamicObjectService::Request GetDynamicObjectServiceRequest;
-    typedef typename object_manager::GetDynamicObjectService::Response GetDynamicObjectServiceResponse;
+    typedef typename object_manager_msgs::GetDynamicObjectService::Request GetDynamicObjectServiceRequest;
+    typedef typename object_manager_msgs::GetDynamicObjectService::Response GetDynamicObjectServiceResponse;
 
-    typedef typename object_manager::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest;
-    typedef typename object_manager::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse;
+    typedef typename object_manager_msgs::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest;
+    typedef typename object_manager_msgs::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse;
 
     struct GetObjStruct
     {
@@ -98,7 +98,7 @@ class ObjectManager {
     bool returnObjectMask(std::string waypoint, std::string object_id, std::string observation_xml, GetObjStruct& returned_object);
     void additionalViewsCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
     void additionalViewsStatusCallback(const std_msgs::String& msg);
-    void dynamicObjectTracksCallback(const object_manager::DynamicObjectTracksConstPtr& msg);
+    void dynamicObjectTracksCallback(const object_manager_msgs::DynamicObjectTracksConstPtr& msg);
 
     static CloudPtr filterGroundClusters(CloudPtr dynamic, double min_height)
     {
@@ -167,7 +167,7 @@ ObjectManager<PointType>::ObjectManager(ros::NodeHandle nh) : m_TransformListene
     m_PublisherRequestedObjectCloud = m_NodeHandle.advertise<sensor_msgs::PointCloud2>("/object_manager/requested_object", 1, true);
     m_PublisherRequestedObjectImage = m_NodeHandle.advertise<sensor_msgs::Image>("/object_manager/requested_object_mask", 1, true);
     m_PublisherLearnedObjectXml = m_NodeHandle.advertise<std_msgs::String>("/object_learning/learned_object_xml", 1, false);
-    m_PublisherLearnedObjectTrackingData = m_NodeHandle.advertise<object_manager::DynamicObjectTrackingData>("/object_learning/learned_object_tracking_data", 1, false);
+    m_PublisherLearnedObjectTrackingData = m_NodeHandle.advertise<object_manager_msgs::DynamicObjectTrackingData>("/object_learning/learned_object_tracking_data", 1, false);
     m_PublisherLearnedObjectModel = m_NodeHandle.advertise<sensor_msgs::PointCloud2>("/object_learning/learned_object_model", 1, false);
 
     m_DynamicObjectsServiceServer = m_NodeHandle.advertiseService("ObjectManager/DynamicObjectsService", &ObjectManager::dynamicObjectsServiceCallback, this);
@@ -297,8 +297,8 @@ void ObjectManager<PointType>::additionalViewsStatusCallback(const std_msgs::Str
         }
 
         // create additional view masks
-        ros::ServiceClient client_masks = m_NodeHandle.serviceClient<object_manager::DynamicObjectComputeMaskService>("/dynamic_object_compute_mask_server");
-        object_manager::DynamicObjectComputeMaskService srv_masks;
+        ros::ServiceClient client_masks = m_NodeHandle.serviceClient<object_manager_msgs::DynamicObjectComputeMaskService>("/dynamic_object_compute_mask_server");
+        object_manager_msgs::DynamicObjectComputeMaskService srv_masks;
         srv_masks.request.observation_xml = m_objectTrackedObservation;
         srv_masks.request.object_xml = xml_file;
 
@@ -320,7 +320,7 @@ void ObjectManager<PointType>::additionalViewsStatusCallback(const std_msgs::Str
 
         // re-load object and publish tracks data
         ObjectData object = semantic_map_load_utilties::loadDynamicObjectFromSingleSweep<PointType>(xml_file);
-        object_manager::DynamicObjectTrackingData tracking_data_msg;
+        object_manager_msgs::DynamicObjectTrackingData tracking_data_msg;
 
         // views
         for (auto obj_view : object.vAdditionalViews){
@@ -407,7 +407,7 @@ void ObjectManager<PointType>::additionalViewsCallback(const sensor_msgs::PointC
 }
 
 template <class PointType>
-void ObjectManager<PointType>::dynamicObjectTracksCallback(const object_manager::DynamicObjectTracksConstPtr& msg)
+void ObjectManager<PointType>::dynamicObjectTracksCallback(const object_manager_msgs::DynamicObjectTracksConstPtr& msg)
 {
     if (m_objectTracked == NULL)
     {
@@ -522,14 +522,16 @@ bool ObjectManager<PointType>::dynamicObjectsServiceCallback(DynamicObjectsServi
 template <class PointType>
 bool ObjectManager<PointType>::getDynamicObject(std::string waypoint, std::string object_id, DynamicObject::Ptr& object, std::string& object_observation)
 {
+	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     auto it =  m_waypointToObjMap.find(waypoint);
     if (it == m_waypointToObjMap.end() )
     {
         ROS_ERROR_STREAM("No objects loaded for waypoint "+waypoint);
         return false;
     }
-
+	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     std::vector<DynamicObject::Ptr> objects = m_waypointToObjMap[waypoint];
+    printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     bool found = false;
     for (auto objectStruct : objects)
     {
@@ -541,10 +543,12 @@ bool ObjectManager<PointType>::getDynamicObject(std::string waypoint, std::strin
             break;
         }
     }
+    	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     if(!found)
     {
         ROS_ERROR_STREAM("Object "<<object_id<<" at waypoint "<<waypoint<<" could not be found.");
     }
+    	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     return true;
 }
 
@@ -624,8 +628,8 @@ bool ObjectManager<PointType>::processDynamicObjectServiceCallback(ProcessDynami
 //    }
 
     // create additional view masks
-    ros::ServiceClient client_masks = m_NodeHandle.serviceClient<object_manager::DynamicObjectComputeMaskService>("/dynamic_object_compute_mask_server");
-    object_manager::DynamicObjectComputeMaskService srv_masks;
+    ros::ServiceClient client_masks = m_NodeHandle.serviceClient<object_manager_msgs::DynamicObjectComputeMaskService>("/dynamic_object_compute_mask_server");
+    object_manager_msgs::DynamicObjectComputeMaskService srv_masks;
     srv_masks.request.observation_xml = req.observation_xml;
     srv_masks.request.object_xml = req.object_xml;
 
@@ -639,7 +643,7 @@ bool ObjectManager<PointType>::processDynamicObjectServiceCallback(ProcessDynami
 
     // re-load object and publish tracks data
     ObjectData processed_object = semantic_map_load_utilties::loadDynamicObjectFromSingleSweep<PointType>(req.object_xml);
-    object_manager::DynamicObjectTrackingData tracking_data_msg;
+    object_manager_msgs::DynamicObjectTrackingData tracking_data_msg;
 
     // views
     for (auto obj_view : processed_object.vAdditionalViews){
@@ -703,6 +707,7 @@ bool ObjectManager<PointType>::getDynamicObjectServiceCallback(GetDynamicObjectS
         return true;
     }
 
+	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     GetObjStruct object;
     bool found =returnObjectMask(req.waypoint_id, req.object_id,m_waypointToSweepFileMap[req.waypoint_id], object);
     if (!found)
@@ -710,23 +715,23 @@ bool ObjectManager<PointType>::getDynamicObjectServiceCallback(GetDynamicObjectS
         ROS_ERROR_STREAM("Could not compute mask for object id "<<req.object_id);
         return true;
     }
-
+	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     res.object_mask = object.object_indices;
     tf::transformTFToMsg(object.transform_to_map, res.transform_to_map);
     pcl::toROSMsg(*object.object_cloud, res.object_cloud);
     res.object_cloud.header.frame_id="/map";
     res.pan_angle = -object.pan_angle;
     res.tilt_angle = -object.tilt_angle;
-
+	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     m_PublisherRequestedObjectCloud.publish(res.object_cloud);
-
+	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     // convert to sensor_msgs::Image
     cv_bridge::CvImage aBridgeImage;
     aBridgeImage.image = object.object_mask;
     aBridgeImage.encoding = "bgr8";
     sensor_msgs::ImagePtr rosImage = aBridgeImage.toImageMsg();
     m_PublisherRequestedObjectImage.publish(rosImage);
-
+	printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
     // update tracked object
     bool trackedUpdated = getDynamicObject(req.waypoint_id, req.object_id, m_objectTracked, m_objectTrackedObservation);
     if (!trackedUpdated)
@@ -746,6 +751,7 @@ bool ObjectManager<PointType>::getDynamicObjectServiceCallback(GetDynamicObjectS
 template <class PointType>
 bool ObjectManager<PointType>::updateObjectsAtWaypoint(std::string waypoint_id)
 {
+	printf("START bool ObjectManager<PointType>::updateObjectsAtWaypoint(std::string waypoint_id)\n");
     using namespace std;
     std::vector<std::string> matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint<PointType>(m_dataFolder, waypoint_id);
     if (matchingObservations.size() == 0)
@@ -816,6 +822,7 @@ bool ObjectManager<PointType>::updateObjectsAtWaypoint(std::string waypoint_id)
 template <class PointType>
 std::vector<DynamicObject::Ptr>  ObjectManager<PointType>::loadDynamicObjectsFromObservation(std::string obs_file)
 {
+	printf("START std::vector<DynamicObject::Ptr>  ObjectManager<PointType>::loadDynamicObjectsFromObservation(std::string obs_file)\n");
     std::vector<DynamicObject::Ptr> dynamicObjects;
 
     // check if the objects have been computed already
diff --git a/object_manager/package.xml b/object_manager/package.xml
index bebc1ab2..e349122b 100644
--- a/object_manager/package.xml
+++ b/object_manager/package.xml
@@ -28,8 +28,10 @@
   <build_depend>convex_segmentation</build_depend>
   <build_depend>k_means_tree</build_depend>
   <build_depend>observation_registration_services</build_depend>
+  <build_depend>object_manager_msgs</build_depend>
 
 
+  <run_depend>object_manager_msgs</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
diff --git a/object_manager/src/dynamic_object.cpp b/object_manager/src/dynamic_object.cpp
index e01f1356..49b3c52f 100644
--- a/object_manager/src/dynamic_object.cpp
+++ b/object_manager/src/dynamic_object.cpp
@@ -20,13 +20,19 @@ DynamicObject::DynamicObject(bool verbose) : m_points(new Cloud()), m_normals(ne
     m_roomStringId = "";
     m_noAdditionalViews = 0;
     m_AdditionalViewsTransformToObservation.setIdentity();
+	//m_label = "";
 }
 
+
+
 DynamicObject::~DynamicObject()
 {
 
 }
 
+
+void DynamicObject::setLabel(std::string label){m_label = label;}
+
 void DynamicObject::setTime(boost::posix_time::ptime time)
 {
     m_time = time;
diff --git a/object_manager/src/dynamic_object_utilities.cpp b/object_manager/src/dynamic_object_utilities.cpp
index 97d308f7..b71fdd0f 100644
--- a/object_manager/src/dynamic_object_utilities.cpp
+++ b/object_manager/src/dynamic_object_utilities.cpp
@@ -5,6 +5,7 @@ using namespace std;
 
 std::vector<DynamicObject::Ptr> dynamic_object_utilities::loadDynamicObjects(std::string folder, bool verbose)
 {
+	printf("std::vector<DynamicObject::Ptr> dynamic_object_utilities::loadDynamicObjects(std::string folder, bool verbose)\n");
     std::vector<DynamicObject::Ptr>  objects;
 
     folder+=std::string("/");
@@ -23,6 +24,7 @@ std::vector<DynamicObject::Ptr> dynamic_object_utilities::loadDynamicObjects(std
     for (size_t i=0; i<objectFiles.size(); i++)
     {
         string object_file = folder+objectFiles[i].toStdString();
+		printf("object_file: %s\n",object_file.c_str());
         if (verbose)
         {
             cout<<"Now parsing object "<<object_file<<endl;
diff --git a/object_manager_msgs/CMakeLists.txt b/object_manager_msgs/CMakeLists.txt
new file mode 100644
index 00000000..f2cec13c
--- /dev/null
+++ b/object_manager_msgs/CMakeLists.txt
@@ -0,0 +1,183 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(object_manager_msgs)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs geometry_msgs message_generation)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+add_service_files(
+   FILES
+   DynamicObjectsService.srv
+   GetDynamicObjectService.srv
+   DynamicObjectComputeMaskService.srv
+   ProcessDynamicObjectService.srv
+)
+
+add_message_files(
+  FILES
+  DynamicObjectTracks.msg
+  DynamicObjectTrackingData.msg
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs  # Or other packages containing msgs
+  sensor_msgs
+  geometry_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES object_manager_msgs
+  CATKIN_DEPENDS message_runtime
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+
+## Declare a C++ library
+# add_library(quasimodo_msgs
+#   src/${PROJECT_NAME}/quasimodo_msgs.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(quasimodo_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(quasimodo_msgs_node src/quasimodo_msgs_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(quasimodo_msgs_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(quasimodo_msgs_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS quasimodo_msgs quasimodo_msgs_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_quasimodo_msgs.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/object_manager/msg/DynamicObjectTrackingData.msg b/object_manager_msgs/msg/DynamicObjectTrackingData.msg
similarity index 100%
rename from object_manager/msg/DynamicObjectTrackingData.msg
rename to object_manager_msgs/msg/DynamicObjectTrackingData.msg
diff --git a/object_manager/msg/DynamicObjectTracks.msg b/object_manager_msgs/msg/DynamicObjectTracks.msg
similarity index 100%
rename from object_manager/msg/DynamicObjectTracks.msg
rename to object_manager_msgs/msg/DynamicObjectTracks.msg
diff --git a/object_manager_msgs/package.xml b/object_manager_msgs/package.xml
new file mode 100644
index 00000000..f8b13cb6
--- /dev/null
+++ b/object_manager_msgs/package.xml
@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+  <name>object_manager_msgs</name>
+  <version>0.0.0</version>
+  <description>The quasimodo_msgs package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="nbore@todo.todo">nbore</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/quasimodo_msgs</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/object_manager/srv/DynamicObjectComputeMaskService.srv b/object_manager_msgs/srv/DynamicObjectComputeMaskService.srv
similarity index 100%
rename from object_manager/srv/DynamicObjectComputeMaskService.srv
rename to object_manager_msgs/srv/DynamicObjectComputeMaskService.srv
diff --git a/object_manager/srv/DynamicObjectsService.srv b/object_manager_msgs/srv/DynamicObjectsService.srv
similarity index 100%
rename from object_manager/srv/DynamicObjectsService.srv
rename to object_manager_msgs/srv/DynamicObjectsService.srv
diff --git a/object_manager/srv/GetDynamicObjectService.srv b/object_manager_msgs/srv/GetDynamicObjectService.srv
similarity index 100%
rename from object_manager/srv/GetDynamicObjectService.srv
rename to object_manager_msgs/srv/GetDynamicObjectService.srv
diff --git a/object_manager/srv/ProcessDynamicObjectService.srv b/object_manager_msgs/srv/ProcessDynamicObjectService.srv
similarity index 100%
rename from object_manager/srv/ProcessDynamicObjectService.srv
rename to object_manager_msgs/srv/ProcessDynamicObjectService.srv
diff --git a/object_view_generator/scripts/view_points_service.py b/object_view_generator/scripts/view_points_service.py
index 53078b5e..3eb286a0 100755
--- a/object_view_generator/scripts/view_points_service.py
+++ b/object_view_generator/scripts/view_points_service.py
@@ -105,7 +105,7 @@ def generate_trajectory_points(self,req):
         try:
             rospy.loginfo("Getting a map...")
             msg = rospy.wait_for_message(self.map_frame, OccupancyGrid ,
-                                         timeout=10.0)
+										 timeout=120.0)
             rospy.loginfo("got map.")
             self.process_map(msg)
         except rospy.ROSException, e:
diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt
index b5d2e4dd..c4096314 100644
--- a/quasimodo/quasimodo_brain/CMakeLists.txt
+++ b/quasimodo/quasimodo_brain/CMakeLists.txt
@@ -1,7 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(quasimodo_brain)
 
-find_package(catkin REQUIRED COMPONENTS roscpp soma2_msgs soma_manager quasimodo_msgs quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge)
+
+find_package(catkin REQUIRED COMPONENTS quasimodo_models roscpp soma_msgs object_manager object_manager_msgs semantic_map_msgs soma_manager local_metric_map_people_rejection quasimodo_msgs quasimodo_conversions quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge)
+
 
 set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fPIC -std=c++0x -o popcnt -mssse3")
 
@@ -17,30 +19,8 @@ include_directories(${catkin_INCLUDE_DIRS})
 # This is the root dir of https://github.com/nilsbore/dynamic_object_retrieval
 set(retrieval_project_dir ${CMAKE_CURRENT_SOURCE_DIR}/../../dynamic_object_retrieval/)
 
-
-# This is the base vocabulary tree representation
-#set(k_means_tree_dir ${retrieval_project_dir}/k_means_tree)
-#set(k_means_tree_library_dir ${k_means_tree_dir}/build)
-#include_directories(${k_means_tree_dir}/include ${k_means_tree_dir}/impl)
-#link_directories(${k_means_tree_library_dir})
-
-# This is the stopwatch by Whelan, also included in the base dir
-#set(stopwatch_dir ${retrieval_dir}/stopwatch)
-#set(stopwatch_include_dir ${stopwatch_dir}/src)
-#include_directories(${stopwatch_include_dir})
-
 find_package(OpenGL)
 
-# The benchmark project is also in the base dir
-#set(benchmark_dir ${retrieval_project_dir}/benchmark)
-#set(benchmark_libraries benchmark_result benchmark_retrieval benchmark_visualization benchmark_overlap surfel_renderer ${OPENGL_gl_LIBRARY} ${QT_QTOPENGL_LIBRARY})
-#include_directories(${benchmark_dir}/include)
-#link_directories(${benchmark_dir}/build)
-
-# Define the locations of cereal serialization header files
-#set(cereal_include_dir ${k_means_tree_dir}/cereal/include)
-#include_directories(${cereal_include_dir})
-
 ##############################################
 ##################### INCLUDES ###############
 ##############################################
@@ -85,8 +65,29 @@ catkin_package(
 
 include_directories(${catkin_INCLUDE_DIRS})
 
-add_library(quasimodo_ModelDatabase src/ModelDatabase/ModelDatabase.cpp src/ModelDatabase/ModelDatabaseBasic.cpp  src/ModelDatabase/ModelDatabaseRGBHistogram.cpp)# src/ModelDatabase/ModelDatabaseRetrieval.cpp)
+
+add_library(quasimodo_brain_util src/Util/Util.cpp)
+target_link_libraries(quasimodo_brain_util
+					  #${retrieval_libraries}
+					  #${benchmark_libraries}
+					  ${OpenCV_LIBS}
+					  ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES}
+					  ${PCL_LIBRARIES}
+					  ${catkin_LIBRARIES})
+
+add_library(quasimodo_ModelStorage src/ModelStorage/ModelStorage.cpp src/ModelStorage/ModelStorageFile.cpp)
+target_link_libraries(quasimodo_ModelStorage
+						quasimodo_brain_util
+					  #${retrieval_libraries}
+					  #${benchmark_libraries}
+					  ${OpenCV_LIBS}
+					  ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES}
+					  ${PCL_LIBRARIES}
+					  ${catkin_LIBRARIES})
+
+add_library(quasimodo_ModelDatabase src/ModelDatabase/ModelDatabase.cpp src/ModelDatabase/ModelDatabaseBasic.cpp  src/ModelDatabase/ModelDatabaseRGBHistogram.cpp src/ModelDatabase/ModelDatabaseRetrieval.cpp )
 target_link_libraries(quasimodo_ModelDatabase
+						quasimodo_ModelStorage quasimodo_brain_util
 					  #${retrieval_libraries}
 					  #${benchmark_libraries}
                       ${OpenCV_LIBS}
@@ -99,20 +100,25 @@ target_link_libraries(	preload_object_data ${catkin_LIBRARIES} ${PCL_LIBRARIES}
 add_dependencies(		preload_object_data roscpp quasimodo_msgs_generate_messages_cpp)
 target_link_libraries(	preload_object_data  image_geometry cpp_common roscpp rosconsole tf_conversions ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
 
-add_executable(		robot_listener src/robot_listener.cpp)
+add_executable(			robot_listener src/robot_listener.cpp)
 target_link_libraries(	robot_listener ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${QT_LIBRARIES} metaroom_xml_parser)
-add_dependencies(	robot_listener roscpp quasimodo_msgs_generate_messages_cpp)
+add_dependencies(		robot_listener roscpp quasimodo_msgs_generate_messages_cpp)
 target_link_libraries(	robot_listener  image_geometry cpp_common roscpp rosconsole tf_conversions ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
 
 add_executable(			modelserver src/modelserver.cpp)
 add_dependencies(		modelserver roscpp quasimodo_msgs_generate_messages_cpp)
-target_link_libraries(	modelserver quasimodo_ModelDatabase  image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+target_link_libraries(	modelserver quasimodo_superpoint quasimodo_brain_util quasimodo_ModelDatabase  image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
 
 
 add_executable(			massregPCD src/massregPCD.cpp)
 add_dependencies(		massregPCD roscpp quasimodo_msgs_generate_messages_cpp)
 target_link_libraries(	massregPCD quasimodo_ModelDatabase quasimodo_ModelUpdater image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
 
+add_executable(			voPCD src/voPCD.cpp)
+add_dependencies(		voPCD roscpp quasimodo_msgs_generate_messages_cpp)
+target_link_libraries(	voPCD quasimodo_brain_util quasimodo_ModelDatabase quasimodo_ModelUpdater image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+
 add_executable(			velodyne2 src/velodyne2.cpp)
 add_dependencies(		velodyne2 roscpp quasimodo_msgs_generate_messages_cpp)
 target_link_libraries(	velodyne2 quasimodo_ModelDatabase quasimodo_ModelUpdater image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
@@ -121,6 +127,43 @@ add_executable(			MeshTest src/MeshTest.cpp)
 add_dependencies(		MeshTest roscpp quasimodo_msgs_generate_messages_cpp)
 target_link_libraries(	MeshTest quasimodo_ModelDatabase quasimodo_Mesher  image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
 
+add_executable(			test_align2 src/test_align2.cpp)
+add_dependencies(		test_align2 roscpp quasimodo_msgs_generate_messages_cpp)
+target_link_libraries(	test_align2 quasimodo_ModelDatabase quasimodo_Mesher  image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+add_executable(			test_segment src/test_segment.cpp)
+add_dependencies(		test_segment roscpp quasimodo_msgs_generate_messages_cpp)
+target_link_libraries(	test_segment quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher  image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+add_executable(			segment_room_pairs src/segment_room_pairs.cpp)
+add_dependencies(		segment_room_pairs roscpp quasimodo_msgs_generate_messages_cpp)
+target_link_libraries(	segment_room_pairs quasimodo_ModelUpdater quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher  image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+add_executable(			test_segment_room_pairs src/test_segment_room_pairs.cpp)
+add_dependencies(		test_segment_room_pairs roscpp quasimodo_msgs_generate_messages_cpp)
+target_link_libraries(	test_segment_room_pairs quasimodo_ModelUpdater quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher  image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+add_executable(			segmentationserver src/segmentationserver.cpp)
+add_dependencies(		segmentationserver roscpp quasimodo_msgs_generate_messages_cpp)
+target_link_libraries(	segmentationserver quasimodo_brain_util quasimodo_ModelUpdater quasimodo_ModelDatabase quasimodo_Mesher  image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+add_executable(			metaroom_additional_view_processing src/metaroom_additional_view_processing.cpp)
+add_dependencies(		metaroom_additional_view_processing roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp)
+target_link_libraries(	metaroom_additional_view_processing quasimodo_superpoint quasimodo_ModelUpdater quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+add_executable(			AnnotationTool src/AnnotationTool.cpp)
+add_dependencies(		AnnotationTool roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp)
+target_link_libraries(	AnnotationTool quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+add_executable(			pcl_multivisualizer src/pcl_multivisualizer.cpp)
+add_dependencies(		pcl_multivisualizer roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp)
+target_link_libraries(	pcl_multivisualizer quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+add_executable(			zoomer src/zoomer.cpp)
+add_dependencies(		zoomer roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp)
+target_link_libraries(	zoomer quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+
 #############
 ## Install ##
 #############
@@ -130,7 +173,7 @@ target_link_libraries(	MeshTest quasimodo_ModelDatabase quasimodo_Mesher  image_
 
 # Mark executables and/or libraries for installation
 
-install(TARGETS quasimodo_ModelDatabase preload_object_data modelserver robot_listener
+install(TARGETS zoomer pcl_multivisualizer quasimodo_brain_util quasimodo_ModelDatabase preload_object_data modelserver robot_listener massregPCD voPCD velodyne2 MeshTest test_align2 test_segment segment_room_pairs test_segment_room_pairs segmentationserver metaroom_additional_view_processing AnnotationTool
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/quasimodo/quasimodo_brain/launch/modelserver.launch b/quasimodo/quasimodo_brain/launch/modelserver.launch
index 23cf19d1..514bd62d 100644
--- a/quasimodo/quasimodo_brain/launch/modelserver.launch
+++ b/quasimodo/quasimodo_brain/launch/modelserver.launch
@@ -1,8 +1,3 @@
 <launch>
-    <arg name="occlusion_penalty" default="5"/>
-    <arg name="massreg_timeout" default="60"/>
-    <arg name="modelpath" default="./"/>
-    <arg name="flags" default=""/>
-    <node pkg="quasimodo_brain" type="modelserver" name="modelserver" output="screen" respawn="false"  args="-occlusion_penalty $(arg occlusion_penalty) -massreg_timeout $(arg massreg_timeout) -p $(arg modelpath) $(arg flags)">
-    </node>
+    <node pkg="quasimodo_brain" type="modelserver" name="modelserver" output="screen" respawn="true"/>
 </launch>
diff --git a/quasimodo/quasimodo_brain/launch/segmentation.launch b/quasimodo/quasimodo_brain/launch/segmentation.launch
new file mode 100644
index 00000000..a7584fef
--- /dev/null
+++ b/quasimodo/quasimodo_brain/launch/segmentation.launch
@@ -0,0 +1,4 @@
+<launch>
+    <arg name="data_path" default="/home/strands/.semanticMap"/>
+    <node pkg="quasimodo_brain" type="metaroom_additional_view_processing" name="metaroom_additional_view_processing" output="screen" respawn="true" args="-folder $(arg data_path)"/>
+</launch>
diff --git a/quasimodo/quasimodo_brain/launch/segmentationserver.launch b/quasimodo/quasimodo_brain/launch/segmentationserver.launch
new file mode 100644
index 00000000..735fb51c
--- /dev/null
+++ b/quasimodo/quasimodo_brain/launch/segmentationserver.launch
@@ -0,0 +1,7 @@
+<launch>
+	<node pkg="quasimodo_brain" type="segmentationserver" name="segmentationserver" output="screen" respawn="false"  args="">
+    </node>
+
+	<node pkg="quasimodo_brain" type="segment_room_pairs" name="segment_room_pairs" output="screen" respawn="false"  args="">
+	</node>
+</launch>
diff --git a/quasimodo/quasimodo_brain/package.xml b/quasimodo/quasimodo_brain/package.xml
index 0f0281f1..6d689239 100644
--- a/quasimodo/quasimodo_brain/package.xml
+++ b/quasimodo/quasimodo_brain/package.xml
@@ -51,11 +51,21 @@
   <build_depend>libqt4-dev</build_depend>
   <build_depend>libceres-dev</build_depend>
   <build_depend>roscpp</build_depend>
-  <build_depend>soma2_msgs</build_depend>
+  <build_depend>soma_msgs</build_depend>
   <build_depend>soma_manager</build_depend>
   <build_depend>message_runtime</build_depend>
   <build_depend>eigen_conversions</build_depend>
   <build_depend>cv_bridge</build_depend>
+  <build_depend>semantic_map_msgs</build_depend>
+  <build_depend>object_manager_msgs</build_depend>
+  <build_depend>object_manager</build_depend>
+  
+  <build_depend>local_metric_map_people_rejection</build_depend>
+  <run_depend>local_metric_map_people_rejection</run_depend>
+  
+  <run_depend>object_manager</run_depend>
+  <run_depend>object_manager_msgs</run_depend>
+  <run_depend>semantic_map_msgs</run_depend>
   <run_depend>quasimodo_models</run_depend>
   <run_depend>quasimodo_msgs</run_depend>
   <run_depend>metaroom_xml_parser</run_depend>
@@ -66,7 +76,7 @@
   <run_depend>libqt4</run_depend>
   <run_depend>libceres-dev</run_depend>
   <run_depend>roscpp</run_depend>
-  <run_depend>soma2_msgs</run_depend>
+  <run_depend>soma_msgs</run_depend>
   <run_depend>soma_manager</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>eigen_conversions</run_depend>
diff --git a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp
new file mode 100644
index 00000000..5df94ba7
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp
@@ -0,0 +1,803 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+#include "metaroom_xml_parser/simple_summary_parser.h"
+
+#include <tf_conversions/tf_eigen.h>
+
+#include "ros/ros.h"
+#include <metaroom_xml_parser/load_utilities.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "metaroom_xml_parser/load_utilities.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include <tf_conversions/tf_eigen.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+#include "quasimodo_msgs/segment_model.h"
+#include "quasimodo_msgs/metaroom_pair.h"
+
+#include "ros/ros.h"
+#include <quasimodo_msgs/query_cloud.h>
+#include <quasimodo_msgs/visualize_query.h>
+#include <metaroom_xml_parser/load_utilities.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "metaroom_xml_parser/load_utilities.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+#include "metaroom_xml_parser/load_utilities.h"
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+#include "Util/Util.h"
+
+// Services
+#include <object_manager_msgs/DynamicObjectsService.h>
+#include <object_manager_msgs/GetDynamicObjectService.h>
+#include <object_manager_msgs/ProcessDynamicObjectService.h>
+
+// Registration service
+#include <observation_registration_services/ObjectAdditionalViewRegistrationService.h>
+
+// Additional view mask service
+#include <object_manager_msgs/DynamicObjectComputeMaskService.h>
+
+
+#include <sys/time.h>
+#include <sys/resource.h>
+
+// Custom messages
+#include "object_manager/dynamic_object.h"
+#include "object_manager/dynamic_object_xml_parser.h"
+#include "object_manager/dynamic_object_utilities.h"
+#include "object_manager/dynamic_object_mongodb_interface.h"
+
+#include <object_manager_msgs/DynamicObjectTracks.h>
+#include <object_manager_msgs/DynamicObjectTrackingData.h>
+
+
+#include <semantic_map_msgs/RoomObservation.h>
+
+#include <iostream>
+#include <fstream>
+
+
+std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers;
+std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers;
+
+typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest;
+typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse;
+
+typedef typename object_manager_msgs::GetDynamicObjectService::Request GetDynamicObjectServiceRequest;
+typedef typename object_manager_msgs::GetDynamicObjectService::Response GetDynamicObjectServiceResponse;
+
+typedef typename object_manager_msgs::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest;
+typedef typename object_manager_msgs::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse;
+
+ros::ServiceClient segmentation_client;
+using namespace std;
+using namespace semantic_map_load_utilties;
+
+typedef pcl::PointXYZRGB PointType;
+typedef pcl::PointCloud<PointType> Cloud;
+typedef typename Cloud::Ptr CloudPtr;
+typedef pcl::search::KdTree<PointType> Tree;
+typedef semantic_map_load_utilties::DynamicObjectData<PointType> ObjectData;
+
+using namespace std;
+using namespace semantic_map_load_utilties;
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+int visualization_lvl		= 0;
+
+Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){
+	QXmlStreamReader::TokenType token = xmlReader->readNext();//Translation
+	QString elementName = xmlReader->name().toString();
+
+	token = xmlReader->readNext();//fx
+	double tx = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//fy
+	double ty = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//cx
+	double tz = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//Translation
+	elementName = xmlReader->name().toString();
+
+	token = xmlReader->readNext();//Rotation
+	elementName = xmlReader->name().toString();
+
+	token = xmlReader->readNext();//qw
+	double qw = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//qx
+	double qx = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//qy
+	double qy = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//qz
+	double qz = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//Rotation
+	elementName = xmlReader->name().toString();
+
+	Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix();
+	regpose(0,3) = tx;
+	regpose(1,3) = ty;
+	regpose(2,3) = tz;
+
+	return regpose;
+}
+
+void readViewXML(std::string xmlFile, std::vector<reglib::RGBDFrame *> & frames, std::vector<Eigen::Matrix4d> & poses){
+	QFile file(xmlFile.c_str());
+
+	if (!file.exists())
+	{
+		ROS_ERROR("Could not open file %s to load room.",xmlFile.c_str());
+		return;
+	}
+
+	QString xmlFileQS(xmlFile.c_str());
+	int index = xmlFileQS.lastIndexOf('/');
+	std::string roomFolder = xmlFileQS.left(index).toStdString();
+
+
+	file.open(QIODevice::ReadOnly);
+	ROS_INFO_STREAM("Parsing xml file: "<<xmlFile.c_str());
+
+	QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+
+
+	while (!xmlReader->atEnd() && !xmlReader->hasError())
+	{
+		QXmlStreamReader::TokenType token = xmlReader->readNext();
+		if (token == QXmlStreamReader::StartDocument)
+			continue;
+
+		if (xmlReader->hasError())
+		{
+			ROS_ERROR("XML error: %s in %s",xmlReader->errorString().toStdString().c_str(),xmlFile.c_str());
+			return;
+		}
+
+		QString elementName = xmlReader->name().toString();
+
+		if (token == QXmlStreamReader::StartElement)
+		{
+
+			if (xmlReader->name() == "View")
+			{
+				cv::Mat rgb;
+				cv::Mat depth;
+				//printf("elementName: %s\n",elementName.toStdString().c_str());
+				QXmlStreamAttributes attributes = xmlReader->attributes();
+				if (attributes.hasAttribute("RGB"))
+				{
+					std::string imgpath = attributes.value("RGB").toString().toStdString();
+					printf("rgb filename: %s\n",(roomFolder+"/"+imgpath).c_str());
+					rgb = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED);
+
+					//QString rgbpath = attributes.value("RGB").toString();
+					//rgb = cv::imread(roomFolder+"/"+(rgbpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED);
+					//rgb = cv::imread((rgbpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED);
+				}else{break;}
+
+
+				if (attributes.hasAttribute("DEPTH"))
+				{
+					std::string imgpath = attributes.value("DEPTH").toString().toStdString();
+					printf("depth filename: %s\n",(roomFolder+"/"+imgpath).c_str());
+					depth = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED);
+					//QString depthpath = attributes.value("DEPTH").toString();
+					//printf("depth filename: %s\n",depthpath.toStdString().c_str());
+					//depth = cv::imread((roomFolder+"/"+depthpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED);
+					//depth = cv::imread(roomFolder+"/"+(depthpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED);
+				}else{break;}
+
+
+				token = xmlReader->readNext();//Stamp
+				elementName = xmlReader->name().toString();
+
+				token = xmlReader->readNext();//sec
+				elementName = xmlReader->name().toString();
+				int sec = atoi(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//nsec
+				elementName = xmlReader->name().toString();
+				int nsec = atoi(xmlReader->readElementText().toStdString().c_str());
+				token = xmlReader->readNext();//end stamp
+
+				token = xmlReader->readNext();//Camera
+				elementName = xmlReader->name().toString();
+
+				reglib::Camera * cam = new reglib::Camera();
+
+				token = xmlReader->readNext();//fx
+				cam->fx = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//fy
+				cam->fy = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//cx
+				cam->cx = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//cy
+				cam->cy = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//Camera
+				elementName = xmlReader->name().toString();
+
+				double time = double(sec)+double(nsec)/double(1e9);
+
+				token = xmlReader->readNext();//RegisteredPose
+				elementName = xmlReader->name().toString();
+
+				Eigen::Matrix4d regpose = getPose(xmlReader);
+
+				token = xmlReader->readNext();//RegisteredPose
+				elementName = xmlReader->name().toString();
+
+
+				token = xmlReader->readNext();//Pose
+				elementName = xmlReader->name().toString();
+
+				Eigen::Matrix4d pose = getPose(xmlReader);
+
+				token = xmlReader->readNext();//Pose
+				elementName = xmlReader->name().toString();
+
+				reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, pose);
+				frames.push_back(frame);
+				poses.push_back(pose);
+			}
+		}
+	}
+	delete xmlReader;
+}
+
+std::vector<Eigen::Matrix4d> readPoseXML(std::string xmlFile){
+	std::vector<Eigen::Matrix4d> poses;
+	QFile file(xmlFile.c_str());
+
+	if (!file.exists()){
+		ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str());
+		return poses;
+	}
+
+	file.open(QIODevice::ReadOnly);
+	ROS_INFO_STREAM("Parsing xml file: "<<xmlFile.c_str());
+
+	QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+
+	while (!xmlReader->atEnd() && !xmlReader->hasError()){
+		QXmlStreamReader::TokenType token = xmlReader->readNext();
+		if (token == QXmlStreamReader::StartDocument)
+			continue;
+
+		if (xmlReader->hasError())
+		{
+			ROS_ERROR("XML error: %s in %s",xmlReader->errorString().toStdString().c_str(),xmlFile.c_str());
+			return poses;
+		}
+
+		QString elementName = xmlReader->name().toString();
+
+		if (token == QXmlStreamReader::StartElement){
+			if (xmlReader->name() == "Pose"){
+				Eigen::Matrix4d pose = getPose(xmlReader);
+
+				token = xmlReader->readNext();//Pose
+				elementName = xmlReader->name().toString();
+
+				std::cout << pose << std::endl << std::endl;
+				poses.push_back(pose);
+			}
+		}
+	}
+	delete xmlReader;
+	printf("done readPoseXML\n");
+	return poses;
+}
+
+std::vector<reglib::Model *> loadModels(std::string path){
+	printf("loadModels: %s\n",path.c_str());
+	std::vector<reglib::Model *> models;
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+	QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml"));
+	if(objectFiles.size() == 0){return models;}
+
+	std::vector<reglib::RGBDFrame *> frames;
+	std::vector<Eigen::Matrix4d> poses;
+	readViewXML(sweep_folder+"ViewGroup.xml",frames,poses);
+
+
+	for (auto objectFile : objectFiles){
+		std::string object = sweep_folder+objectFile.toStdString();
+		printf("object: %s\n",object.c_str());
+
+		QFile file(object.c_str());
+
+		if (!file.exists()){
+			ROS_ERROR("Could not open file %s to masks.",object.c_str());
+			continue;
+		}
+
+		file.open(QIODevice::ReadOnly);
+		ROS_INFO_STREAM("Parsing xml file: "<<object.c_str());
+
+		reglib::Model * mod = new reglib::Model();
+		QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+
+		while (!xmlReader->atEnd() && !xmlReader->hasError()){
+			QXmlStreamReader::TokenType token = xmlReader->readNext();
+			if (token == QXmlStreamReader::StartDocument)
+				continue;
+
+			if (xmlReader->hasError()){
+				ROS_ERROR("XML error: %s in %s",xmlReader->errorString().toStdString().c_str(),object.c_str());
+				break;
+			}
+
+			QString elementName = xmlReader->name().toString();
+
+			if (token == QXmlStreamReader::StartElement){
+				if (xmlReader->name() == "Mask"){
+					int number = 0;
+					cv::Mat mask;
+					QXmlStreamAttributes attributes = xmlReader->attributes();
+					if (attributes.hasAttribute("filename")){
+						QString maskpath = attributes.value("filename").toString();
+						//printf("mask filename: %s\n",(sweep_folder+maskpath.toStdString()).c_str());
+						mask = cv::imread(sweep_folder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED);
+						//mask = cv::imread((maskpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED);
+					}else{break;}
+
+
+					if (attributes.hasAttribute("image_number")){
+						QString depthpath = attributes.value("image_number").toString();
+						number = atoi(depthpath.toStdString().c_str());
+						printf("number: %i\n",number);
+					}else{break;}
+
+					mod->frames.push_back(frames[number]->clone());
+					mod->relativeposes.push_back(poses[number]);
+					mod->modelmasks.push_back(new reglib::ModelMask(mask));
+				}
+			}
+		}
+
+		delete xmlReader;
+		models.push_back(mod);
+	}
+
+	for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];}
+
+	return models;
+}
+
+bool annotate(std::string path){
+	printf("annotate: %s\n",path.c_str());
+
+	std::string xmlFile = path;
+	std::vector< std::string > rgbpaths;
+
+
+	QString xmlFileQS(xmlFile.c_str());
+	int index = xmlFileQS.lastIndexOf('/');
+	std::string roomFolder = xmlFileQS.left(index).toStdString();
+
+
+
+	QFile file((roomFolder+"/ViewGroup.xml").c_str());
+	if (!file.exists()){
+		ROS_ERROR("Could not open file %s to load room.",(roomFolder+"/ViewGroup.xml").c_str());
+		return false;
+	}
+
+	file.open(QIODevice::ReadOnly);
+
+	ROS_INFO_STREAM("Parsing xml file: "<<(roomFolder+"/ViewGroup.xml").c_str());
+
+	QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+
+	while (!xmlReader->atEnd() && !xmlReader->hasError()){
+		QXmlStreamReader::TokenType token = xmlReader->readNext();
+		if (token == QXmlStreamReader::StartDocument)
+			continue;
+
+		if (xmlReader->hasError()){
+			ROS_ERROR("XML error: %s in %s",xmlReader->errorString().toStdString().c_str(), (roomFolder+"/ViewGroup.xml").c_str());
+			return false;
+		}
+
+		QString elementName = xmlReader->name().toString();
+
+		if (token == QXmlStreamReader::StartElement){
+			if (xmlReader->name() == "View"){
+				QXmlStreamAttributes attributes = xmlReader->attributes();
+				if (attributes.hasAttribute("RGB")){
+					rgbpaths.push_back(roomFolder+"/"+attributes.value("RGB").toString().toStdString());
+					printf("%s\n",rgbpaths.back().c_str());
+				}else{break;}
+			}
+		}
+	}
+
+	delete xmlReader;
+
+	printf("rgbpaths: %i\n",rgbpaths.size());
+
+	QStringList objectFiles = QDir(roomFolder.c_str()).entryList(QStringList("dynamic_obj*.xml"));
+	if(objectFiles.size() == 0){return true;}
+
+
+
+	for (auto objectFile : objectFiles){
+		std::string object = roomFolder+"/"+objectFile.toStdString();
+		printf("object: %s\n",object.c_str());
+
+		std::vector<cv::Mat > objectMasks;
+		std::vector<unsigned int > imgNumber;
+		unsigned int current_displayInd = 0;
+		unsigned int maxNrPixels = 0;
+
+		std::string classname = "";
+		std::string instancename = "";
+		std::string tags = "";
+
+
+		QFile objfile(object.c_str());
+
+		if (!objfile.exists()){
+			ROS_ERROR("Could not open file %s to masks.",object.c_str());
+			continue;
+		}
+
+		objfile.open(QIODevice::ReadOnly);
+		ROS_INFO_STREAM("Parsing xml file: "<<object.c_str());
+
+		QXmlStreamReader* objxmlReader = new QXmlStreamReader(&objfile);
+
+		while (!objxmlReader->atEnd() && !objxmlReader->hasError()){
+			QXmlStreamReader::TokenType token = objxmlReader->readNext();
+			if (token == QXmlStreamReader::StartDocument)
+				continue;
+
+			if (objxmlReader->hasError()){
+				ROS_ERROR("XML error: %s in %s",objxmlReader->errorString().toStdString().c_str(),object.c_str());
+				break;
+			}
+
+			QString elementName = objxmlReader->name().toString();
+
+			if (token == QXmlStreamReader::StartElement){
+				if (objxmlReader->name() == "Object"){
+					QXmlStreamAttributes attributes = objxmlReader->attributes();
+					if (attributes.hasAttribute("classname")){		classname		= attributes.value("classname").toString().toStdString();}
+					if (attributes.hasAttribute("instancename")){	instancename	= attributes.value("instancename").toString().toStdString();}
+					if (attributes.hasAttribute("tags")){			tags			= attributes.value("tags").toString().toStdString();}
+				}
+
+				if (objxmlReader->name() == "Mask"){
+					int number = 0;
+					QXmlStreamAttributes attributes = objxmlReader->attributes();
+					if (attributes.hasAttribute("filename")){
+						QString maskpath = attributes.value("filename").toString();
+						objectMasks.push_back(cv::imread(roomFolder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED));
+						unsigned char * data = objectMasks.back().data;
+						unsigned int nr_points = objectMasks.back().rows * objectMasks.back().cols;
+						unsigned int nrPixels = 0;
+						for(unsigned int ind = 0; ind < nr_points; ind++ ){nrPixels += data[ind] != 0;}
+
+						if( maxNrPixels < nrPixels ){
+							maxNrPixels = nrPixels;
+							current_displayInd = objectMasks.size()-1;
+						}
+					}else{break;}
+
+
+					if (attributes.hasAttribute("image_number")){
+						QString depthpath = attributes.value("image_number").toString();
+						number = atoi(depthpath.toStdString().c_str());
+						printf("number: %i\n",number);
+						imgNumber.push_back(number);
+						if(imgNumber.back() >= rgbpaths.size()){return false;}
+					}else{break;}
+				}
+			}
+		}
+
+		if(objectMasks.size() != imgNumber.size()){return false;}
+
+
+		int fontFace = cv::FONT_HERSHEY_COMPLEX_SMALL;
+		double fontScale = 1;
+		int thickness = 1;
+		int state = 0;
+
+		cv::Mat rgb = cv::imread(rgbpaths[imgNumber[current_displayInd]], CV_LOAD_IMAGE_UNCHANGED);
+		cv::Mat mask = objectMasks[current_displayInd];
+		while(true){
+
+
+			std::vector<std::vector<cv::Point> > contours;
+			std::vector<cv::Vec4i> hierarchy;
+
+			cv::findContours( mask, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
+			for( unsigned int i = 0; i < contours.size(); i++ ){
+				cv::drawContours( rgb, contours, i, cv::Scalar( 0, 0, 255 ), 3, 8, hierarchy, 0, cv::Point() );
+				cv::drawContours( rgb, contours, i, cv::Scalar( 0, 255, 0 ), 1, 8, hierarchy, 0, cv::Point() );
+			}
+
+
+			unsigned int height	= rgb.rows;
+			unsigned int width	= rgb.cols;
+			unsigned int newwidth = width+600;
+
+
+			unsigned char * rgbdata = rgb.data;
+
+			cv::Mat img;
+			img.create(height,newwidth,CV_8UC3);
+			unsigned char * imgdata = img.data;
+
+			for(unsigned int i = 0; i < 3*height*newwidth; i++){imgdata[i] = 0;}
+
+			for(unsigned int w = 0; w < width;w++){
+				for(unsigned int h = 0; h < height;h++){
+					int oind = h*width+w;
+					int nind = h*newwidth+w;
+					imgdata[3*nind + 0] = rgbdata[3*oind + 0];
+					imgdata[3*nind + 1] = rgbdata[3*oind + 1];
+					imgdata[3*nind + 2] = rgbdata[3*oind + 2];
+				}
+			}
+
+			int textnr = 0;
+			putText(img, "   Class:    "+classname,		cv::Point(width+10,	30+(textnr++)*25   ),	fontFace, fontScale, cv::Scalar::all(255), thickness, 8);
+			putText(img, "   Instance: "+instancename,	cv::Point(width+10,	30+(textnr++)*25   ),	fontFace, fontScale, cv::Scalar::all(255), thickness, 8);
+			putText(img, "   Tags:     "+tags,			cv::Point(width+10,	30+(textnr++)*25   ),	fontFace, fontScale, cv::Scalar::all(255), thickness, 8);
+
+			char buf [1024];
+			sprintf(buf,"   Image:     %i / %i",current_displayInd+1,objectMasks.size());
+			putText(img, std::string(buf), cv::Point(width+10,			30+(textnr++)*25   ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8);
+
+			putText(img, "   Select state(pushed CTRL)",cv::Point(width+10,	30+(textnr++)*25   ),	fontFace, fontScale, cv::Scalar::all(255), thickness, 8);
+			putText(img, "--", cv::Point(width+5,	30+state*25   ), fontFace, fontScale, cv::Scalar(0,255,0), thickness, 8);
+
+			cv::namedWindow( "Annotation tool",				cv::WINDOW_AUTOSIZE );	cv::imshow( "Annotation tool",img );
+			char c = cv::waitKey(0);
+printf("%i\n",int(c));
+			if(c == 27){break;}
+			else if(c == -29){
+				state = 4;
+			}else if(state == 0){
+				if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z')  || (c >= '0' && c <= '9') || c == ' '){classname += c;}
+				if(c == 8 && classname.size() > 0){classname.pop_back();}
+				if(c == 10){state = 1;}
+			}else if(state == 1){
+				if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z')  || (c >= '0' && c <= '9') || c == ' '){instancename += c;}
+				if(c == 8 && instancename.size() > 0){instancename.pop_back();}
+				if(c == 10){state = 2;}
+			}else if(state == 2){
+				if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z')  || (c >= '0' && c <= '9') || c == ' '){tags += c;}
+				if(c == 8 && tags.size() > 0){tags.pop_back();}
+				if(c == 10){break;}
+			}else if(state == 3){
+				if(c == 'q' || c == 'Q'){
+					current_displayInd = std::max(int(current_displayInd-1),0);
+					rgb = cv::imread(rgbpaths[imgNumber[current_displayInd]], CV_LOAD_IMAGE_UNCHANGED);
+					mask = objectMasks[current_displayInd];
+				}
+				if(c == 'w' || c == 'W'){
+					current_displayInd = std::min(int(current_displayInd+1),int(imgNumber.size()-1));
+					rgb = cv::imread(rgbpaths[imgNumber[current_displayInd]], CV_LOAD_IMAGE_UNCHANGED);
+					mask = objectMasks[current_displayInd];
+				}
+				if(c == 10){state = 0;}
+			}else if(state == 4){
+				if(c == '1'){state = 0;}
+				if(c == '2'){state = 1;}
+				if(c == '3'){state = 2;}
+				if(c == '4'){state = 3;}
+				if(c == 'j' || c == 'q'){
+					classname = "segmentation junk";
+					instancename = "segmentation junk";
+					break;
+				}
+				if(c == 10){ state = 0;}
+			}
+		}
+		delete objxmlReader;
+
+
+
+		std::streampos size;
+		char * memblock;
+
+		std::ifstream file (object, ios::in|ios::binary|ios::ate);
+		if (file.is_open()){
+			size = file.tellg();
+			memblock = new char [size+1];
+			file.seekg (0, ios::beg);
+			file.read (memblock, size);
+			file.close();
+			memblock[size] = 0;
+
+			std::string filedata = std::string(memblock);
+
+			std::size_t found1 = filedata.find("<Object");
+			std::size_t found1a = filedata.find("\"",found1+1);
+			std::size_t found1b = filedata.find("\"",found1a+1);
+
+
+
+			std::size_t found2 = filedata.find(">",found1b+1);
+			printf("found: %i\n",found1);
+			printf("found2: %i\n",found2);
+
+			filedata.replace(found1b+1,found2-found1b," classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\">");
+
+			std::ofstream myfile;
+			myfile.open (object);
+			myfile << filedata;
+			myfile.close();
+
+			std::cout << "the entire file content is in memory" << std::endl;
+			std::cout << filedata << std::endl << std::endl;
+
+/*
+			std::string frontpart = filedata.substr(0,found1b+1);
+
+			std::string endpart = filedata.substr(found2,filedata.size()-found2-1);
+
+//			std::cout << frontpart << std::endl;
+//			std::cout << endpart << std::endl;
+
+//			if (attributes.hasAttribute("classname")){		classname		= attributes.value("classname").toString().toStdString();}
+//			if (attributes.hasAttribute("instancename")){	instancename	= attributes.value("instancename").toString().toStdString();}
+//			if (attributes.hasAttribute("tags")){			tags			= attributes.value("tags").toString().toStdString();}
+
+
+			std::string total = frontpart+" classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\""+endpart;
+			std::string total2 = filedata;
+			total2.replace(found1b+1,found2-found1b," classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\">");//std::string total2 = frontpart+" classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\""+endpart;
+
+//			std::ofstream myfile;
+//			myfile.open (object);
+//			myfile << total2;
+//			myfile.close();
+
+			//std::cout << std::endl << std::endl << std::endl << total << std::endl;
+//exit(0);
+//			while (found!=std::string::npos){
+//				path.replace(found,2,"/");
+//				found = path.find("//");
+//			}
+
+			printf("size: %i\n",size);
+			printf("filedata.size(): %i\n",filedata.size());
+
+			std::cout << "the entire file content is in memory" << std::endl;
+			std::cout << filedata << std::endl << std::endl;
+			std::cout << total << std::endl << std::endl;
+			std::cout << total2 << std::endl << std::endl;
+*/
+
+
+			delete[] memblock;
+		}else{
+			std::cout << "Unable to open file\n";
+		}
+//	    return 0;
+	}
+
+/*
+
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+	QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml"));
+
+	if(models.size() == 0 || models.size() != objectFiles.size()){return false;}
+
+	for(unsigned int m = 0; m < models.size(); m++){
+
+		std::string object = sweep_folder+objectFiles[m].toStdString();
+		printf("object: %s\n",object.c_str());
+
+		reglib::Model * model = models[m];
+		unsigned int current_displayInd = 0;
+		unsigned int maxNrPixels = 0;
+		for(unsigned int j = 0; j < model->frames.size(); j++){
+			unsigned int nrPixels = model->modelmasks[j]->testw.size();
+
+		}
+
+
+
+		printf("current_displayInd: %i\n",current_displayInd);
+		model->fullDelete();
+		delete model;
+	}
+*/
+	return true;
+}
+
+bool annotateFiles(std::string path){
+	vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(path);
+	for (auto sweep_xml : sweep_xmls) {
+		annotate(sweep_xml);
+	}
+	return false;
+}
+
+int main(int argc, char** argv){
+	visualization_lvl = 0;
+
+
+	std::vector< std::string > folders;
+	int inputstate = 0;
+	for(int i = 1; i < argc;i++){
+		printf("input: %s\n",argv[i]);
+		if(std::string(argv[i]).compare("-v") == 0){			inputstate	= 1;}
+		else if(std::string(argv[i]).compare("-folder") == 0){	inputstate	= 2;}
+		else if(std::string(argv[i]).compare("-folders") == 0){	inputstate	= 2;}
+		else if(std::string(argv[i]).compare("-file") == 0){	inputstate	= 2;}
+		else if(std::string(argv[i]).compare("-files") == 0){	inputstate	= 2;}
+		else if(inputstate == 1){visualization_lvl = atoi(argv[i]);}
+		else if(inputstate == 2){folders.push_back(std::string(argv[i]));}
+	}
+
+	if(visualization_lvl > 0){
+		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
+		viewer->setBackgroundColor (0.5, 0, 0.5);
+		viewer->addCoordinateSystem (1.0);
+		viewer->initCameraParameters ();
+	}
+
+	if(folders.size() == 0){folders.push_back(std::string(getenv ("HOME"))+"/.semanticMap/");}
+
+	for(unsigned int i = 0; i < folders.size(); i++){annotateFiles(folders[i]);}
+
+
+	printf("done annotating\n");
+
+	return 0;
+}
diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp
index 7aaf71b3..5260f8c9 100644
--- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp
+++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp
@@ -1,16 +1,18 @@
 #include "ModelDatabase.h"
 
-ModelDatabase::ModelDatabase(){}
+ModelDatabase::ModelDatabase(){storage = new ModelStorageFile();}
 ModelDatabase::~ModelDatabase(){}
 
 //Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching
-void ModelDatabase::add(reglib::Model * model){
-
+bool ModelDatabase::add(reglib::Model * model){
+	storage->add(model);
+	return true;
 }
 		
 // return true if successfull
 // return false if fail
 bool ModelDatabase::remove(reglib::Model * model){
+	storage->remove(model);
 	return false;
 }
 		
diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h
index 66621531..e55061ae 100644
--- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h
+++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h
@@ -8,13 +8,17 @@
 #include <pcl/point_types.h>
 
 #include "model/Model.h"
+#include "../Util/Util.h"
+
+#include "../ModelStorage/ModelStorage.h"
 
 class ModelDatabase{
 	public:
 	std::vector<reglib::Model * > models;
+	ModelStorage * storage;
 
 	//Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching
-	virtual void add(reglib::Model * model);
+	virtual bool add(reglib::Model * model);
 		
 	// return true if successfull
 	// return false if fail
@@ -29,5 +33,5 @@ class ModelDatabase{
 
 #include "ModelDatabaseBasic.h"
 #include "ModelDatabaseRGBHistogram.h"
-//#include "ModelDatabaseRetrieval.h"
+#include "ModelDatabaseRetrieval.h"
 #endif // ModelDatabase_H
diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp
index 27a8240f..f39997a5 100644
--- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp
+++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp
@@ -1,15 +1,18 @@
 #include "ModelDatabaseBasic.h"
 
-ModelDatabaseBasic::ModelDatabaseBasic(){}
+ModelDatabaseBasic::ModelDatabaseBasic(){storage = new ModelStorageFile();}
 ModelDatabaseBasic::~ModelDatabaseBasic(){}
 
 
-void ModelDatabaseBasic::add(reglib::Model * model){
+bool ModelDatabaseBasic::add(reglib::Model * model){
+	storage->add(model);
 	models.push_back(model);
+	return true;
 	//printf("number of models: %i\n",models.size());
 }
 
 bool ModelDatabaseBasic::remove(reglib::Model * model){
+	storage->remove(model);
 	for(unsigned int i = 0; i < models.size(); i++){
 		if(models[i] == model){
 			models[i] = models.back();
diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.h b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.h
index 3ba39de8..a63bba6a 100644
--- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.h
+++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.h
@@ -7,7 +7,7 @@
 class ModelDatabaseBasic: public ModelDatabase{
 	public:
 	
-	virtual void add(reglib::Model * model);
+	virtual bool add(reglib::Model * model);
 	virtual bool remove(reglib::Model * model);
 	virtual std::vector<reglib::Model *> search(reglib::Model * model, int number_of_matches);
 		
diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.cpp
index 6d395581..1cce7d32 100644
--- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.cpp
+++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.cpp
@@ -2,6 +2,7 @@
 
 ModelDatabaseRGBHistogram::ModelDatabaseRGBHistogram(int res_){
 	res = res_;
+	storage = new ModelStorageFile();
 	printf("made a ModelDatabaseRGBHistogram(%i)\n",res);
 }
 ModelDatabaseRGBHistogram::~ModelDatabaseRGBHistogram(){}
@@ -26,15 +27,18 @@ std::vector< double > getDescriptor(int res, reglib::Model * model){
 	return descriptor;
 }
 
-void ModelDatabaseRGBHistogram::add(reglib::Model * model){
+bool ModelDatabaseRGBHistogram::add(reglib::Model * model){
+	storage->add(model);
 	//std::vector< std::vector< double > > descriptors;
 	std::vector< double > descriptor = getDescriptor(res,model);
 	descriptors.push_back(descriptor);
 	models.push_back(model);
+	return true;
 	//printf("number of models: %i\n",models.size());
 }
 
 bool ModelDatabaseRGBHistogram::remove(reglib::Model * model){
+	storage->remove(model);
 	for(unsigned int i = 0; i < models.size(); i++){
 		if(models[i] == model){
 			models[i] = models.back();
diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.h b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.h
index 7b20528f..81471b34 100644
--- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.h
+++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.h
@@ -11,7 +11,7 @@ class ModelDatabaseRGBHistogram: public ModelDatabase{
 	std::vector< std::vector< double > > descriptors;
 
 	
-	virtual void add(reglib::Model * model);
+	virtual bool add(reglib::Model * model);
 	virtual bool remove(reglib::Model * model);
 	virtual std::vector<reglib::Model *> search(reglib::Model * model, int number_of_matches);
 		
diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp
index a8104c34..a12d9f86 100644
--- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp
+++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp
@@ -1,143 +1,106 @@
 #include "ModelDatabaseRetrieval.h"
-
-#include <dynamic_object_retrieval/dynamic_retrieval.h>
-#include <dynamic_object_retrieval/visualize.h>
-#include <object_3d_benchmark/benchmark_retrieval.h>
-
-#include <dynamic_object_retrieval/extract_surfel_features.h>
+//#include "mongo/client/dbclient.h"
 
 using namespace std;
 
-using PointT = pcl::PointXYZRGB;
-using CloudT = pcl::PointCloud<PointT>;
-using NormalT = pcl::Normal;
-using NormalCloudT = pcl::PointCloud<NormalT>;
 
-POINT_CLOUD_REGISTER_POINT_STRUCT (HistT,
-                                   (float[250], histogram, histogram)
-)
 
-ModelDatabaseRetrieval::ModelDatabaseRetrieval(std::string vpath) : vt_features()
-{
-    // actually, maybe we should just add some features to the vocabulary so it's not empty, like load an old vocabulary?
-    // good idea
-	boost::filesystem::path vocabulary_path(vpath);
-    dynamic_object_retrieval::load_vocabulary(vt, vocabulary_path);
-    vt.set_min_match_depth(3);
-    vt.compute_normalizing_constants();
-    training_indices = vt.max_ind();
+ModelDatabaseRetrieval::ModelDatabaseRetrieval(ros::NodeHandle & n, std::string retrieval_name, std::string conversion_name, std::string insert_name){
+	retrieval_client	= n.serviceClient<quasimodo_msgs::query_cloud>				(retrieval_name);
+	conversion_client	= n.serviceClient<quasimodo_msgs::model_to_retrieval_query>	(conversion_name);
+	insert_client		= n.serviceClient<quasimodo_msgs::insert_model>				(insert_name);
+	storage = new ModelStorageFile();
+	//mongo::DBClientConnection c;
+	//c.connect("localhost");
+	//c.dropCollection("databaseName.collectionName");
 }
 
 ModelDatabaseRetrieval::~ModelDatabaseRetrieval(){}
 
-void ModelDatabaseRetrieval::add(reglib::Model * model){
-	models.push_back(model);
-	printf("number of models: %i\n",models.size());
+bool ModelDatabaseRetrieval::add(reglib::Model * model){
+	storage->add(model);
+	quasimodo_msgs::insert_model im;
+	im.request.model = quasimodo_brain::getModelMSG(model,true);
+	im.request.action = im.request.INSERT;
+	if (insert_client.call(im)){
+		object_ids.push_back(im.response.object_id);
+		models.push_back(model);
+		vocabulary_ids[im.response.vocabulary_id] = model;
+		v_ids.push_back(im.response.vocabulary_id);
+		printf("ADD: number of models total added to database: %6.6i last added vocabulary_id: %6.6i last added object_ids: %s\n",models.size(),v_ids.back(),object_ids.back().c_str());
+		return true;
+	}else{
+		ROS_ERROR("insert_client service INSERT FAIL!");
+		return false;
+	}
 }
 
 bool ModelDatabaseRetrieval::remove(reglib::Model * model){
+	storage->remove(model);
 	for(unsigned int i = 0; i < models.size(); i++){
 		if(models[i] == model){
-			models[i] = models.back();
-			models.pop_back();
-			return true;
+			//Remove stuff
+			quasimodo_msgs::insert_model im;
+			im.request.object_id = object_ids[i];
+			im.request.action = im.request.REMOVE;
+			if (insert_client.call(im)){
+				vocabulary_ids.erase(v_ids[i]);
+				models[i] = models.back();
+				models.pop_back();
+				std::string oid = object_ids[i];
+				object_ids[i] = object_ids.back();
+				object_ids.pop_back();
+				int vid = v_ids[i];
+				v_ids[i] = v_ids.back();
+				v_ids.pop_back();
+				printf("REMOVE: number of models in database: %6.6i removed vocabulary_id: %6.6i last added object_ids: %s\n",models.size(),vid,oid.c_str());
+				return true;
+			}else{
+				ROS_ERROR("insert_client service REMOVE FAIL!");
+				return false;
+			}
 		}
 	}
-	return false;
+	return true;
 }
 
 std::vector<reglib::Model *> ModelDatabaseRetrieval::search(reglib::Model * model, int number_of_matches){
+printf("start: %s\n",__PRETTY_FUNCTION__);
+	printf("---------------ids in database: %i----------------\n",v_ids.size());
+	for(unsigned int i = 0; i < v_ids.size(); i++){
+		printf("v_ids[%i] = %i\n",i,v_ids[i]);
+	}
+
 	std::vector<reglib::Model *> ret;
-		for(unsigned int i = 0; i < models.size(); i++){
-		if(models[i] != model){
-			ret.push_back(models[i]);
+	quasimodo_msgs::model_to_retrieval_query m2r;
+	m2r.request.model = quasimodo_brain::getModelMSG(model,true);
+	if (conversion_client.call(m2r)){
+		quasimodo_msgs::query_cloud qc;
+		qc.request.query = m2r.response.query;
+		qc.request.query.query_kind = qc.request.query.MONGODB_QUERY;
+		qc.request.query.number_query = number_of_matches+10;
+		if (retrieval_client.call(qc)){
+			quasimodo_msgs::retrieval_result result = qc.response.result;
+			printf("---------------ids in searchresult: %i----------------\n",result.vocabulary_ids.size());
+			for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){
+				int ind = std::stoi(result.vocabulary_ids[i]);
+				printf("found object with ind: %i\n",ind);
+			}
+			printf("----------------------------------------------\n");
+			for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){
+				int ind = std::stoi(result.vocabulary_ids[i]);
+				printf("found object with ind: %i\n",ind);
+				if(vocabulary_ids.count(ind) == 0){	printf("does not exist in db, continue\n"); continue; }
+				reglib::Model * search_model = vocabulary_ids[ind];
+				if(search_model != model){printf("adding to search results\n"); ret.push_back(search_model);}
+				if(ret.size() == number_of_matches){printf("found enough search results\n"); return ret;}
+			}
+		}else{
+			ROS_ERROR("retrieval_client service FAIL!");
 		}
-		if(ret.size() == number_of_matches){break;}
+	}else{
+		ROS_ERROR("model_to_retrieval_query service FAIL!");
 	}
+printf("stop: %s\n",__PRETTY_FUNCTION__);
 	return ret;
 }
-
-
-//Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching
-int ModelDatabaseRetrieval::add(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud, double weight)
-{
-    CloudT::Ptr points(new CloudT);
-    NormalCloudT::Ptr normals(new NormalCloudT);
-    for (const pcl::PointXYZRGBNormal& pn : cloud->points) {
-        points->push_back(PointT());
-        points->back().getVector3fMap() = pn.getVector3fMap();
-        points->back().getRGBVector4i() = pn.getRGBVector4i();
-        normals->push_back(NormalT());
-        normals->back().getNormalVector3fMap() = pn.getNormalVector3fMap();
-    }
-
-    HistCloudT::Ptr features(new HistCloudT);
-    CloudT::Ptr keypoints(new CloudT);
-    dynamic_object_retrieval::compute_features(features, keypoints, points, normals, false, true);
-
-    cout << "Got features!" << endl;
-
-    vt_features.push_back(HistCloudT::Ptr(new HistCloudT(*features)));
-
-    int old_index = vt.max_ind();
-
-    cout << "Trying to append cloud" << endl;
-
-    vector<int> indices(features->size(), old_index);
-
-    cout << "Features cloud size: " << features->size() << endl;
-    vt.append_cloud(features, indices, false);
-
-    cout << "Got to append the cloud!" << endl;
-
-    int new_index = vt.max_ind()-1;
-
-    added_indices.insert(make_pair(new_index, added_indices.size()));
-
-    cout << "Added cloud with index: " << new_index << endl;
-
-    return added_indices.size()-1;
-}
-
-// return true if successfull
-// return false if fail
-bool ModelDatabaseRetrieval::remove(int index)
-{
-    // don't do this for now
-    removed_indices.insert(index);
-
-    return false;
-}
-
-//Find the number_of_matches closest matches in dabase to the pointcloud for index
-std::vector<int> ModelDatabaseRetrieval::search(int index, int number_of_matches)
-{
-    using result_type = vocabulary_tree<HistT, 8>::result_type;
-    vector<result_type> scores;
-
-    cout << "Trying to search with index: " << index << endl;
-    cout << "Number clouds added: " << vt_features.size() << endl;
-
-    vt.query_vocabulary(scores, vt_features[index], 0);
-    vector<int> rtn;
-
-    cout << "Finished querying, got " << scores.size() << " results!" << endl;
-
-    for (const result_type& r : scores) {
-        if (rtn.size() >= number_of_matches) {
-            break;
-        }
-        if (r.index < training_indices) {
-            continue;
-        }
-        cout << "Accessing added cloud at vt index: " << r.index << endl;
-        int return_index = added_indices[r.index];
-        if (removed_indices.count(return_index) == 0) {
-            rtn.push_back(return_index);
-        }
-    }
-
-    cout << "Filtered out only the added cloud, got " << rtn.size() << " clouds!" << endl;
-
-    return rtn;
-}
diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h
index f19286c7..e1e650b4 100644
--- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h
+++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h
@@ -2,38 +2,27 @@
 #define MODELDATABASERETRIEVAL_H
 
 #include "ModelDatabase.h"
-#include <vocabulary_tree/vocabulary_tree.h>
-#include <dynamic_object_retrieval/visualize.h>
-
-using HistT = pcl::Histogram<250>;
-using HistCloudT = pcl::PointCloud<HistT>;
+#include "quasimodo_msgs/query_cloud.h"
+#include "quasimodo_msgs/model_to_retrieval_query.h"
+#include "quasimodo_msgs/insert_model.h"
+#include <map>
 
 class ModelDatabaseRetrieval: public ModelDatabase{
-private:
-
-    vocabulary_tree<HistT, 8> vt;
-    vector<HistCloudT::Ptr> vt_features;
-    map<int, int> added_indices;
-    int training_indices;
-    set<int> removed_indices;
-
 public:
+	ros::ServiceClient retrieval_client;
+	ros::ServiceClient conversion_client;
+	ros::ServiceClient insert_client;
+
 
+	std::vector< std::string > object_ids;
+	std::vector< unsigned long > v_ids;
+	std::map<unsigned long, reglib::Model * > vocabulary_ids;
 
-	virtual void add(reglib::Model * model);
+	virtual bool add(reglib::Model * model);
 	virtual bool remove(reglib::Model * model);
 	virtual std::vector<reglib::Model *> search(reglib::Model * model, int number_of_matches);
 
-    virtual int add(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud, double weight = 1);
-
-    // return true if successfull
-    // return false if fail
-    virtual bool remove(int index);
-
-    //Find the number_of_matches closest matches in dabase to the pointcloud for index
-    virtual std::vector<int> search(int index, int number_of_matches);
-
-	ModelDatabaseRetrieval(std::string vpath = "/media/johane/SSDstorage/vocabulary_johan/");
+	ModelDatabaseRetrieval(ros::NodeHandle & n, std::string retrieval_name = "/quasimodo_retrieval_service", std::string conversion_name = "/models/server",std::string insert_name = "/insert_model_service");
     ~ModelDatabaseRetrieval();
 };
 
diff --git a/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.cpp b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.cpp
new file mode 100644
index 00000000..cccdcfe3
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.cpp
@@ -0,0 +1,14 @@
+#include "ModelStorage.h"
+
+ModelStorage::ModelStorage(){}
+ModelStorage::~ModelStorage(){}
+
+//Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching
+bool ModelStorage::add(reglib::Model * model, std::string parrentpath){
+	return true;
+}
+
+bool ModelStorage::remove(reglib::Model * model){
+	return false;
+}
+		
diff --git a/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.h b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.h
new file mode 100644
index 00000000..1669f93a
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.h
@@ -0,0 +1,28 @@
+#ifndef ModelStorage_H
+#define ModelStorage_H
+
+#include <vector>
+
+// PCL specific includes
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "model/Model.h"
+#include "../Util/Util.h"
+
+class ModelStorage{
+	public:
+
+	//Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching
+	virtual bool add(reglib::Model * model, std::string parrentpath = "");
+		
+	// return true if successfull
+	// return false if fail
+	virtual bool remove(reglib::Model * model);
+		
+	ModelStorage();
+	~ModelStorage();
+};
+
+#include "ModelStorageFile.h"
+#endif // ModelStorage_H
diff --git a/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.cpp b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.cpp
new file mode 100644
index 00000000..9bfe3597
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.cpp
@@ -0,0 +1,48 @@
+#include "ModelStorageFile.h"
+
+ModelStorageFile::ModelStorageFile(std::string filepath_){
+	filepath = filepath_;
+	if(!quasimodo_brain::fileExists(filepath+"/database_tmp")){
+		char command [1024];
+		sprintf(command,"mkdir %s",(filepath+"/database_tmp").c_str());
+		int r = system(command);
+		if(!quasimodo_brain::fileExists(filepath+"/database_tmp/frames")){
+			sprintf(command,"mkdir %s",(filepath+"/database_tmp/frames").c_str());
+			r = system(command);
+		}
+	}
+}
+ModelStorageFile::~ModelStorageFile(){}
+
+
+bool ModelStorageFile::add(reglib::Model * model, std::string parrentpath){
+//	printf("%s\n",__PRETTY_FUNCTION__);
+
+
+//	for(unsigned int i = 0; i < model->frames.size(); i++){//Add all frames to the frame database
+//		std::string keyval = model->frames[i]->keyval;
+//		printf("keyval: %s\n",keyval.c_str());
+//	}
+
+
+//	for(unsigned int i = 0; i < model->submodels.size(); i++){//Add all frames to the frame database
+//		add(model->submodels[i],parrentpath+"/sub");
+//	}
+	//	models.push_back(model);
+	//	return true;
+	//	//printf("number of models: %i\n",models.size());
+	//exit(0);
+}
+
+bool ModelStorageFile::remove(reglib::Model * model){
+	//printf("%s\n",__PRETTY_FUNCTION__);
+	//	for(unsigned int i = 0; i < models.size(); i++){
+	//		if(models[i] == model){
+	//			models[i] = models.back();
+	//			models.pop_back();
+	//			return true;
+	//		}
+	//	}
+	//	return false;
+	//exit(0);
+}
diff --git a/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.h b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.h
new file mode 100644
index 00000000..78d83fce
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.h
@@ -0,0 +1,18 @@
+#ifndef ModelStorageFile_H
+#define ModelStorageFile_H
+
+#include "ModelStorage.h"
+
+
+class ModelStorageFile: public ModelStorage{
+	public:
+	std::string filepath;
+	virtual bool add(reglib::Model * model, std::string parrentpath = "");
+	virtual bool remove(reglib::Model * model);
+	ModelStorageFile(std::string filepath_ = "./");
+	~ModelStorageFile();
+};
+
+
+
+#endif // ModelStorageFile_H
diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp
new file mode 100644
index 00000000..5f43083f
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp
@@ -0,0 +1,1498 @@
+#include "Util.h"
+
+namespace quasimodo_brain {
+
+bool fileExists(std::string path){
+	QFile file(path.c_str());
+	if (file.exists()){
+		return true;
+	}else{
+		return false;
+	}
+}
+
+int getdir (std::string dir, std::vector<std::string> & files){
+	DIR *dp;
+	struct dirent *dirp;
+	if((dp  = opendir(dir.c_str())) == NULL) {
+		cout << "Error(" << errno << ") opening " << dir << endl;
+		return errno;
+	}
+
+	while ((dirp = readdir(dp)) != NULL) {
+		files.push_back(std::string(dirp->d_name));
+	}
+	closedir(dp);
+	return 0;
+}
+
+std::string getPoseString(Eigen::Matrix4d pose){
+	char buf [1024];
+	for(unsigned int i = 0; i < 4; i ++){
+		for(unsigned int j = 0; j < 4; j ++){
+			if(i == 0 && j == 0){
+				sprintf(buf,"%10.10f",pose(i,j));
+			}else{
+				std::string str = std::string(buf);
+				sprintf(buf,"%s %10.10f",str.c_str(),pose(i,j));
+			}
+		}
+	}
+	return std::string(buf);
+}
+
+
+Eigen::Matrix4d getPoseFromString(std::string str){
+	Eigen::Matrix4d pose;
+	for(unsigned int i = 0; i < 4; i ++){
+		for(unsigned int j = 0; j < 4; j ++){
+			std::size_t found = str.find(" ");
+			if (found!=std::string::npos){
+				pose(i,j) = atof(str.substr(0,found).c_str());
+				str = str.substr(found+1,str.length()-found);
+			}else{
+				pose(i,j) = atof(str.c_str());
+			}
+		}
+	}
+	return pose;
+}
+
+std::string initSegment(ros::NodeHandle& n, reglib::Model * model){
+	std::vector< std::string > scid;
+	std::vector< cv::Mat > masks;
+	std::vector< Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix<double, 4, 4> > > poses;
+	for(unsigned int i = 0; i < model->frames.size(); i++){
+		printf("frame: %i\n",i);
+		soma_llsd_msgs::Scene sc = getScene(n,model->frames[i]);
+		model->frames[i]->soma_id = sc.id;
+		poses.push_back(model->relativeposes[i]);
+		masks.push_back(model->modelmasks[i]->getMask());
+		scid.push_back(sc.id);
+		//		getPoseFromString(getPoseString(model->relativeposes[i]));
+	}
+
+	soma_llsd_msgs::Segment segment;
+	if(quasimodo_conversions::add_masks_to_soma_segment(n,scid, masks,poses,segment)){
+		std::string id = segment.id;
+		model->soma_id = id;
+		std::string metadata = "";
+		metadata += "<last_changed>";
+		metadata += std::to_string(model->last_changed);
+		metadata += "<\\last_changed>";
+
+		metadata += "<id>";
+		metadata += std::to_string(model->id);
+		metadata += "<\\id>";
+
+		metadata += "<score>";
+		metadata += std::to_string(model->score);
+		metadata += "<\\score>";
+
+		metadata += "<total_scores>";
+		metadata += std::to_string(model->total_scores);
+		metadata += "<\\total_scores>";
+
+		metadata += "<pointspath>";
+		metadata += model->pointspath;
+		metadata += "<\\pointspath>";
+
+		for(unsigned int i = 0; i < model->submodels.size(); i++){
+			metadata += "<submodel>";
+			std::string subid = initSegment(n, model->submodels[i]);
+			metadata += subid;
+			metadata += "<relativepose>";
+			metadata += getPoseString(model->submodels_relativeposes[i]);
+			metadata += "<\\relativepose>";
+			metadata += "<\\submodel>";
+		}
+		printf("metadata: %s\n",metadata.c_str());
+		return id;
+	}
+
+	return "";
+}
+
+reglib::Model * getModelFromSegment(ros::NodeHandle& n, std::string segment_id){
+	reglib::Model * model = new reglib::Model();
+
+    ros::ServiceClient segclient = n.serviceClient<soma_llsd::GetSegment>("/soma_llsd/get_segment");
+	ROS_INFO("Waiting for /soma_llsd/get_segment service...");
+    if (!segclient.waitForExistence(ros::Duration(1.0))) {
+		ROS_INFO("Failed to get /soma_llsd/get_segment service!");
+		return model;
+	}
+	ROS_INFO("Got /soma_llsd/get_segment service");
+
+    soma_llsd::GetSegment segsrv;
+    segsrv.request.segment_id = model->soma_id;
+    if (segclient.call(segsrv)) {
+		ROS_INFO("Got /soma_llsd/get_segment");
+		return model;
+	}
+
+    ros::ServiceClient client = n.serviceClient<soma_llsd::GetScene>("/soma_llsd/get_scene");
+    ROS_INFO("Waiting for /soma_llsd/get_scene service...");
+    if (!client.waitForExistence(ros::Duration(1.0))) {
+        ROS_INFO("Failed to get /soma_llsd/get_scene service!");
+        return model;
+    }
+    ROS_INFO("Got /soma_llsd/get_scene service");
+
+    soma_llsd_msgs::Segment segment = segsrv.response.response;
+    for (const soma_llsd_msgs::Observation& obs : segment.observations) {
+        soma_llsd::GetScene srv;
+        srv.request.scene_id = obs.scene_id;
+        if (!client.call(srv)) {
+            ROS_ERROR("Failed to call service /soma_llsd/get_scene");
+            return model;
+        }
+
+        cv::Mat mask;
+
+        sensor_msgs::Image image_mask = obs.image_mask;
+        reglib::RGBDFrame * frame = getFrame(srv.response.response);
+
+        model->frames.push_back(frame);
+
+        geometry_msgs::Pose	pose1 = obs.pose;
+        Eigen::Affine3d epose1;
+        tf::poseMsgToEigen(pose1, epose1);
+        model->relativeposes.push_back(epose1.matrix());
+
+//		cv_bridge::CvImagePtr			mask_ptr;
+//		try{							mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);}
+//		catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}
+//		cv::Mat mask = mask_ptr->image;
+        quasimodo_conversions::convert_to_mask_msg(mask,image_mask);
+        model->modelmasks.push_back(new reglib::ModelMask(mask));
+    }
+    model->recomputeModelPoints();
+
+	//Read frames etc
+	//Read parameters
+	//Read submodels
+	return model;
+	//    std::vector< std::string > scid;
+	//    std::vector< cv::Mat > masks;
+	//    std::vector< Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix<double, 4, 4> > > poses;
+	//    for(unsigned int i = 0; i < model->frames.size(); i++){
+	//		printf("frame: %i\n",i);
+	//        soma_llsd_msgs::Scene sc = getScene(n,model->frames[i]);
+	//        poses.push_back(model->relativeposes[i]);
+	//        masks.push_back(model->modelmasks[i]->getMask());
+	//        scid.push_back(sc.id);
+	////		getPoseFromString(getPoseString(model->relativeposes[i]));
+	//    }
+
+	//    soma_llsd_msgs::Segment segment;
+	//    if(quasimodo_conversions::add_masks_to_soma_segment(n,scid, masks,poses,segment)){
+	//        std::string id = segment.id;
+	//		model->soma_id = id;
+	//		std::string metadata = "";
+	//		metadata += "<last_changed>";
+	//		metadata += std::to_string(model->last_changed);
+	//		metadata += "<\\last_changed>";
+
+	//		metadata += "<id>";
+	//		metadata += std::to_string(model->id);
+	//		metadata += "<\\id>";
+
+	//		metadata += "<score>";
+	//		metadata += std::to_string(model->score);
+	//		metadata += "<\\score>";
+
+	//		metadata += "<total_scores>";
+	//		metadata += std::to_string(model->total_scores);
+	//		metadata += "<\\total_scores>";
+
+	//		metadata += "<pointspath>";
+	//		metadata += model->pointspath;
+	//		metadata += "<\\pointspath>";
+
+	//        for(unsigned int i = 0; i < model->submodels.size(); i++){
+	//            metadata += "<submodel>";
+	//            std::string subid = initSegment(n, model->submodels[i]);
+	//            metadata += subid;
+	//            metadata += "<relativepose>";
+	//			metadata += getPoseString(model->submodels_relativeposes[i]);
+	//			metadata += "<\\relativepose>";
+	//			metadata += "<\\submodel>";
+	//        }
+	//        printf("metadata: %s\n",metadata.c_str());
+	//        return id;
+	//    }
+
+	//	return "";
+}
+
+soma_llsd_msgs::Scene getScene(ros::NodeHandle & n, reglib::RGBDFrame * frame, std::string current_waypointid, std::string roomRunNumber){
+	soma_llsd_msgs::Scene sc;
+	if(frame->soma_id.length() > 0){//Frame already connected to scene, get from db
+		ros::ServiceClient client = n.serviceClient<soma_llsd::GetScene>("/soma_llsd/get_scene");
+		ROS_INFO("Waiting for /soma_llsd/get_scene service...");
+		if (!client.waitForExistence(ros::Duration(1.0))) {
+			ROS_INFO("Failed to get /soma_llsd/get_scene service!");
+			return sc;
+		}
+		ROS_INFO("Got /soma_llsd/get_scene service");
+
+		soma_llsd::GetScene srv;
+		srv.request.scene_id = frame->soma_id;
+		if (client.call(srv)) {
+			ROS_INFO("Got /soma_llsd/get_scene");
+			return srv.response.response;
+		}
+	}
+
+	geometry_msgs::Pose		pose;
+	tf::poseEigenToMsg (Eigen::Affine3d(frame->pose), pose);
+
+	cv_bridge::CvImage rgbBridgeImage;
+	rgbBridgeImage.image = frame->rgb;
+	rgbBridgeImage.encoding = "bgr8";
+
+	cv_bridge::CvImage depthBridgeImage;
+	depthBridgeImage.image = frame->depth;
+	depthBridgeImage.encoding = "mono16";
+
+	sensor_msgs::PointCloud2 input;
+	pcl::toROSMsg (*frame->getPCLcloud(),input);//, *transformed_cloud);
+	input.header.frame_id = "/map";
+
+	ros::ServiceClient insertclient = n.serviceClient<soma_llsd::InsertScene>("/soma_llsd/insert_scene");
+	ROS_INFO("Waiting for /soma_llsd/insert_scene service...");
+	if (!insertclient.waitForExistence(ros::Duration(1.0))) {
+		ROS_INFO("Failed to get /soma_llsd/insert_scene service!");
+		return sc;
+	}
+	ROS_INFO("Got /soma_llsd/insert_scene service");
+
+	soma_llsd::InsertScene scene;
+	scene.request.rgb_img = *(rgbBridgeImage.toImageMsg());
+	scene.request.depth_img = *(depthBridgeImage.toImageMsg());
+	scene.request.camera_info.K[0] = frame->camera->fx;
+	scene.request.camera_info.K[4] = frame->camera->fy;
+	scene.request.camera_info.K[2] = frame->camera->cx;
+	scene.request.camera_info.K[5] = frame->camera->cy;
+	scene.request.robot_pose = pose;
+	scene.request.cloud = input;
+	scene.request.waypoint = current_waypointid;
+	scene.request.episode_id = roomRunNumber;
+
+	if (!insertclient.call(scene)) {
+		ROS_ERROR("Failed to call service /soma_llsd/insert_scene");
+		return sc;
+	}
+	return scene.response.response;
+}
+
+reglib::Camera * getCam(sensor_msgs::CameraInfo & info){
+	reglib::Camera * cam		= new reglib::Camera();
+	if(info.K[0] > 0){
+		cam->fx = info.K[0];
+		cam->fy = info.K[4];
+		cam->cx = info.K[2];
+		cam->cy = info.K[5];
+	}
+	return cam;
+}
+
+reglib::RGBDFrame * getFrame(soma_llsd_msgs::Scene & scene){
+
+	reglib::Camera * cam = getCam(scene.camera_info);
+    cam->idepth_scale = 1.0/5000.0;
+
+	cv_bridge::CvImagePtr			rgb_ptr;
+	try{							rgb_ptr = cv_bridge::toCvCopy(scene.rgb_img, sensor_msgs::image_encodings::BGR8);}
+	catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}
+	cv::Mat rgb = rgb_ptr->image;
+
+	cv_bridge::CvImagePtr			depth_ptr;
+	//try{							depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::MONO16);}
+	try{							depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::TYPE_32FC1);}
+	catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}
+	//cv::Mat depth = depth_ptr->image;
+    cv::Mat depth;
+    depth_ptr->image.convertTo(depth, CV_16UC1, 5000.0);
+
+
+	Eigen::Affine3d epose;
+	tf::poseMsgToEigen(scene.robot_pose, epose);
+	reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, 0, epose.matrix(),true,"",true);
+	frame->soma_id = scene.id;
+	return frame;
+}
+
+
+std::vector<reglib::superpoint> getSuperPoints(std::string path){
+	std::vector<reglib::superpoint> spvec;
+	std::streampos size;
+	char * memblock;
+	std::ifstream file (path, ios::in|ios::binary|ios::ate);
+	if (file.is_open())	{
+		size = file.tellg();
+		if(size == 0){return spvec;}
+
+		memblock = new char [size];
+		file.seekg (0, ios::beg);
+		file.read (memblock, size);
+		file.close();
+
+		cout << "the entire file content is in memory";
+
+		long nr_points = size / (sizeof(float)*12);
+		printf("loading %i points\n",nr_points);
+
+		float * data = (float *)memblock;
+
+		spvec.resize(nr_points);
+		long count = 0;
+		for(unsigned long i = 0; i < nr_points; i++){
+			reglib::superpoint & p = spvec[i];
+			p.point(0)			= data[count++];
+			p.point(1)			= data[count++];
+			p.point(2)			= data[count++];
+			p.normal(0)			= data[count++];
+			p.normal(1)			= data[count++];
+			p.normal(2)			= data[count++];
+			p.feature(0)		= data[count++];
+			p.feature(1)		= data[count++];
+			p.feature(2)		= data[count++];
+			p.point_information	= data[count++];
+			p.normal_information = data[count++];
+			p.feature_information = data[count++];
+		}
+
+		delete[] memblock;
+	}else{
+		std::cout << "Unable to open file superpoint file, go process that data" << std::endl;
+	}
+	return spvec;
+}
+
+std::vector<reglib::superpoint> getRoomSuperPoints(std::string path, std::string savePath){
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+	return getSuperPoints(sweep_folder+"/superpoints.bin");
+}
+
+void transformSuperPoints(std::vector<reglib::superpoint> & spvec, Eigen::Matrix4d cp){
+	float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3);
+	float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3);
+	float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3);
+
+	for(unsigned long i = 0; i < spvec.size(); i++){
+		reglib::superpoint & p = spvec[i];
+
+		float x		= p.point(0);
+		float y		= p.point(1);
+		float z		= p.point(2);
+		float nx	= p.normal(0);
+		float ny	= p.normal(1);
+		float nz	= p.normal(2);
+
+		float tx	= m00*x + m01*y + m02*z + m03;
+		float ty	= m10*x + m11*y + m12*z + m13;
+		float tz	= m20*x + m21*y + m22*z + m23;
+
+		float tnx	= m00*nx + m01*ny + m02*nz;
+		float tny	= m10*nx + m11*ny + m12*nz;
+		float tnz	= m20*nx + m21*ny + m22*nz;
+
+		p.point(0)	= tx;
+		p.point(1)	= ty;
+		p.point(2)	= tz;
+		p.normal(0) = tnx;
+		p.normal(1) = tny;
+		p.normal(2) = tnz;
+	}
+}
+
+void saveSuperPoints(std::string path, std::vector<reglib::superpoint> & spvec, Eigen::Matrix4d pose, float ratio_keep){
+	printf("saveSuperPoints(%s)\n",path.c_str());
+	transformSuperPoints(spvec,pose);
+	//XYZ RGB NXNYNZ W1 W2
+	long sizeofSuperPoint = 3*(3+1);
+
+	std::ofstream file;
+	file.open(path, ios::out | ios::binary);
+	if(spvec.size() != 0){
+		float * data = new float[spvec.size() * sizeofSuperPoint];
+		long added = 0;
+		long count = 0;
+		for(unsigned long i = 0; i < spvec.size(); i++){
+			if(double(rand() % 1000)*0.001 > ratio_keep ){continue;}
+			reglib::superpoint & p = spvec[i];
+			data[count++] = p.point(0);
+			data[count++] = p.point(1);
+			data[count++] = p.point(2);
+			data[count++] = p.normal(0);
+			data[count++] = p.normal(1);
+			data[count++] = p.normal(2);
+			data[count++] = p.feature(0);
+			data[count++] = p.feature(1);
+			data[count++] = p.feature(2);
+			data[count++] = p.point_information;
+			data[count++] = p.normal_information;
+			data[count++] = p.feature_information;
+			added++;
+		}
+		if(added > 0){
+			printf("saving %i points\n",added);
+			file.write( (char*)data, added*sizeofSuperPoint*sizeof(float));
+		}
+		delete[] data;
+	}
+	file.close();
+}
+
+std::vector<Eigen::Matrix4d> readPoseXML(std::string xmlFile){
+	std::vector<Eigen::Matrix4d> poses;
+	QFile file(xmlFile.c_str());
+
+	if (!file.exists()){
+		//ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str());
+		return poses;
+	}
+
+	file.open(QIODevice::ReadOnly);
+	//ROS_INFO_STREAM("Parsing xml file: "<<xmlFile.c_str());
+
+	QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+
+	while (!xmlReader->atEnd() && !xmlReader->hasError()){
+		QXmlStreamReader::TokenType token = xmlReader->readNext();
+		if (token == QXmlStreamReader::StartDocument)
+			continue;
+
+		if (xmlReader->hasError()){
+			ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str());
+			return poses;
+		}
+
+		QString elementName = xmlReader->name().toString();
+
+		if (token == QXmlStreamReader::StartElement){
+			if (xmlReader->name() == "Pose"){
+				Eigen::Matrix4d pose = quasimodo_brain::getPose(xmlReader);
+
+				token = xmlReader->readNext();//Pose
+				elementName = xmlReader->name().toString();
+
+				//std::cout << pose << std::endl << std::endl;
+				poses.push_back(pose);
+			}
+		}
+	}
+	delete xmlReader;
+	//printf("done readPoseXML\n");
+	return poses;
+}
+
+void savePoses(std::string xmlFile, std::vector<Eigen::Matrix4d> poses, int maxposes){
+	QFile file(xmlFile.c_str());
+	if (file.exists()){file.remove();}
+
+	if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){
+		std::cerr<<"Could not open file "<< xmlFile <<" to save views as XML"<<std::endl;
+		return;
+	}
+
+	QXmlStreamWriter* xmlWriter = new QXmlStreamWriter();
+	xmlWriter->setDevice(&file);
+
+	xmlWriter->writeStartDocument();
+	xmlWriter->writeStartElement("Poses");
+	//	xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size()));
+	for(unsigned int i = 0; i < poses.size() && (maxposes == -1 || int(i) < maxposes); i++){
+		printf("saving %i\n",i);
+		xmlWriter->writeStartElement("Pose");
+		writePose(xmlWriter,poses[i]);
+		xmlWriter->writeEndElement();
+	}
+	xmlWriter->writeEndElement(); // Poses
+	xmlWriter->writeEndDocument();
+	delete xmlWriter;
+}
+
+int readNumberOfViews(std::string xmlFile){
+	QFile file(xmlFile.c_str());
+	if (!file.exists()){return -1;}
+
+	file.open(QIODevice::ReadOnly);
+	QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+	int count = 0;
+	while (!xmlReader->atEnd() && !xmlReader->hasError()){
+		QXmlStreamReader::TokenType token = xmlReader->readNext();
+		if (token == QXmlStreamReader::StartDocument)
+			continue;
+
+		if (xmlReader->hasError()){return -1;}
+
+		if (token == QXmlStreamReader::StartElement){
+			if (xmlReader->name() == "View"){
+				count++;
+			}
+		}
+	}
+	delete xmlReader;
+	return count;
+}
+
+void writeXml(std::string xmlFile, std::vector<reglib::RGBDFrame *> & frames, std::vector<Eigen::Matrix4d> & poses){
+	int slash_pos = xmlFile.find_last_of("/");
+	std::string sweep_folder = xmlFile.substr(0, slash_pos) + "/";
+
+	QFile file(xmlFile.c_str());
+	if (file.exists()){file.remove();}
+
+
+	if (!file.open(QIODevice::ReadWrite | QIODevice::Text))
+	{
+		std::cerr<<"Could not open file "<<sweep_folder<<"additionalViews.xml to save views as XML"<<std::endl;
+		return;
+	}
+
+	QXmlStreamWriter* xmlWriter = new QXmlStreamWriter();
+	xmlWriter->setDevice(&file);
+
+	xmlWriter->writeStartDocument();
+	xmlWriter->writeStartElement("Views");
+	xmlWriter->writeAttribute("number_of_views", QString::number(frames.size()));
+	for(unsigned int i = 0; i < frames.size(); i++){
+		reglib::RGBDFrame * frame = frames[i];
+		char buf [1024];
+
+		xmlWriter->writeStartElement("View");
+		sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),i);
+		cv::imwrite(buf, frame->rgb );
+
+		sprintf(buf,"view_RGB%10.10i.png",i);
+		xmlWriter->writeAttribute("RGB", QString(buf));
+
+
+		sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),i);
+		cv::imwrite(buf, frame->depth );
+		sprintf(buf,"view_DEPTH%10.10i.png",i);
+		xmlWriter->writeAttribute("DEPTH", QString(buf));
+
+		long nsec = 1e9*((frame->capturetime)-std::floor(frame->capturetime));
+		long sec = frame->capturetime;
+		xmlWriter->writeStartElement("Stamp");
+
+		xmlWriter->writeStartElement("sec");
+		xmlWriter->writeCharacters(QString::number(sec));
+		xmlWriter->writeEndElement();
+
+		xmlWriter->writeStartElement("nsec");
+		xmlWriter->writeCharacters(QString::number(nsec));
+		xmlWriter->writeEndElement();
+
+		xmlWriter->writeEndElement(); // Stamp
+
+		xmlWriter->writeStartElement("Camera");
+
+		xmlWriter->writeStartElement("fx");
+		xmlWriter->writeCharacters(QString::number(frame->camera->fx));
+		xmlWriter->writeEndElement();
+
+		xmlWriter->writeStartElement("fy");
+		xmlWriter->writeCharacters(QString::number(frame->camera->fy));
+		xmlWriter->writeEndElement();
+
+		xmlWriter->writeStartElement("cx");
+		xmlWriter->writeCharacters(QString::number(frame->camera->cx));
+		xmlWriter->writeEndElement();
+
+		xmlWriter->writeStartElement("cy");
+		xmlWriter->writeCharacters(QString::number(frame->camera->cy));
+		xmlWriter->writeEndElement();
+
+		xmlWriter->writeEndElement(); // camera
+
+		xmlWriter->writeStartElement("RegisteredPose");
+		quasimodo_brain::writePose(xmlWriter,poses[i]);
+		xmlWriter->writeEndElement();
+
+		xmlWriter->writeStartElement("Pose");
+		quasimodo_brain::writePose(xmlWriter,frame->pose);
+		xmlWriter->writeEndElement();
+
+		xmlWriter->writeEndElement();
+	}
+	xmlWriter->writeEndElement(); // Semantic Room
+	xmlWriter->writeEndDocument();
+	delete xmlWriter;
+}
+
+Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){
+	QXmlStreamReader::TokenType token = xmlReader->readNext();//Translation
+	QString elementName = xmlReader->name().toString();
+
+	token = xmlReader->readNext();//fx
+	double tx = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//fy
+	double ty = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//cx
+	double tz = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//Translation
+	elementName = xmlReader->name().toString();
+
+	token = xmlReader->readNext();//Rotation
+	elementName = xmlReader->name().toString();
+
+	token = xmlReader->readNext();//qw
+	double qw = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//qx
+	double qx = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//qy
+	double qy = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//qz
+	double qz = atof(xmlReader->readElementText().toStdString().c_str());
+
+	token = xmlReader->readNext();//Rotation
+	elementName = xmlReader->name().toString();
+
+	Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix();
+	regpose(0,3) = tx;
+	regpose(1,3) = ty;
+	regpose(2,3) = tz;
+
+	return regpose;
+}
+
+void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose){
+	Eigen::Quaterniond q = Eigen::Quaterniond ((Eigen::Affine3d (pose)).rotation());
+
+	xmlWriter->writeStartElement("Translation");
+	xmlWriter->writeStartElement("x");
+	xmlWriter->writeCharacters(QString::number(pose(0,3)));
+	xmlWriter->writeEndElement();
+	xmlWriter->writeStartElement("y");
+	xmlWriter->writeCharacters(QString::number(pose(1,3)));
+	xmlWriter->writeEndElement();
+	xmlWriter->writeStartElement("z");
+	xmlWriter->writeCharacters(QString::number(pose(2,3)));
+	xmlWriter->writeEndElement();
+	xmlWriter->writeEndElement(); // Translation
+
+	xmlWriter->writeStartElement("Rotation");
+	xmlWriter->writeStartElement("w");
+	xmlWriter->writeCharacters(QString::number(q.w()));
+	xmlWriter->writeEndElement();
+	xmlWriter->writeStartElement("x");
+	xmlWriter->writeCharacters(QString::number(q.x()));
+	xmlWriter->writeEndElement();
+	xmlWriter->writeStartElement("y");
+	xmlWriter->writeCharacters(QString::number(q.y()));
+	xmlWriter->writeEndElement();
+	xmlWriter->writeStartElement("z");
+	xmlWriter->writeCharacters(QString::number(q.z()));
+	xmlWriter->writeEndElement();
+	xmlWriter->writeEndElement(); //Rotation
+}
+
+void remove_old_seg(std::string sweep_folder){
+	printf("remove_old_seg: %s\n",sweep_folder.c_str());
+	DIR *dir;
+	struct dirent *ent;
+	if ((dir = opendir (sweep_folder.c_str())) != NULL) {
+		/* print all the files and directories within directory */
+		while ((ent = readdir (dir)) != NULL) {
+			std::string file = std::string(ent->d_name);
+
+			if (file.find("dynamic_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){
+				printf ("removing %s\n", ent->d_name);
+				std::remove((sweep_folder+"/"+file).c_str());
+			}
+
+			if (file.find("dynamicmask") !=std::string::npos && file.find(".png") !=std::string::npos){
+				printf ("removing %s\n", ent->d_name);
+				std::remove((sweep_folder+"/"+file).c_str());
+			}
+
+			if (file.find("moving_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){
+				printf ("removing %s\n", ent->d_name);
+				std::remove((sweep_folder+"/"+file).c_str());
+			}
+
+			if (file.find("movingmask") !=std::string::npos && file.find(".png") !=std::string::npos){
+				printf ("removing %s\n", ent->d_name);
+				std::remove((sweep_folder+"/"+file).c_str());
+			}
+		}
+		closedir (dir);
+	}
+	printf("done remove_old_seg\n");
+}
+
+std::string replaceAll(std::string str, std::string from, std::string to){
+	std::size_t found = str.find(from);
+	while(found!=std::string::npos){
+		str.replace(found,from.size(),to);
+		found = str.find(from);
+	}
+	return str;
+}
+
+void cleanPath(std::string & path){
+	std::size_t found = path.find("//");
+	while (found!=std::string::npos){
+		path.replace(found,2,"/");
+		found = path.find("//");
+	}
+}
+
+bool xmlSortFunction (std::string & a, std::string & b) {
+	return true;
+}
+
+void sortXMLs(std::vector<std::string> & sweeps){
+
+}
+
+reglib::Model * loadFromRaresFormat(std::string path){
+	reglib::Model * model = new reglib::Model();
+	reglib::Model * sweep = load_metaroom_model(path);
+	if(sweep->frames.size() == 0){
+		printf("no frames in sweep, returning\n");
+		return sweep;
+	}
+
+	model->submodels.push_back(sweep);
+	model->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity());
+
+
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+	printf("folder: %s\n",sweep_folder.c_str());
+
+
+	std::vector<semantic_map_load_utilties::DynamicObjectData<pcl::PointXYZRGB>> objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep<pcl::PointXYZRGB>(sweep_folder+"room.xml");
+
+	for (auto object : objects){
+		if (!object.objectScanIndices.size()){continue;}
+
+		printf("%i %i %i\n",int(object.vAdditionalViews.size()),int(object.vAdditionalViewsTransformsRegistered.size()),int(object.vAdditionalViewsTransforms.size()));
+		for (unsigned int i = 0; i < object.vAdditionalViews.size(); i ++){
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = object.vAdditionalViews[i];
+
+			cv::Mat rgb;
+			rgb.create(cloud->height,cloud->width,CV_8UC3);
+			unsigned char * rgbdata = (unsigned char *)rgb.data;
+
+			cv::Mat depth;
+			depth.create(cloud->height,cloud->width,CV_16UC1);
+			unsigned short * depthdata = (unsigned short *)depth.data;
+
+			unsigned int nr_data = cloud->height * cloud->width;
+			for(unsigned int j = 0; j < nr_data; j++){
+				pcl::PointXYZRGB p = cloud->points[j];
+				rgbdata[3*j+0]	= p.b;
+				rgbdata[3*j+1]	= p.g;
+				rgbdata[3*j+2]	= p.r;
+				depthdata[j]	= short(5000.0 * p.z);
+			}
+
+			cv::Mat fullmask;
+			fullmask.create(cloud->height,cloud->width,CV_8UC1);
+			unsigned char * maskdata = (unsigned char *)fullmask.data;
+			for(int j = 0; j < nr_data; j++){maskdata[j] = 255;}
+
+			reglib::Camera * cam		= sweep->frames.front()->camera->clone();
+
+			reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth,0, getMat(object.vAdditionalViewsTransforms[i]));
+			model->frames.push_back(frame);
+			if(model->relativeposes.size() == 0){
+				model->relativeposes.push_back(Eigen::Matrix4d::Identity());//current_room_frames.front()->pose.inverse() * frame->pose);
+			}else{
+				model->relativeposes.push_back(model->frames.front()->pose.inverse() * frame->pose);
+			}
+			model->modelmasks.push_back(new reglib::ModelMask(fullmask));
+		}
+	}
+
+
+	//	if(model->relativeposes.size() == 0){
+	//		model->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity());
+	//	}else{
+	//		model->submodels_relativeposes.push_back(model->relativeposes.front().inverse() * sweep->frames.front()->pose);
+	//	}
+
+
+	return model;
+}
+
+std::vector<Eigen::Matrix4f> getRegisteredViewPoses(std::string poses_file, int no_transforms){
+	std::vector<Eigen::Matrix4f> toRet;
+	ifstream in(poses_file);
+	if (!in.is_open()){
+		cout<<"ERROR: cannot find poses file "<<poses_file<<endl;
+		return toRet;
+	}
+	cout<<"Loading additional view registered poses from "<<poses_file<<endl;
+
+	for (int i=0; i<no_transforms+1; i++){
+		Eigen::Matrix4f transform;
+		float temp;
+		for (size_t j=0; j<4; j++){
+			for (size_t k=0; k<4; k++){
+				in >> temp;
+				transform(j,k) = temp;
+			}
+		}
+		toRet.push_back(transform);
+	}
+	return toRet;
+}
+
+double getTime(){
+	struct timeval start1;
+	gettimeofday(&start1, NULL);
+	return double(start1.tv_sec+(start1.tv_usec/1000000.0));
+}
+
+reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg, bool compute_edges){
+	reglib::Model * model = new reglib::Model();
+
+	for(unsigned int i = 0; i < msg.local_poses.size(); i++){
+		sensor_msgs::CameraInfo		camera			= msg.frames[i].camera;
+		ros::Time					capture_time	= msg.frames[i].capture_time;
+		geometry_msgs::Pose			pose			= msg.frames[i].pose;
+
+		cv_bridge::CvImagePtr			rgb_ptr;
+		try{							rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);}
+		catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}
+		cv::Mat rgb = rgb_ptr->image;
+
+		cv_bridge::CvImagePtr			depth_ptr;
+		try{							depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);}
+		catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}
+		cv::Mat depth = depth_ptr->image;
+
+		Eigen::Affine3d epose;
+		tf::poseMsgToEigen(pose, epose);
+
+		reglib::Camera * cam		= new reglib::Camera();
+		if(camera.K[0] > 0){
+			cam->fx = camera.K[0];
+			cam->fy = camera.K[4];
+			cam->cx = camera.K[2];
+			cam->cy = camera.K[5];
+		}
+
+		reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix(),true,"",compute_edges);
+		model->frames.push_back(frame);
+
+		geometry_msgs::Pose	pose1 = msg.local_poses[i];
+		Eigen::Affine3d epose1;
+		tf::poseMsgToEigen(pose1, epose1);
+		model->relativeposes.push_back(epose1.matrix());
+
+		cv_bridge::CvImagePtr			mask_ptr;
+		try{							mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);}
+		catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}
+		cv::Mat mask = mask_ptr->image;
+
+		model->modelmasks.push_back(new reglib::ModelMask(mask));
+	}
+	model->recomputeModelPoints();
+	return model;
+}
+
+void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp, bool addClouds){
+	int startsize = msg.local_poses.size();
+	msg.local_poses.resize(startsize+model->relativeposes.size());
+	msg.frames.resize(startsize+model->frames.size());
+	msg.masks.resize(startsize+model->modelmasks.size());
+	for(unsigned int i = 0; i < model->relativeposes.size(); i++){
+		geometry_msgs::Pose		pose1;
+		tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1);
+		geometry_msgs::Pose		pose2;
+		tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2);
+		cv_bridge::CvImage rgbBridgeImage;
+		rgbBridgeImage.image = model->frames[i]->rgb;
+		rgbBridgeImage.encoding = "bgr8";
+		cv_bridge::CvImage depthBridgeImage;
+		depthBridgeImage.image = model->frames[i]->depth;
+		depthBridgeImage.encoding = "mono16";
+		cv_bridge::CvImage maskBridgeImage;
+		maskBridgeImage.image			= model->modelmasks[i]->getMask();
+		maskBridgeImage.encoding		= "mono8";
+		msg.local_poses[startsize+i]			= pose1;
+		msg.frames[startsize+i].capture_time	= ros::Time();
+		msg.frames[startsize+i].pose			= pose2;
+		msg.frames[startsize+i].frame_id		= model->frames[i]->id;
+		msg.frames[startsize+i].rgb				= *(rgbBridgeImage.toImageMsg());
+		msg.frames[startsize+i].depth			= *(depthBridgeImage.toImageMsg());
+		msg.masks[startsize+i]					= *(maskBridgeImage.toImageMsg());
+
+		msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx;
+		msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy;
+		msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx;
+		msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy;
+
+		sensor_msgs::PointCloud2 output;
+		if(addClouds){pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output);}
+		msg.clouds.push_back(output);
+	}
+	for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){
+		addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp,addClouds);
+	}
+}
+
+quasimodo_msgs::model getModelMSG(reglib::Model * model, bool addClouds){
+	quasimodo_msgs::model msg;
+	msg.model_id = model->id;
+	addToModelMSG(msg,model,Eigen::Affine3d::Identity(),addClouds);
+	return msg;
+}
+
+std::vector<Eigen::Matrix4f> getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms){
+	std::vector<Eigen::Matrix4f> toRet;
+	ifstream in(poses_file);
+	if (!in.is_open()){
+		cout<<"ERROR: cannot find poses file "<<poses_file<<endl;
+		return toRet;
+	}
+	cout<<"Loading additional view registered poses from "<<poses_file<<endl;
+
+	for (int i=0; i<no_transforms+1; i++){
+		Eigen::Matrix4f transform;
+		float temp;
+		for (size_t j=0; j<4; j++){
+			for (size_t k=0; k<4; k++){
+				in >> temp;
+				transform(j,k) = temp;
+			}
+		}
+		toRet.push_back(transform);
+	}
+	return toRet;
+}
+
+Eigen::Matrix4d getMat(tf::StampedTransform tf){
+
+	//printf("getMat\n");
+	//Transform
+	geometry_msgs::TransformStamped tfstmsg;
+	tf::transformStampedTFToMsg (tf, tfstmsg);
+	geometry_msgs::Transform tfmsg = tfstmsg.transform;
+	geometry_msgs::Pose		pose;
+	pose.orientation		= tfmsg.rotation;
+	pose.position.x		= tfmsg.translation.x;
+	pose.position.y		= tfmsg.translation.y;
+	pose.position.z		= tfmsg.translation.z;
+	Eigen::Affine3d epose;
+	tf::poseMsgToEigen(pose, epose);
+
+	//std::cout << epose.matrix() << std::endl << std::endl;
+	return epose.matrix();
+}
+
+reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath){
+	int slash_pos = sweep_xml.find_last_of("/");
+	std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/";
+	//	printf("load_metaroom_model(%s)\n",sweep_folder.c_str());
+
+	SimpleXMLParser<pcl::PointXYZRGB> parser;
+	std::vector<std::string> dummy;
+	dummy.push_back("RoomIntermediateCloud");
+	dummy.push_back("IntermediatePosition");
+	SimpleXMLParser<pcl::PointXYZRGB>::RoomData roomData  = parser.loadRoomFromXML(sweep_folder+"/room.xml",dummy);
+	printf("loaded room XML: %s\n",(sweep_folder+"/room.xml").c_str());
+
+	if(roomData.vIntermediateRoomClouds.size() == 0 ){
+		printf("WARNING:: no clouds in room\n");
+		return new reglib::Model();
+	}
+
+	reglib::Model * sweepmodel = 0;
+	printf("%i\n",roomData.vIntermediateRoomCloudTransforms.size());
+	Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]);
+	std::vector<reglib::RGBDFrame * > current_room_frames;
+
+
+	//	printf("roomData.vIntermediateRoomClouds.size() = %i\n",roomData.vIntermediateRoomClouds.size());
+	//	printf("roomData.vIntermediateRoomCloudCamParamsCorrected.size() = %i\n",roomData.vIntermediateRoomCloudCamParamsCorrected.size());
+	//	printf("roomData.vIntermediateRoomCloudTransformsRegistered.size() = %i\n",roomData.vIntermediateRoomCloudTransformsRegistered.size());
+
+	for (size_t i=0; i<roomData.vIntermediateRoomClouds.size(); i++){
+		//printf("loading intermedite %i\n",i);
+		cv::Mat fullmask;
+		fullmask.create(480,640,CV_8UC1);
+		unsigned char * maskdata = (unsigned char *)fullmask.data;
+		for(int j = 0; j < 480*640; j++){maskdata[j] = 255;}
+
+		image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected[i];
+		//image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParams[i];
+		reglib::Camera * cam		= new reglib::Camera();
+		cam->fx = aCameraModel.fx();
+		cam->fy = aCameraModel.fy();
+		cam->cx = aCameraModel.cx();
+		cam->cy = aCameraModel.cy();
+		//cam->print();
+
+		Eigen::Matrix4d m = m2*getMat(roomData.vIntermediateRoomCloudTransformsRegistered[i]);
+		//Eigen::Matrix4d m =      getMat(roomData.vIntermediateRoomCloudTransforms[i]);
+
+		reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m,true,savePath);
+
+		current_room_frames.push_back(frame);
+		if(sweepmodel == 0){
+			sweepmodel = new reglib::Model(frame,fullmask);
+		}else{
+			sweepmodel->frames.push_back(frame);
+			sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose);
+			sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask));
+		}
+	}
+
+
+	sweepmodel->recomputeModelPoints();
+
+	return sweepmodel;
+}
+
+void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg, std::string savePath){
+	double startTime = getTime();
+	printf("running segment method\n");
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+	if(debugg){
+		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("viewer"));
+		viewer->addCoordinateSystem(0.01);
+		viewer->setBackgroundColor(1.0,1.0,1.0);
+	}
+
+	reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.15);
+	if(savePath.size() != 0){
+		massregmod->savePath = savePath+"/segment_"+std::to_string(models.front()->id);
+	}
+	massregmod->timeout = 1200;
+	massregmod->viewer = viewer;
+	massregmod->visualizationLvl = debugg > 1;
+	massregmod->maskstep = 6;//std::max(1,int(0.4*double(models[i]->frames.size())));
+	massregmod->nomaskstep = 6;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size())));
+	massregmod->nomask = true;
+	massregmod->stopval = 0.0001;
+
+	reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+	reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( models.front(), reg);
+	mu->occlusion_penalty               = 15;
+	mu->massreg_timeout                 = 60*4;
+	mu->viewer							= viewer;
+	printf("total segment part1 time: %5.5fs\n",getTime()-startTime);
+
+	std::vector<Eigen::Matrix4d> cpmod;
+	if(models.size() > 0 && bgs.size() > 0){
+		for(unsigned int i = 0; i < bgs.size(); i++){
+			if(bgs[i]->points.size() == 0){bgs[i]->recomputeModelPoints();}
+			cpmod.push_back(bgs.front()->frames.front()->pose.inverse() * bgs[i]->frames.front()->pose);
+			massregmod->addModel(bgs[i]);
+		}
+
+		for(int j = 0; j < models.size(); j++){
+			if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();}
+			cpmod.push_back(bgs.front()->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front());
+			massregmod->addModel(models[j]);
+		}
+		printf("total segment part1b time: %5.5fs\n",getTime()-startTime);
+		reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod);
+
+		for(unsigned int i = 0; i < bgs.size(); i++){cpmod[i] = mfrmod.poses[i];}
+
+		for(unsigned int j = 0; j < models.size(); j++){
+			Eigen::Matrix4d change = mfrmod.poses[j+bgs.size()];
+			for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){
+				models[j]->relativeposes[k] = change*models[j]->relativeposes[k];
+			}
+			for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){
+				models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k];
+			}
+		}
+	}else if(models.size() > 1){
+		for(unsigned int j = 0; j < models.size(); j++){
+			if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();}
+			cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front());
+			massregmod->addModel(models[j]);
+		}
+		reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod);
+		for(unsigned int j = 0; j < models.size(); j++){
+			Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse();
+
+			for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){
+				models[j]->relativeposes[k] = change*models[j]->relativeposes[k];
+			}
+
+			for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){
+				models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k];
+			}
+		}
+	}
+	delete massregmod;
+
+	printf("total segment part2 time: %5.5fs\n",getTime()-startTime);
+
+	std::vector<Eigen::Matrix4d> bgcp;
+	std::vector<reglib::RGBDFrame*> bgcf;
+	std::vector<cv::Mat> bgmask;
+	for(unsigned int i = 0; i < bgs.size(); i++){
+		Eigen::Matrix4d cp = cpmod[i];
+		std::cout << cp << std::endl;
+		for(unsigned int k = 0; k < bgs[i]->relativeposes.size(); k++){
+			bgcp.push_back(cp * bgs[i]->relativeposes[k]);
+			bgcf.push_back(bgs[i]->frames[k]);
+			bgmask.push_back(bgs[i]->modelmasks[k]->getMask());
+		}
+	}
+	for(unsigned int j = 0; j < models.size(); j++){
+		reglib::Model * model = models[j];
+
+		std::vector<cv::Mat> masks;
+		for(unsigned int i = 0; i < model->frames.size(); i++){
+			reglib::RGBDFrame * frame = model->frames[i];
+			reglib::Camera * cam = frame->camera;
+			cv::Mat mask;
+			mask.create(cam->height,cam->width,CV_8UC1);
+			unsigned char * maskdata = (unsigned char *)(mask.data);
+			for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;}
+			masks.push_back(mask);
+		}
+
+		std::vector<cv::Mat> movemask;
+		std::vector<cv::Mat> dynmask;
+		printf("computeMovingDynamicStatic\n");
+		mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg,savePath);//Determine self occlusions
+		external.push_back(movemask);
+		internal.push_back(masks);
+		dynamic.push_back(dynmask);
+	}
+
+	delete reg;
+	delete mu;
+	printf("total segment time: %5.5fs\n",getTime()-startTime);
+}
+
+/*
+void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){
+	double startTime = getTime();
+	printf("running segment method\n");
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+	if(debugg){
+		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("viewer"));
+		viewer->addCoordinateSystem(0.01);
+		viewer->setBackgroundColor(1.0,1.0,1.0);
+	}
+
+	reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05);
+	massregmod->timeout = 1200;
+	massregmod->viewer = viewer;
+	massregmod->visualizationLvl = 1;
+	massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size())));
+	massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size())));
+	massregmod->nomask = true;
+	massregmod->stopval = 0.0001;
+
+	reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+	reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( models.front(), reg);
+	mu->occlusion_penalty               = 15;
+	mu->massreg_timeout                 = 60*4;
+	mu->viewer							= viewer;
+	printf("total segment part1 time: %5.5fs\n",getTime()-startTime);
+
+	if(models.size() > 0 && bg->frames.size() > 0){
+		std::vector<Eigen::Matrix4d> cpmod;
+		if(bg->points.size() == 0){bg->recomputeModelPoints();}
+
+		cpmod.push_back(Eigen::Matrix4d::Identity());
+		massregmod->addModel(bg);
+
+		for(int j = 0; j < models.size(); j++){
+			if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();}
+			cpmod.push_back(bg->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front());
+			massregmod->addModel(models[j]);
+		}
+		printf("total segment part1b time: %5.5fs\n",getTime()-startTime);
+		reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod);
+		for(int j = 0; j < models.size(); j++){
+			Eigen::Matrix4d change = mfrmod.poses[j+1];
+			for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){
+				models[j]->relativeposes[k] = change*models[j]->relativeposes[k];
+			}
+			for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){
+				models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k];
+			}
+		}
+	}else if(models.size() > 1){
+		std::vector<Eigen::Matrix4d> cpmod;
+		for(int j = 0; j < models.size(); j++){
+			if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();}
+			cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front());
+			massregmod->addModel(models[j]);
+		}
+		reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod);
+		for(int j = 0; j < models.size(); j++){
+			Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse();
+
+			for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){
+				models[j]->relativeposes[k] = change*models[j]->relativeposes[k];
+			}
+
+			for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){
+				models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k];
+			}
+		}
+	}
+	delete massregmod;
+
+	printf("total segment part2 time: %5.5fs\n",getTime()-startTime);
+
+	std::vector<Eigen::Matrix4d> bgcp;
+	std::vector<reglib::RGBDFrame*> bgcf;
+	std::vector<cv::Mat> bgmask;
+	for(unsigned int k = 0; k < bg->relativeposes.size(); k++){
+		bgcp.push_back(bg->relativeposes[k]);
+		bgcf.push_back(bg->frames[k]);
+		bgmask.push_back(bg->modelmasks[k]->getMask());
+	}
+	for(int j = 0; j < models.size(); j++){
+		reglib::Model * model = models[j];
+
+		std::vector<cv::Mat> masks;
+		for(unsigned int i = 0; i < model->frames.size(); i++){
+			reglib::RGBDFrame * frame = model->frames[i];
+			reglib::Camera * cam = frame->camera;
+			cv::Mat mask;
+			mask.create(cam->height,cam->width,CV_8UC1);
+			unsigned char * maskdata = (unsigned char *)(mask.data);
+			for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;}
+			masks.push_back(mask);
+		}
+
+		std::vector<cv::Mat> movemask;
+		std::vector<cv::Mat> dynmask;
+		printf("computeMovingDynamicStatic\n");
+		mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions
+		external.push_back(movemask);
+		internal.push_back(masks);
+		dynamic.push_back(dynmask);
+	}
+
+	delete reg;
+	delete mu;
+	printf("total segment time: %5.5fs\n",getTime()-startTime);
+}
+*/
+
+std::vector<std::string> getFileList(std::string path){
+	std::vector<std::string> files;
+
+	DIR *dp;
+	struct dirent *ep;
+	dp = opendir (path.c_str());
+	if (dp != NULL) {
+		while (ep = readdir (dp)){
+			if (ep->d_type != DT_DIR) {
+				files.push_back(std::string(ep->d_name));
+			}
+		}
+		closedir (dp);
+	}else{
+		printf("Couldn't open %s\n",path.c_str());
+	}
+
+	return files;
+}
+
+std::vector<std::string> getFolderList(std::string path){
+	std::vector<std::string> files;
+
+	DIR *dp;
+	struct dirent *ep;
+	dp = opendir (path.c_str());
+	if (dp != NULL) {
+		while (ep = readdir (dp)){
+			std::string data = std::string(ep->d_name);
+			if (data.compare(".") != 0 && data.compare("..") != 0 && ep->d_type == DT_DIR) {
+				files.push_back(data);
+			}
+		}
+		closedir (dp);
+	}else{
+		printf("Couldn't open %s\n",path.c_str());
+	}
+	return files;
+}
+
+void recursiveGetFolderList(std::vector<std::string> & folders, std::string path){
+	std::vector<std::string> list = getFolderList(path);
+	for(unsigned int i = 0; i < list.size(); i++){
+		folders.push_back(path+"/"+list[i]);
+		recursiveGetFolderList(folders, path+"/"+list[i]);
+	}
+}
+
+std::vector<std::string> recursiveGetFolderList(std::string path){
+	std::vector<std::string> folders;
+	recursiveGetFolderList(folders, path);
+	return folders;
+}
+
+Eigen::Matrix4f getRegisteredViewPoses(const std::string& poses_file){
+	Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+	std::ifstream in(poses_file);
+	if (!in.is_open()){
+		cout<<"ERROR: cannot find poses file "<<poses_file<<endl;
+		return transform;
+	}
+	//cout<<"Loading view pose from "<<poses_file<<endl;
+
+	float temp;
+	for (size_t j=0; j<4; j++){
+		for (size_t k=0; k<4; k++){
+			in >> temp;
+			transform(j,k) = temp;
+		}
+	}
+	in.close();
+	return transform;
+}
+
+std::vector<int> getIndsFromFile(const std::string& poses_file){
+	std::vector<int> indexes;
+	std::ifstream in(poses_file);
+	if (!in.is_open()){
+		cout<<"ERROR: cannot find idnex file "<<poses_file<<endl;
+		return indexes;
+	}
+	//cout<<"Loading indexes from "<<poses_file<<endl;
+
+	std::string line;
+	while ( getline (in,line) ){
+		indexes.push_back(std::stoi(line));
+	}
+	in.close();
+
+	return indexes;
+}
+
+
+
+
+std::vector<reglib::Model *> loadModelsPCDs(std::string path){
+
+//	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("Modelserver Viewer"));
+//	viewer->addCoordinateSystem(0.1);
+//	viewer->setBackgroundColor(1.0,0.0,1.0);
+
+
+	std::vector<reglib::Model *> models;
+
+	std::vector<std::string> folders = getFolderList(path);//recursiveGetFolderList(path);//getFolderList(path);//
+	for(unsigned int i = 0; i < folders.size(); i++){
+		printf("folders: %s\n",folders[i].c_str());
+		std::string views = path+folders[i]+"/views";
+		std::vector<std::string> files = getFileList(views);
+
+		std::vector<std::string> cloudspaths;
+		std::vector<std::string> indexpaths;
+		std::vector<std::string> posepaths;
+
+		for(unsigned int j = 0; j < files.size(); j++){
+			std::string file = views+"/"+files[j];
+			printf("Files: %s\n",file.c_str());
+			if ((files[j].find("cloud") != std::string::npos) && (files[j].find(".pcd") !=std::string::npos)){cloudspaths.push_back(file);}
+			if (files[j].find("indices") != std::string::npos){indexpaths.push_back(file);}
+			if (files[j].find("pose") != std::string::npos){posepaths.push_back(file);}
+		}
+
+		if((cloudspaths.size() == indexpaths.size()) && (cloudspaths.size() == posepaths.size())){
+			std::sort (cloudspaths.begin(), cloudspaths.end());
+			std::sort (indexpaths.begin(), indexpaths.end());
+			std::sort (posepaths.begin(), posepaths.end());
+
+
+			reglib::Model * model = new reglib::Model();
+			for(unsigned int j =  0; j  < cloudspaths.size(); j++){
+				std::string cloudpath = cloudspaths[j];
+				std::string indexpath = indexpaths[j];
+				std::string posepath = posepaths[j];
+				//printf("loading: %s %s %s\n", cloudpath.c_str(),indexpath.c_str(),posepath.c_str());
+
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+				if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (cloudpath, *cloud) != -1){
+
+//					viewer->removeAllPointClouds();
+//					viewer->addPointCloud<pcl::PointXYZRGB> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(cloud), "cloud");
+//					viewer->spin();
+
+					Eigen::Matrix4f pose = getRegisteredViewPoses(posepath);
+					std::vector<int> indexes = getIndsFromFile(indexpath);
+//					std::cout << pose << std::endl << std::endl;
+//					std::cout << indexes.size() << std::endl;
+
+					unsigned int width = cloud->width;
+					unsigned int height = cloud->height;
+					cv::Mat rgb;
+					rgb.create(height,width,CV_8UC3);
+					unsigned char   * rgbdata   = rgb.data;
+
+					cv::Mat depth;
+					depth.create(height,width,CV_16UC1);
+					unsigned short  * depthdata = (unsigned short *)(depth.data);
+
+					reglib::Camera * cam = new reglib::Camera();//figure out cam params
+					cam->fx = 525;
+					cam->fy = 525;
+
+					for(unsigned int k = 0; k < width*height; k++){
+						pcl::PointXYZRGB p = cloud->points[k];
+						rgbdata[3*k+0] = p.b;
+						rgbdata[3*k+1] = p.g;
+						rgbdata[3*k+2] = p.r;
+						if(!std::isnan(p.z)){
+							depthdata[k] = 5000*p.z;
+						}else{
+							depthdata[k] = 0;
+						}
+					}
+
+					cv::Mat mask;
+					mask.create(height,width,CV_8UC1);
+					unsigned char   * maskdata   = mask.data;
+
+					for(unsigned int k = 0; k < height*width; k++){maskdata[k] = 0;}
+					for(unsigned int k = 0; k < indexes.size(); k++){maskdata[indexes[k]] = 255;}
+
+
+//					cv::imshow( "mask", mask );
+//					cv::waitKey(0);
+
+					reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, 0 , pose.cast<double>() , true,"",false);
+					frame->show(false);
+					model->frames.push_back(frame);
+					model->relativeposes.push_back(model->frames.front()->pose.inverse() * pose.cast<double>());
+					model->modelmasks.push_back(new reglib::ModelMask(mask));
+					model->modelmasks.back()->label = folders[i];
+				}else{
+					printf ("Couldn't read pcd file\n");
+				}
+			}
+			model->recomputeModelPoints();//model->recomputeModelPoints(Eigen::Matrix4d::Identity(),viewer);
+			models.push_back(model);
+		}else{
+			printf("number of clouds, indices and poses dont match... ignoring\n");
+		}
+	}
+
+	return models;
+}
+
+}
diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h
new file mode 100644
index 00000000..21f11705
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/Util/Util.h
@@ -0,0 +1,110 @@
+#ifndef brainUtil_H
+#define brainUtil_H
+
+#include <thread>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <dirent.h>
+#include <errno.h>
+#include <vector>
+#include <string>
+#include <iostream>
+
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+
+
+#include "quasimodo_msgs/segment_model.h"
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+#include <metaroom_xml_parser/load_utilities.h>
+
+#include <ros/ros.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <soma_llsd/GetScene.h>
+#include <soma_llsd/GetSegment.h>
+#include <soma_llsd/InsertScene.h>
+#include <soma_llsd_msgs/Segment.h>
+#include <quasimodo_conversions/conversions.h>
+
+namespace quasimodo_brain {
+
+bool fileExists(std::string path);
+int getdir (std::string dir, std::vector<std::string> & files);
+std::string getPoseString(Eigen::Matrix4d pose);
+
+Eigen::Matrix4d getPoseFromString(std::string str);
+
+std::string initSegment(ros::NodeHandle& n, reglib::Model * model);
+reglib::Model * getModelFromSegment(ros::NodeHandle& n, std::string segment_id);
+
+soma_llsd_msgs::Scene getScene(ros::NodeHandle& n, reglib::RGBDFrame * frame, std::string current_waypointid = "", std::string roomRunNumber = "");
+
+reglib::Camera * getCam(sensor_msgs::CameraInfo & info);
+reglib::RGBDFrame * getFrame(soma_llsd_msgs::Scene & scene);
+
+
+std::vector<reglib::superpoint> getSuperPoints(std::string path);
+
+std::vector<reglib::superpoint> getRoomSuperPoints(std::string path, std::string savePath);
+
+void transformSuperPoints(std::vector<reglib::superpoint> & spvec, Eigen::Matrix4d cp);
+
+void saveSuperPoints(std::string path, std::vector<reglib::superpoint> & spvec, Eigen::Matrix4d pose, float ratio_keep = 0.1);
+
+std::vector<Eigen::Matrix4d> readPoseXML(std::string xmlFile);
+
+void savePoses(std::string xmlFile, std::vector<Eigen::Matrix4d> poses, int maxposes = -1);
+
+Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader);
+
+int readNumberOfViews(std::string xmlFile);
+
+void writeXml(std::string xmlFile, std::vector<reglib::RGBDFrame *> & frames, std::vector<Eigen::Matrix4d> & poses);
+
+void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose);
+
+void remove_old_seg(std::string sweep_folder);
+
+std::string replaceAll(std::string str, std::string from, std::string to);
+
+void cleanPath(std::string & path);
+void sortXMLs(std::vector<std::string> & sweeps);
+
+std::vector<Eigen::Matrix4f> getRegisteredViewPosesFromFile(std::string poses_file, int no_transforms);
+reglib::Model * loadFromRaresFormat(std::string path);
+
+double getTime();
+reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg, bool compute_edges = true);
+
+void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp = Eigen::Affine3d::Identity(), bool addClouds = false);
+quasimodo_msgs::model getModelMSG(reglib::Model * model, bool addClouds = false);
+
+std::vector<Eigen::Matrix4f> getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms);
+
+Eigen::Matrix4d getMat(tf::StampedTransform tf);
+reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath = "");
+
+void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg = 0, std::string savePath = "");
+std::vector<reglib::Model *> loadModelsXML(std::string path);
+std::vector<reglib::Model *> loadModelsPCDs(std::string path);
+
+}
+
+#endif
diff --git a/quasimodo/quasimodo_brain/src/massregPCD.cpp b/quasimodo/quasimodo_brain/src/massregPCD.cpp
index 71fe4d34..496ee58a 100644
--- a/quasimodo/quasimodo_brain/src/massregPCD.cpp
+++ b/quasimodo/quasimodo_brain/src/massregPCD.cpp
@@ -15,9 +15,9 @@
 #include "quasimodo_msgs/retrieval_query_result.h"
 #include "quasimodo_msgs/retrieval_query.h"
 #include "quasimodo_msgs/retrieval_result.h"
-#include "soma2_msgs/SOMA2Object.h"
+#include "soma_msgs/SOMAObject.h"
 
-#include "soma_manager/SOMA2InsertObjs.h"
+#include "soma_manager/SOMAInsertObjs.h"
 
 //#include "modelupdater/ModelUpdater.h"
 //#include "/home/johane/catkin_ws_dyn/src/quasimodo_models/include/modelupdater/ModelUpdater.h"
diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp
new file mode 100644
index 00000000..cbedbcf3
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp
@@ -0,0 +1,2415 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <string.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+#include "metaroom_xml_parser/simple_summary_parser.h"
+#include <metaroom_xml_parser/load_utilities.h>
+
+#include <pcl_ros/point_cloud.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+#include "quasimodo_msgs/segment_model.h"
+#include "quasimodo_msgs/metaroom_pair.h"
+#include <quasimodo_msgs/query_cloud.h>
+#include <quasimodo_msgs/visualize_query.h>
+
+#include <image_geometry/pinhole_camera_model.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+#include "Util/Util.h"
+
+#include <observation_registration_services/ObjectAdditionalViewRegistrationService.h>
+
+#include <sys/time.h>
+#include <sys/resource.h>
+
+#include "object_manager/dynamic_object.h"
+#include "object_manager/dynamic_object_xml_parser.h"
+#include "object_manager/dynamic_object_utilities.h"
+#include "object_manager/dynamic_object_mongodb_interface.h"
+#include <object_manager_msgs/DynamicObjectTracks.h>
+#include <object_manager_msgs/DynamicObjectTrackingData.h>
+#include <object_manager_msgs/DynamicObjectComputeMaskService.h>
+#include <object_manager_msgs/DynamicObjectsService.h>
+#include <object_manager_msgs/GetDynamicObjectService.h>
+#include <object_manager_msgs/ProcessDynamicObjectService.h>
+
+#include <semantic_map_msgs/RoomObservation.h>
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <signal.h>
+
+#include <quasimodo_conversions/conversions.h>
+
+#include <soma_llsd/GetScene.h>
+#include <soma_llsd/InsertScene.h>
+
+#include <metaroom_detections/metaroom_detections.h>
+#include <iostream>
+#include <tuple>
+
+std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers;
+std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers;
+
+typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest;
+typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse;
+
+typedef typename object_manager_msgs::GetDynamicObjectService::Request GetDynamicObjectServiceRequest;
+typedef typename object_manager_msgs::GetDynamicObjectService::Response GetDynamicObjectServiceResponse;
+
+typedef typename object_manager_msgs::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest;
+typedef typename object_manager_msgs::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse;
+
+ros::ServiceClient segmentation_client;
+using namespace std;
+using namespace semantic_map_load_utilties;
+
+typedef pcl::PointXYZRGB PointType;
+typedef pcl::PointCloud<PointType> Cloud;
+typedef typename Cloud::Ptr CloudPtr;
+typedef pcl::search::KdTree<PointType> Tree;
+typedef semantic_map_load_utilties::DynamicObjectData<PointType> ObjectData;
+
+using namespace std;
+using namespace semantic_map_load_utilties;
+
+int	minClusterSize = 2000;
+std::string overall_folder = "";
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+int visualization_lvl			= 0;
+int visualization_lvl_regref	= 0;
+int visualization_lvl_regini	= 0;
+std::string outtopic			= "";
+std::string modelouttopic		= "";
+ros::Publisher out_pub;
+ros::Publisher model_pub;
+
+std::vector< ros::Publisher > m_PublisherStatuss;
+std::vector< ros::Publisher > out_pubs;
+std::vector< ros::Publisher > model_pubs;
+std::vector< ros::Publisher > soma_segment_id_pubs;
+ros::Publisher roomObservationCallback_pubs;
+
+std::string saveVisuals = "";
+
+std::string posepath = "testposes.xml";
+std::vector<Eigen::Matrix4d> sweepPoses;
+reglib::Camera * basecam;
+
+ros::NodeHandle * np;
+
+bool recomputeRelativePoses = false;
+bool do_last_and_send = true;
+
+
+void sendMetaroomToServer(std::string path);
+bool testDynamicObjectServiceCallback(std::string path);
+bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res);
+void processSweepForDatabase(std::string path, std::string savePath);
+
+void readViewXML(std::string xmlFile, std::vector<reglib::RGBDFrame *> & frames, std::vector<Eigen::Matrix4d> & poses, bool compute_edges = true, std::string savePath = ""){
+	QFile file(xmlFile.c_str());
+	if (!file.exists()){
+		ROS_ERROR("Could not open file %s to load room.",xmlFile.c_str());
+		return;
+	}
+
+	QString xmlFileQS(xmlFile.c_str());
+	int index = xmlFileQS.lastIndexOf('/');
+	std::string roomFolder = xmlFileQS.left(index).toStdString();
+
+	file.open(QIODevice::ReadOnly);
+	ROS_INFO_STREAM("Parsing xml file: "<<xmlFile.c_str());
+
+	QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+
+	while (!xmlReader->atEnd() && !xmlReader->hasError()){
+		QXmlStreamReader::TokenType token = xmlReader->readNext();
+		if (token == QXmlStreamReader::StartDocument)
+			continue;
+
+		if (xmlReader->hasError()){
+			ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str());
+			return;
+		}
+
+		QString elementName = xmlReader->name().toString();
+
+		if (token == QXmlStreamReader::StartElement){
+			if (xmlReader->name() == "View"){
+				cv::Mat rgb;
+				cv::Mat depth;
+				//printf("elementName: %s\n",elementName.toStdString().c_str());
+				QXmlStreamAttributes attributes = xmlReader->attributes();
+				if (attributes.hasAttribute("RGB")){
+					std::string imgpath = attributes.value("RGB").toString().toStdString();
+					rgb = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED);
+				}else{break;}
+
+				if (attributes.hasAttribute("DEPTH")){
+					std::string imgpath = attributes.value("DEPTH").toString().toStdString();
+					depth = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED);
+				}else{break;}
+
+
+				token = xmlReader->readNext();//Stamp
+				elementName = xmlReader->name().toString();
+
+				token = xmlReader->readNext();//sec
+				elementName = xmlReader->name().toString();
+				int sec = atoi(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//nsec
+				elementName = xmlReader->name().toString();
+				int nsec = atoi(xmlReader->readElementText().toStdString().c_str());
+				token = xmlReader->readNext();//end stamp
+
+				token = xmlReader->readNext();//Camera
+				elementName = xmlReader->name().toString();
+
+				reglib::Camera * cam = new reglib::Camera();
+
+				token = xmlReader->readNext();//fx
+				cam->fx = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//fy
+				cam->fy = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//cx
+				cam->cx = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//cy
+				cam->cy = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//Camera
+				elementName = xmlReader->name().toString();
+
+				double time = double(sec)+double(nsec)/double(1e9);
+
+				token = xmlReader->readNext();//RegisteredPose
+				elementName = xmlReader->name().toString();
+
+				Eigen::Matrix4d regpose = quasimodo_brain::getPose(xmlReader);
+
+				token = xmlReader->readNext();//RegisteredPose
+				elementName = xmlReader->name().toString();
+
+
+				token = xmlReader->readNext();//Pose
+				elementName = xmlReader->name().toString();
+
+				Eigen::Matrix4d pose = quasimodo_brain::getPose(xmlReader);
+
+				token = xmlReader->readNext();//Pose
+				elementName = xmlReader->name().toString();
+
+				reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, regpose,true,savePath,compute_edges);
+				frames.push_back(frame);
+				poses.push_back(pose);
+			}
+		}
+	}
+	delete xmlReader;
+}
+
+
+void setBaseSweep(std::string path){
+	printf("setBaseSweep(%s)\n",path.c_str());
+	SimpleXMLParser<pcl::PointXYZRGB> parser;
+	SimpleXMLParser<pcl::PointXYZRGB>::RoomData roomData  = parser.loadRoomFromXML(path);//, std::vector<std::string>(),false,false);
+	printf("loaded room data\n");
+	image_geometry::PinholeCameraModel baseCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected.front();
+	if(basecam != 0){delete basecam;}
+	basecam		= new reglib::Camera();
+	basecam->fx = baseCameraModel.fx();
+	basecam->fy = baseCameraModel.fy();
+	basecam->cx = baseCameraModel.cx();
+	basecam->cy = baseCameraModel.cy();
+
+	sweepPoses.clear();
+	for (size_t i=0; i<roomData.vIntermediateRoomCloudTransformsRegistered.size(); i++){
+		sweepPoses.push_back(quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransformsRegistered[i]));
+		std::cout << "sweepPoses" << i << std::endl;
+		std::cout << sweepPoses.back() << std::endl;
+	}
+}
+
+reglib::Model * processAV(std::string path, bool compute_edges = true, std::string savePath = ""){
+	printf("processAV: %s\n",path.c_str());
+
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+	std::vector<cv::Mat> viewrgbs;
+	std::vector<cv::Mat> viewdepths;
+	std::vector<tf::StampedTransform > viewtfs;
+
+	QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml"));
+	for (auto objectFile : objectFiles){
+		auto object = loadDynamicObjectFromSingleSweep<PointType>(sweep_folder+objectFile.toStdString(),false);
+		for (unsigned int i=0; i<object.vAdditionalViews.size(); i++){
+			CloudPtr cloud = object.vAdditionalViews[i];
+
+			cv::Mat rgb;
+			rgb.create(cloud->height,cloud->width,CV_8UC3);
+			unsigned char * rgbdata = (unsigned char *)rgb.data;
+
+			cv::Mat depth;
+			depth.create(cloud->height,cloud->width,CV_16UC1);
+			unsigned short * depthdata = (unsigned short *)depth.data;
+
+			unsigned int nr_data = cloud->height * cloud->width;
+			for(unsigned int j = 0; j < nr_data; j++){
+				PointType p = cloud->points[j];
+				rgbdata[3*j+0]	= p.b;
+				rgbdata[3*j+1]	= p.g;
+				rgbdata[3*j+2]	= p.r;
+				depthdata[j]	= short(5000.0 * p.z);
+			}
+
+			viewrgbs.push_back(rgb);
+			viewdepths.push_back(depth);
+			viewtfs.push_back(object.vAdditionalViewsTransforms[i]);
+		}
+	}
+
+	reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath);
+	if(sweep->frames.size() == 0){
+		printf("no frames in sweep, returning\n");
+		return sweep;
+	}
+
+	for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){
+		sweep->frames[i]->pose	= sweep->frames.front()->pose * sweepPoses[i];
+		sweep->relativeposes[i] = sweepPoses[i];
+		if(basecam != 0){
+			delete sweep->frames[i]->camera;
+			sweep->frames[i]->camera = basecam->clone();
+		}
+	}
+
+	std::vector<reglib::RGBDFrame *> frames;
+	std::vector<reglib::ModelMask *> masks;
+	std::vector<Eigen::Matrix4d> unrefined;
+
+	std::vector<Eigen::Matrix4d> both_unrefined;
+	both_unrefined.push_back(Eigen::Matrix4d::Identity());
+	std::vector<double> times;
+	for(unsigned int i = 0; i < 3000 &&  i < viewrgbs.size(); i++){
+		printf("additional view: %i\n",i);
+		geometry_msgs::TransformStamped msg;
+		tf::transformStampedTFToMsg(viewtfs[i], msg);
+		long sec = msg.header.stamp.sec;
+		long nsec = msg.header.stamp.nsec;
+		double time = double(sec)+1e-9*double(nsec);
+
+		Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]);
+
+		cout << m << endl << endl;
+
+		unrefined.push_back(m);
+		times.push_back(time);
+
+		cv::Mat fullmask;
+		fullmask.create(480,640,CV_8UC1);
+		unsigned char * maskdata = (unsigned char *)fullmask.data;
+		for(int j = 0; j < 480*640; j++){maskdata[j] = 255;}
+		masks.push_back(new reglib::ModelMask(fullmask));
+
+		reglib::Camera * cam		= sweep->frames.front()->camera->clone();
+		reglib::RGBDFrame * frame	= new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m,true,savePath);//a.matrix());
+		frames.push_back(frame);
+
+		both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m);
+	}
+
+	reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+	reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( sweep, reg);
+	mu->occlusion_penalty               = 15;
+	mu->massreg_timeout                 = 60*4;
+	mu->viewer							= viewer;
+
+	sweep->recomputeModelPoints();
+	reglib::Model * fullmodel = new reglib::Model();
+	fullmodel->savePath = savePath+"/";
+	fullmodel->frames = sweep->frames;
+	fullmodel->relativeposes = sweep->relativeposes;
+	fullmodel->modelmasks = sweep->modelmasks;
+	//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
+	if(frames.size() > 0){
+
+		reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement();
+		refinement->viewer	= viewer;
+		refinement->visualizationLvl	= visualization_lvl_regini;
+		refinement->normalize_matchweights = false;
+		refinement->target_points = 4000;
+		////double register_setup_start = getTime();
+
+		//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
+		reglib::CloudData * cd1 = sweep->getCD(sweep->points.size());
+		refinement->setDst(cd1);
+
+		fullmodel->points = frames.front()->getSuperPoints(Eigen::Matrix4d::Identity(),1,false);
+		reglib::CloudData * cd2	= fullmodel->getCD(fullmodel->points.size());
+		refinement->setSrc(cd2);
+		//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
+		//////printf("register_setup_start:          %5.5f\n",getTime()-register_setup_start);
+		////double register_compute_start = getTime();
+		Eigen::Matrix4d guess = both_unrefined[1]*both_unrefined[0].inverse();
+
+		reglib::FusionResults fr = refinement->getTransform(guess);
+		Eigen::Matrix4d offset = fr.guess*guess.inverse();
+		for(unsigned int i = 1; i < both_unrefined.size(); i++){
+			both_unrefined[i] = offset*both_unrefined[i];
+		}
+
+		delete refinement;
+
+		fullmodel->points.clear();
+		delete cd1;
+		delete cd2;
+
+		reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25);
+		if(savePath.size() != 0){
+			bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id);
+		}
+		//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
+
+		bgmassreg->timeout = 300;
+		bgmassreg->viewer = viewer;
+		bgmassreg->use_surface = true;
+		bgmassreg->use_depthedge = false;
+		bgmassreg->visualizationLvl = visualization_lvl_regref;
+		bgmassreg->maskstep = 5;
+		bgmassreg->nomaskstep = 5;
+		bgmassreg->nomask = true;
+		bgmassreg->stopval = 0.0005;
+		bgmassreg->addModel(sweep);
+		bgmassreg->setData(frames,masks);
+		//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
+
+		reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined);
+
+		delete bgmassreg;
+
+		for(unsigned int i = 0; i < frames.size(); i++){frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1];}
+
+		for(unsigned int i = 0; i < frames.size(); i++){
+			fullmodel->frames.push_back(frames[i]);
+			fullmodel->modelmasks.push_back(masks[i]);
+			fullmodel->relativeposes.push_back(bgmfr.poses[i+1]);
+		}
+		fullmodel->recomputeModelPoints();
+
+		if(savePath.size() != 0){
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cld = fullmodel->getPCLnormalcloud(1, false);
+			pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld);
+		}
+		//pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr Model::getPCLnormalcloud(int step, bool color){
+
+	}else{
+		fullmodel->points = sweep->points;
+	}
+
+	delete reg;
+	delete mu;
+	delete sweep;
+
+	return fullmodel;
+}
+
+reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std::string saveVisuals_sp = ""){
+	printf("getAVMetaroom: %s\n",path.c_str());
+
+	if ( ! boost::filesystem::exists( path ) ){return 0;}
+
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+	int viewgroup_nrviews = quasimodo_brain::readNumberOfViews(sweep_folder+"ViewGroup.xml");
+
+	printf("sweep_folder: %s\n",sweep_folder.c_str());
+
+	int additional_nrviews = 0;
+	QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml"));
+	for (auto objectFile : objectFiles){
+		printf("objectFile: %s\n",(sweep_folder+objectFile.toStdString()).c_str());
+		auto object = loadDynamicObjectFromSingleSweep<PointType>(sweep_folder+objectFile.toStdString(),false);
+		additional_nrviews += object.vAdditionalViews.size();
+	}
+
+	SimpleXMLParser<pcl::PointXYZRGB> parser;
+	SimpleXMLParser<pcl::PointXYZRGB>::RoomData roomData  = parser.loadRoomFromXML(path,std::vector<std::string>{"RoomIntermediateCloud","IntermediatePosition"});
+	int metaroom_nrviews = roomData.vIntermediateRoomClouds.size();
+
+
+	printf("viewgroup_nrviews: %i\n",viewgroup_nrviews);
+	printf("additional_nrviews: %i\n",additional_nrviews);
+	printf("metaroom_nrviews: %i\n",metaroom_nrviews);
+
+	std::vector<reglib::RGBDFrame * > frames;
+
+	char buf [1024];
+	int counter = 0;
+	while(true){//Load all frames that can be loaded
+		sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),counter);
+		string tmp = string(buf)+"_data.txt";
+		QFile file(tmp.c_str());
+		if (!file.exists()){
+			break;
+		}
+
+		reglib::Camera * cam = reglib::Camera::load(std::string(buf)+"_camera");
+		if(cam == 0){break;}
+		reglib::RGBDFrame * frame = reglib::RGBDFrame::load(cam,std::string(buf));
+		if(frame == 0){delete cam; break;}
+		frames.push_back(frame);
+
+		counter++;
+	}
+
+	printf("frames: %i counter: %i\n",frames.size(),counter);
+
+
+	if(roomData.vIntermediateRoomClouds.size() == 0 ){
+		printf("WARNING:: no clouds in room\n");
+	}
+
+	Eigen::Matrix4d m2 = Eigen::Matrix4d ::Identity();
+	if(roomData.vIntermediateRoomClouds.size() != 0 ){m2 = quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransforms[0]);}
+
+
+	reglib::Model * sweepmodel = 0;
+
+	cv::Mat fullmask;
+	fullmask.create(480,640,CV_8UC1);
+	unsigned char * maskdata = (unsigned char *)fullmask.data;
+	for(int j = 0; j < 480*640; j++){maskdata[j] = 255;}
+
+
+	std::vector<reglib::RGBDFrame * > metaroom_frames;
+	for (size_t i=0; i<roomData.vIntermediateRoomClouds.size(); i++){
+		printf("loading intermedite %i\n",i);
+		if(i >= counter){
+			image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected[i];
+			reglib::Camera * cam		= new reglib::Camera();
+			cam->fx = aCameraModel.fx();
+			cam->fy = aCameraModel.fy();
+			cam->cx = aCameraModel.cx();
+			cam->cy = aCameraModel.cy();
+
+			Eigen::Matrix4d m = m2*quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransformsRegistered[i]);
+			reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m,true,saveVisuals_sp);
+
+			sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),counter);
+			frame->save(std::string(buf));
+			cam->save(std::string(buf)+"_camera");
+			metaroom_frames.push_back(frame);
+			counter++;
+		}else{
+			metaroom_frames.push_back(frames[i]);
+		}
+
+
+		if(sweepmodel == 0){
+			sweepmodel = new reglib::Model(metaroom_frames.back(),fullmask);
+		}else{
+			sweepmodel->frames.push_back(metaroom_frames.back());
+			sweepmodel->relativeposes.push_back(metaroom_frames.front()->pose.inverse() * metaroom_frames.back()->pose);
+			sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask));
+		}
+	}
+	//Metaroom frames added...
+
+	sweepmodel->points = quasimodo_brain::getSuperPoints(sweep_folder+"/sweepmodel_superpoints.bin");//Try to load superpoints
+	if(sweepmodel->points.size() == 0){												//If it fails we have to recompute the points...
+		sweepmodel->recomputeModelPoints();
+		quasimodo_brain::saveSuperPoints(sweep_folder+"/sweepmodel_superpoints.bin",sweepmodel->points,Eigen::Matrix4d::Identity(),1.0);
+	}
+	printf("sweepmodel fully loaded!\n");
+	//Load rest of model if possible
+
+
+	std::vector<Eigen::Matrix4d> both_unrefined;
+	both_unrefined.push_back(Eigen::Matrix4d::Identity());
+	unsigned int current_counter = metaroom_frames.size();
+
+	std::vector<reglib::RGBDFrame * > av_frames;
+	std::vector<reglib::ModelMask * > av_mm;
+	for (auto objectFile : objectFiles){
+		auto object = loadDynamicObjectFromSingleSweep<PointType>(sweep_folder+objectFile.toStdString(),false);
+		for (unsigned int i=0; i<object.vAdditionalViews.size(); i++){
+			printf("i:%i current_counter: %i counter: %i\n",i,current_counter,counter);
+			if(current_counter < counter){
+				av_frames.push_back(frames[current_counter]);
+			}else{
+				CloudPtr cloud = object.vAdditionalViews[i];
+
+				cv::Mat rgb;
+				rgb.create(cloud->height,cloud->width,CV_8UC3);
+				unsigned char * rgbdata = (unsigned char *)rgb.data;
+
+				cv::Mat depth;
+					//		exit(0);	depth.create(cloud->height,cloud->width,CV_16UC1);
+				unsigned short * depthdata = (unsigned short *)depth.data;
+
+				unsigned int nr_data = cloud->height * cloud->width;
+				for(unsigned int j = 0; j < nr_data; j++){
+					PointType p = cloud->points[j];
+					rgbdata[3*j+0]	= p.b;
+					rgbdata[3*j+1]	= p.g;
+					rgbdata[3*j+2]	= p.r;
+					depthdata[j]	= short(5000.0 * p.z);
+				}
+
+				geometry_msgs::TransformStamped msg;
+				tf::transformStampedTFToMsg(object.vAdditionalViewsTransforms[i], msg);
+				long sec = msg.header.stamp.sec;
+				long nsec = msg.header.stamp.nsec;
+				double time = double(sec)+1e-9*double(nsec);
+
+				Eigen::Matrix4d m = quasimodo_brain::getMat(object.vAdditionalViewsTransforms[i]);
+
+				reglib::Camera * cam		= metaroom_frames.front()->camera->clone();
+				reglib::RGBDFrame * frame	= new reglib::RGBDFrame(cam,rgb,depth,time, m,true,saveVisuals_sp);//a.matrix());
+
+				sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),current_counter);
+				frame->save(std::string(buf));
+				cam->save(std::string(buf)+"_camera");
+				av_frames.push_back(frame);
+				frames.push_back(frame);
+			}
+			both_unrefined.push_back(metaroom_frames.front()->pose.inverse()*av_frames.back()->pose);
+			av_mm.push_back(new reglib::ModelMask(fullmask));
+			//av_frames.back()->show(true);
+			current_counter++;
+		}
+	}
+
+	reglib::Model * fullmodel	= new reglib::Model();
+	fullmodel->savePath			= saveVisuals_sp+"/";
+	fullmodel->frames			= sweepmodel->frames;
+	fullmodel->relativeposes	= sweepmodel->relativeposes;
+	fullmodel->modelmasks		= sweepmodel->modelmasks;
+	fullmodel->points			= sweepmodel->points;
+	for(unsigned int i = 0; i < av_frames.size(); i++){
+		fullmodel->frames.push_back(av_frames[i]);
+		fullmodel->modelmasks.push_back(av_mm[i]);
+	}
+
+	std::vector<Eigen::Matrix4d> loadedPoses	= quasimodo_brain::readPoseXML(sweep_folder+"/fullmodel_poses.xml");
+	std::vector<reglib::superpoint> spvec		= quasimodo_brain::getSuperPoints(sweep_folder+"/fullmodel_superpoints.bin");
+
+	//Check if poses already computed
+	if(av_frames.size() > 0){
+		if(loadedPoses.size() != (av_frames.size()+metaroom_frames.size())){
+			printf("TIME TO RECOMPUTE\n");
+
+			reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement();
+			refinement->viewer	= viewer;
+			refinement->visualizationLvl	= visualization_lvl_regini;
+			refinement->normalize_matchweights = false;
+			refinement->target_points = 4000;
+
+			reglib::CloudData * cd1 = sweepmodel->getCD(sweepmodel->points.size());
+			refinement->setDst(cd1);
+
+			fullmodel->points = av_frames.front()->getSuperPoints(Eigen::Matrix4d::Identity(),1,false);
+			reglib::CloudData * cd2	= fullmodel->getCD(fullmodel->points.size());
+			refinement->setSrc(cd2);
+
+			Eigen::Matrix4d guess = both_unrefined[1]*both_unrefined[0].inverse();
+
+			reglib::FusionResults fr = refinement->getTransform(guess);
+			Eigen::Matrix4d offset = fr.guess*guess.inverse();
+			for(unsigned int i = 1; i < both_unrefined.size(); i++){
+				both_unrefined[i] = offset*both_unrefined[i];
+			}
+
+			delete refinement;
+
+			fullmodel->points.clear();
+			delete cd1;
+			delete cd2;
+
+			reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25);
+			if(saveVisuals_sp.size() != 0){bgmassreg->savePath = saveVisuals_sp+"/processAV_"+std::to_string(fullmodel->id);}
+			bgmassreg->timeout = 300;
+			bgmassreg->viewer = viewer;
+			bgmassreg->use_surface = true;
+			bgmassreg->use_depthedge = false;
+			bgmassreg->visualizationLvl = visualization_lvl_regref;
+			bgmassreg->maskstep = 5;
+			bgmassreg->nomaskstep = 5;
+			bgmassreg->nomask = true;
+			bgmassreg->stopval = 0.0005;
+			bgmassreg->addModel(sweepmodel);
+			bgmassreg->setData(av_frames,av_mm);
+			reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined);
+			delete bgmassreg;
+
+			for(unsigned int i = 0; i < av_frames.size(); i++){
+				av_frames[i]->pose = sweepmodel->frames.front()->pose * bgmfr.poses[i+1];
+				fullmodel->relativeposes.push_back(bgmfr.poses[i+1]);
+			}
+
+			quasimodo_brain::savePoses(sweep_folder+"/fullmodel_poses.xml", fullmodel->relativeposes);
+//relativeposes[i], frames[i], modelmasks[i]
+		}else{
+			fullmodel->relativeposes = loadedPoses;
+		}
+
+
+		fullmodel->points = sweepmodel->points;
+		if(loadedPoses.size() != (av_frames.size()+metaroom_frames.size())){//If poses changed, recompute superpoints
+			for(unsigned int i = 0; i < av_frames.size(); i++){
+				fullmodel->addSuperPoints(
+							fullmodel->points,
+							fullmodel->relativeposes[i+metaroom_frames.size()],
+							fullmodel->frames[i+metaroom_frames.size()],
+							fullmodel->modelmasks[i+metaroom_frames.size()]);
+			}
+		}
+		quasimodo_brain::saveSuperPoints(sweep_folder+"/fullmodel_superpoints.bin",fullmodel->points,Eigen::Matrix4d::Identity(),1.0);
+
+//		if(savePath.size() != 0){
+//			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cld = fullmodel->getPCLnormalcloud(1, false);
+//			pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld);
+//		}
+//		//pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr Model::getPCLnormalcloud(int step, bool color){
+//
+	}else{
+		quasimodo_brain::savePoses(sweep_folder+"/fullmodel_poses.txt", fullmodel->relativeposes);
+		quasimodo_brain::saveSuperPoints(sweep_folder+"/fullmodel_superpoints.bin",fullmodel->points,Eigen::Matrix4d::Identity(),1.0);
+	}
+
+	printf("fullmodel->points.size() = %i\n",fullmodel->points.size());
+    quasimodo_brain::writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes);
+//	delete reg;
+//	delete mu;
+//	delete sweep;
+
+
+	//Load the unloaded frames...
+/*
+	reglib::Model * fullmodel;
+	if(viewgroup_nrviews == (additional_nrviews+metaroom_nrviews) && !recomputeRelativePoses){
+		printf("time to read old\n");
+		fullmodel = new reglib::Model();
+		fullmodel->savePath = saveVisuals_sp+"/";
+
+		for(unsigned int i = 0; i < viewgroup_nrviews; i++){
+			cv::Mat fullmask;
+			fullmask.create(480,640,CV_8UC1);
+			unsigned char * maskdata = (unsigned char *)fullmask.data;
+			for(int j = 0; j < 480*640; j++){maskdata[j] = 255;}
+			fullmodel->modelmasks.push_back(new reglib::ModelMask(fullmask));
+		}
+
+		readViewXML(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes,compute_edges,saveVisuals_sp);
+		fullmodel->recomputeModelPoints();
+	}else{
+		//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
+		fullmodel = processAV(path,compute_edges,saveVisuals_sp);
+		//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
+
+	}
+	//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__);
+*/
+    double startTime_store = reglib::getTime();
+    fullmodel->pointspath = sweep_folder+"/fullmodel_superpoints.bin";
+    std::string soma_id = quasimodo_brain::initSegment(*np,fullmodel);
+    printf("soma_id: %i\n",soma_id.c_str());
+    printf("add time: %6.6f\n",reglib::getTime()-startTime_store);
+
+//
+//	std::string soma_id2 = quasimodo_brain::initSegment(*np,fullmodel);
+//	printf("soma_id2: %i\n",soma_id2.c_str());
+//	printf("add2 time: %6.6f\n",reglib::getTime()-startTime_store);
+
+    startTime_store = reglib::getTime();
+    reglib::Model * test = quasimodo_brain::getModelFromSegment(*np,soma_id);
+    printf("load time: %6.6f\n",reglib::getTime()-startTime_store);
+
+	return fullmodel;
+}
+
+int totalcounter = 0;
+
+
+int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = true){
+	path = quasimodo_brain::replaceAll(path, "//", "/");
+
+	quasimodo_brain::cleanPath(path);
+	int returnval = 0;
+	printf("processMetaroom: %s\n",path.c_str());
+
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+	if ( ! boost::filesystem::exists( path ) ){return 0;}
+
+	QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml"));
+	store_old_xml = objectFiles.size() == 0;
+
+	SimpleXMLParser<pcl::PointXYZRGB> parser;
+	//SimpleXMLParser<pcl::PointXYZRGB>::RoomData current_roomData  = parser.loadRoomFromXML(path,std::vector<std::string>());
+	//	if(current_roomData.roomWaypointId.compare("ReceptionDesk") != 0){return 0;}
+
+	SimpleXMLParser<pcl::PointXYZRGB>::RoomData current_roomData  = parser.loadRoomFromXML(path,std::vector<std::string>{"RoomIntermediateCloud","IntermediatePosition"});
+	if(current_roomData.vIntermediateRoomClouds.size() != 17){return 0;}
+
+
+
+	reglib::Model * fullmodel = getAVMetaroom(path,true,saveVisuals);
+
+	reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+	reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( fullmodel, reg);
+	mu->occlusion_penalty               = 15;
+	mu->massreg_timeout                 = 60*4;
+	mu->viewer							= viewer;
+
+	std::vector<Eigen::Matrix4d> po;
+	std::vector<reglib::RGBDFrame*> fr;
+	std::vector<reglib::ModelMask*> mm;
+	fullmodel->getData(po, fr, mm);
+
+	if(fr.size() == 0){
+		printf("no frames in model, returning\n");
+		fullmodel->fullDelete();
+		delete fullmodel;
+		return 0;
+	}
+
+	DynamicObjectXMLParser objectparser(sweep_folder, true);
+
+	std::string current_waypointid = current_roomData.roomWaypointId;
+
+	if(overall_folder.back() == '/'){overall_folder.pop_back();}
+
+	int prevind = -1;
+	std::vector<std::string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<pcl::PointXYZRGB>(overall_folder);
+	for (unsigned int i = 0; i < sweep_xmls.size(); i++){
+		sweep_xmls[i] = quasimodo_brain::replaceAll(sweep_xmls[i], "//", "/");
+		SimpleXMLParser<pcl::PointXYZRGB>::RoomData other_roomData  = parser.loadRoomFromXML(sweep_xmls[i],std::vector<std::string>(),false,false);
+		std::string other_waypointid = other_roomData.roomWaypointId;
+
+		if(sweep_xmls[i].compare(path) == 0){break;}
+		if(other_waypointid.compare(current_waypointid) == 0){prevind = i;}
+	}
+
+	printf("prevind: %i\n",prevind);
+	printf("current: %s\n",path.c_str());
+
+	int nextind = sweep_xmls.size();
+	for (int i = int(sweep_xmls.size()-1); i >= 0 ; i--){
+		if(i < 0){break;}
+		SimpleXMLParser<pcl::PointXYZRGB>::RoomData other_roomData  = parser.loadRoomFromXML(sweep_xmls[i],std::vector<std::string>(),false,false);
+		std::string other_waypointid = other_roomData.roomWaypointId;
+
+		if(sweep_xmls[i].compare(path) == 0){break;}
+		if(other_waypointid.compare(current_waypointid) == 0){nextind = i;}
+	}
+
+	std::vector< reglib::Model * > models;
+	models.push_back(fullmodel);
+
+	std::vector< std::vector< cv::Mat > > internal;
+	std::vector< std::vector< cv::Mat > > external;
+	std::vector< std::vector< cv::Mat > > dynamic;
+
+	std::vector< reglib::Model * > bgs;
+	if(prevind != -1){
+		std::string prev = sweep_xmls[prevind];
+		printf("prev: %s\n",prev.c_str());
+		reglib::Model * bg = getAVMetaroom(prev);
+		if(bg->frames.size() == 0){
+			printf("no frames in bg\n");
+			bg->fullDelete();
+			delete bg;
+		}else{
+			bgs.push_back(bg);
+		}
+	}else{
+		printf("no previous...\n");
+	}
+
+
+	if(bgs.size() > 0){
+
+		std::vector<cv::Mat> peopleMasks;
+		for(unsigned int i = 0; i < models.front()->frames.size(); i++){
+			cv::Mat peoplemask;
+			int width = models.front()->frames[i]->camera->width;
+			int height = models.front()->frames[i]->camera->height;
+			peoplemask.create(height,width,CV_8UC1);
+			unsigned int nr_pixels = width*height;
+			for(unsigned int j = 0; j < nr_pixels; j++){peoplemask.data[j] = 0;}
+			peopleMasks.push_back(peoplemask);
+		}
+
+		std::vector<std::vector<std::tuple<float, float, float, float> > > detections;
+		detections = metaroom_detections::detections_for_xml(path, "intermediate_deep_detection");
+
+		unsigned int count = 0;
+		for (std::vector<std::tuple<float, float, float, float> >& image_dets : detections) {
+			for (std::tuple<float, float, float, float>& det : image_dets) {
+				cv::rectangle(peopleMasks[count], cv::Rect(std::get<0>(det),std::get<1>(det),std::get<2>(det),std::get<3>(det)), cv::Scalar(255,255,255), -1);
+			}
+			count++;
+		}
+
+		auto sweep = SimpleXMLParser<PointType>::loadRoomFromXML(path, std::vector<std::string>{},false);
+
+		quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl,saveVisuals);
+
+		quasimodo_brain::remove_old_seg(sweep_folder);
+		if(models.size() == 0){	returnval = 2;}
+		else{					returnval = 3;}
+
+		for(unsigned int i = 0; i < models.size(); i++){
+			printf("processing model %i\n",i);
+			std::vector<cv::Mat> internal_masks = internal[i];
+			std::vector<cv::Mat> external_masks = external[i];
+			std::vector<cv::Mat> dynamic_masks	= dynamic[i];
+			reglib::Model * model = models[i];
+
+			std::vector<Eigen::Matrix4d> mod_po;
+			std::vector<reglib::RGBDFrame*> mod_fr;
+			std::vector<reglib::ModelMask*> mod_mm;
+			model->getData(mod_po, mod_fr, mod_mm);
+
+			int dynamicCounter = 1;
+			while(true){
+				double sum = 0;
+				double sumx = 0;
+				double sumy = 0;
+				double sumz = 0;
+				std::vector<int> imgnr;
+				std::vector<cv::Mat> masks;
+
+				CloudPtr cloud_cluster (new Cloud());
+
+				Eigen::Matrix4d first = model->frames.front()->pose;
+				int peopleoverlaps = 0;
+
+				for(unsigned int j = 0; j < mod_fr.size(); j++){
+					reglib::RGBDFrame * frame = mod_fr[j];
+					//std::cout << first*mod_po[j] << std::endl;
+					Eigen::Matrix4d p = frame->pose;//first*mod_po[j];//*bg->frames.front()->pose;
+					unsigned char  * rgbdata		= (unsigned char	*)(frame->rgb.data);
+					unsigned short * depthdata		= (unsigned short	*)(frame->depth.data);
+					float		   * normalsdata	= (float			*)(frame->normals.data);
+
+					reglib::Camera * camera = frame->camera;
+
+					unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data);
+					unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data);
+					unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data);
+
+					float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+					float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+					float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+					const float idepth			= camera->idepth_scale;
+					const float cx				= camera->cx;
+					const float cy				= camera->cy;
+					const float ifx				= 1.0/camera->fx;
+					const float ify				= 1.0/camera->fy;
+					const unsigned int width	= camera->width;
+					const unsigned int height	= camera->height;
+
+					cv::Mat peoplemask = peopleMasks[j];
+
+					cv::Mat mask;
+					mask.create(height,width,CV_8UC1);
+					unsigned char * maskdata = (unsigned char *)(mask.data);
+					unsigned char * peoplemaskdata = (unsigned char *)(peoplemask.data);
+
+					bool containsData = false;
+					for(unsigned int w = 0; w < width;w++){
+						for(unsigned int h = 0; h < height;h++){
+							int ind = h*width+w;
+
+							if(dynamicmaskdata[ind] == dynamicCounter){
+								maskdata[ind] = 255;
+								containsData = true;
+								float z = idepth*float(depthdata[ind]);
+								if(z > 0){
+									float x = (float(w) - cx) * z * ifx;
+									float y = (float(h) - cy) * z * ify;
+
+									PointType p;
+									p.x += m00*x + m01*y + m02*z + m03;
+									p.y += m10*x + m11*y + m12*z + m13;
+									p.z += m20*x + m21*y + m22*z + m23;
+									p.r  = rgbdata[3*ind+2];
+									p.g  = rgbdata[3*ind+1];
+									p.b  = rgbdata[3*ind+0];
+									cloud_cluster->points.push_back (p);
+
+									sumx += p.x;
+									sumy += p.y;
+									sumz += p.z;
+									sum ++;
+									if(peoplemaskdata[ind] != 0){peopleoverlaps++;}
+								}
+							}else{
+								maskdata[ind] = 0;
+							}
+						}
+					}
+
+					if(containsData){
+						masks.push_back(mask);
+						imgnr.push_back(j);
+					}
+				}
+
+				cloud_cluster->width = cloud_cluster->points.size ();
+				cloud_cluster->height = 1;
+				cloud_cluster->is_dense = true;
+
+				printf("peopleoverlaps: %i\n",peopleoverlaps);
+
+				if(masks.size() > 0 && cloud_cluster->points.size() > 0){
+					if(peopleoverlaps == 0 && cloud_cluster->points.size() >= minClusterSize){
+
+						if(store_old_xml){
+							std::stringstream ss;
+							ss << "object_";
+							ss << (dynamicCounter-1);
+
+							// create and save dynamic object
+							DynamicObject::Ptr roomObject(new DynamicObject());
+							roomObject->setCloud(cloud_cluster);
+							roomObject->setTime(sweep.roomLogStartTime);
+							roomObject->m_roomLogString = sweep.roomLogName;
+							roomObject->m_roomStringId = sweep.roomWaypointId;
+							roomObject->m_roomRunNumber = sweep.roomRunNumber;
+							//				        // create label from room log time; could be useful later on, and would resolve ambiguities
+							std::stringstream ss_obj;
+							ss_obj<<boost::posix_time::to_simple_string(sweep.roomLogStartTime);
+							ss_obj<<"_object_";ss_obj<<(dynamicCounter-1);
+							std::string tmp = ss_obj.str();
+							printf("ss_obj.str(): %s\n",tmp.c_str());
+							//roomObject->m_label = tmp;
+							roomObject->setLabel(tmp);
+							std::string xml_file = objectparser.saveAsXML(roomObject);
+							printf("xml_file: %s\n",xml_file.c_str());
+						}
+
+						char buf [1024];
+						sprintf(buf,"%s/dynamic_obj%10.10i.pcd",sweep_folder.c_str(),dynamicCounter-1);
+						pcl::io::savePCDFileBinaryCompressed(std::string(buf),*cloud_cluster);
+
+						*dyncloud += *cloud_cluster;
+
+
+						//					std::string objectpcd = std::string(buf);//object.substr(0,object.size()-4);
+						//					std::cout << objectpcd.substr(0,objectpcd.size()-4) << std::endl;
+
+						sprintf(buf,"%s/dynamic_obj%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1);
+						printf("saving dynamic objec: %s\n",buf);
+						QFile file(buf);
+						if (file.exists()){file.remove();}
+						if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<<std::endl;}
+						QXmlStreamWriter* xmlWriter = new QXmlStreamWriter();
+						xmlWriter->setDevice(&file);
+
+						xmlWriter->writeStartDocument();
+						xmlWriter->writeStartElement("Object");
+						xmlWriter->writeAttribute("object_number", QString::number(dynamicCounter-1));
+						xmlWriter->writeAttribute("classname", QString(""));
+						xmlWriter->writeAttribute("instancename", QString(""));
+						xmlWriter->writeAttribute("tags", QString(""));
+						xmlWriter->writeStartElement("Mean");
+						xmlWriter->writeAttribute("x", QString::number(sumx/sum));
+						xmlWriter->writeAttribute("y", QString::number(sumy/sum));
+						xmlWriter->writeAttribute("z", QString::number(sumz/sum));
+						xmlWriter->writeEndElement();
+
+
+						for(unsigned int j = 0; j < masks.size(); j++){
+							char buf [1024];
+							sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),dynamicCounter-1,imgnr[j]);
+							cv::imwrite(buf, masks[j] );
+
+							int width = masks[j].cols;
+							int height = masks[j].rows;
+
+							int maxw = 0;
+							int minw = width;
+							int maxh = 0;
+							int minh = height;
+							for(int w = 0; w < width; w++){
+								for(int h = 0; h < height; h++){
+									if(masks[j].data[h*width+w] != 0){
+										maxw = std::max(maxw,w);
+										minw = std::min(minw,w);
+										maxh = std::max(maxh,h);
+										minh = std::min(minh,h);
+									}
+								}
+							}
+
+							double ratio = 0.15;
+							int diffw = maxw-minw;
+							int diffh = maxh-minh;
+
+							maxw = std::min(width-1	,int(maxw+ratio*diffw));
+							minw = std::max(0		,int(minw-ratio*diffw));
+							maxh = std::min(height-1,int(maxh+ratio*diffh));
+							minh = std::max(0		,int(minh-ratio*diffh));
+
+							diffw = maxw-minw;
+							diffh = maxh-minh;
+
+							cv::Mat image = mod_fr[imgnr[j]]->rgb.clone();
+							cv::Rect myROI(minw, minh, diffw, diffh);
+							cv::Mat localimg = image(myROI);
+
+
+							char buf2 [1024];
+							sprintf(buf2,"/home/johane/imgregion/region%10.10i.png",totalcounter++);
+							cv::imwrite(buf2, localimg );
+
+							printf("saving dynamic mask: dynamicmask_%i_%i.png\n",dynamicCounter-1,imgnr[j]);
+
+							sprintf(buf,"dynamicmask_%i_%i.png",dynamicCounter-1,imgnr[j]);
+							xmlWriter->writeStartElement("Mask");
+							xmlWriter->writeAttribute("filename", QString(buf));
+							xmlWriter->writeAttribute("image_number", QString::number(imgnr[j]));
+							xmlWriter->writeEndElement();
+						}
+
+						xmlWriter->writeEndElement();
+						xmlWriter->writeEndDocument();
+						delete xmlWriter;
+					}
+					dynamicCounter++;
+
+				}else{break;}
+			}
+
+			int movingCounter = 1;
+			while(true){
+				double sum = 0;
+				double sumx = 0;
+				double sumy = 0;
+				double sumz = 0;
+				std::vector<int> imgnr;
+				std::vector<cv::Mat> masks;
+
+				for(unsigned int j = 0; j < mod_fr.size(); j++){
+					reglib::RGBDFrame * frame = mod_fr[j];
+					//std::cout << mod_po[j] << std::endl;
+					Eigen::Matrix4d p = mod_po[j]*bgs.front()->frames.front()->pose;
+					unsigned char  * rgbdata		= (unsigned char	*)(frame->rgb.data);
+					unsigned short * depthdata		= (unsigned short	*)(frame->depth.data);
+					float		   * normalsdata	= (float			*)(frame->normals.data);
+
+					reglib::Camera * camera = frame->camera;
+
+					unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data);
+					unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data);
+					unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data);
+
+					float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+					float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+					float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+					const float idepth			= camera->idepth_scale;
+					const float cx				= camera->cx;
+					const float cy				= camera->cy;
+					const float ifx				= 1.0/camera->fx;
+					const float ify				= 1.0/camera->fy;
+					const unsigned int width	= camera->width;
+					const unsigned int height	= camera->height;
+
+					cv::Mat mask;
+					mask.create(height,width,CV_8UC1);
+					unsigned char * maskdata = (unsigned char *)(mask.data);
+
+					bool containsData = false;
+					for(unsigned int w = 0; w < width;w++){
+						for(unsigned int h = 0; h < height;h++){
+							int ind = h*width+w;
+
+							if(internalmaskdata[ind] == movingCounter){
+								maskdata[ind] = 255;
+								containsData = true;
+								float z = idepth*float(depthdata[ind]);
+								if(z > 0){
+									float x = (float(w) - cx) * z * ifx;
+									float y = (float(h) - cy) * z * ify;
+
+									sumx += m00*x + m01*y + m02*z + m03;
+									sumy += m10*x + m11*y + m12*z + m13;
+									sumz += m20*x + m21*y + m22*z + m23;
+									sum ++;
+								}
+							}else{
+								maskdata[ind] = 0;
+							}
+						}
+					}
+
+					if(containsData){
+						masks.push_back(mask);
+						imgnr.push_back(j);
+					}
+				}
+				if(masks.size() > 0){
+					char buf [1024];
+					sprintf(buf,"%s/moving_obj%10.10i.xml",sweep_folder.c_str(),movingCounter-1);
+					QFile file(buf);
+					if (file.exists()){file.remove();}
+					if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<<std::endl;}
+					QXmlStreamWriter* xmlWriter = new QXmlStreamWriter();
+					xmlWriter->setDevice(&file);
+
+					xmlWriter->writeStartDocument();
+					xmlWriter->writeStartElement("Object");
+					xmlWriter->writeAttribute("object_number", QString::number(movingCounter-1));
+					xmlWriter->writeAttribute("label", QString(""));
+					xmlWriter->writeStartElement("Mean");
+					xmlWriter->writeAttribute("x", QString::number(sumx/sum));
+					xmlWriter->writeAttribute("y", QString::number(sumy/sum));
+					xmlWriter->writeAttribute("z", QString::number(sumz/sum));
+					xmlWriter->writeEndElement();
+
+					for(unsigned int j = 0; j < masks.size(); j++){
+						char buf [1024];
+						sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),movingCounter-1,imgnr[j]);
+						cv::imwrite(buf, masks[j] );
+						sprintf(buf,"movingmask_%i_%i.png",movingCounter-1,imgnr[j]);
+						xmlWriter->writeStartElement("Mask");
+						xmlWriter->writeAttribute("filename", QString(buf));
+						xmlWriter->writeAttribute("image_number", QString::number(imgnr[j]));
+						xmlWriter->writeEndElement();
+					}
+
+					xmlWriter->writeEndElement();
+					xmlWriter->writeEndDocument();
+					delete xmlWriter;
+					movingCounter++;
+				}else{break;}
+			}
+		}
+	}else{
+		returnval = 1;
+	}
+
+	if(dyncloud->points.size()){
+		dyncloud->width = dyncloud->points.size();
+		dyncloud->height = 1;
+		pcl::io::savePCDFileBinaryCompressed(sweep_folder+"/dynamic_clusters.pcd",*dyncloud);
+	}
+
+	for(unsigned int i = 0; i < bgs.size(); i++){
+		bgs[i]->fullDelete();
+		delete bgs[i];
+	}
+
+	fullmodel->fullDelete();
+	delete fullmodel;
+	//	delete bgmassreg;
+	delete reg;
+	delete mu;
+	printf("publishing file %s to %s\n",path.c_str(),outtopic.c_str());
+
+	std_msgs::String msg;
+	msg.data = path;
+	for(unsigned int i = 0; i < out_pubs.size(); i++){out_pubs[i].publish(msg);}
+	ros::spinOnce();
+
+	//sendMetaroomToServer(path);
+	return returnval;
+}
+
+void chatterCallback(const std_msgs::String::ConstPtr& msg){
+	CloudPtr dyncloud (new Cloud());
+	processMetaroom(dyncloud,msg->data);
+}
+
+void trainMetaroom(std::string path){
+	printf("processing: %s\n",path.c_str());
+
+	if(posepath.compare("")==0){
+		printf("posepath not set, set before training\n");
+		return ;
+	}
+
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+	reglib::Model * fullmodel = processAV(path);
+
+	//Not needed if metaroom well calibrated
+	reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01);
+	bgmassreg->timeout = 60*10;
+	bgmassreg->viewer = viewer;
+	bgmassreg->use_surface = true;
+	bgmassreg->use_depthedge = false;//true;
+	bgmassreg->visualizationLvl = visualization_lvl;//0;
+	bgmassreg->maskstep = 5;
+	bgmassreg->nomaskstep = 5;
+	bgmassreg->nomask = true;
+	bgmassreg->stopval = 0.0005;
+	bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks);
+	reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes);
+	quasimodo_brain::savePoses(overall_folder+"/"+posepath,bgmfr.poses,17);
+	fullmodel->fullDelete();
+	delete fullmodel;
+	delete bgmassreg;
+}
+
+std::vector<reglib::Model *> loadModels(std::string path){
+	std::vector<reglib::Model *> models;
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+	std::vector<reglib::RGBDFrame *> frames;
+	std::vector<Eigen::Matrix4d> poses;
+	readViewXML(sweep_folder+"ViewGroup.xml",frames,poses,false);
+
+	QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml"));
+	for (auto objectFile : objectFiles){
+		std::string object = sweep_folder+objectFile.toStdString();
+		printf("object: %s\n",object.c_str());
+
+		QFile file(object.c_str());
+
+		if (!file.exists()){
+			ROS_ERROR("Could not open file %s to masks.",object.c_str());
+			continue;
+		}
+
+		file.open(QIODevice::ReadOnly);
+		//ROS_INFO_STREAM("Parsing xml file: "<<object.c_str());
+
+		reglib::Model * mod = new reglib::Model();
+		QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+
+		while (!xmlReader->atEnd() && !xmlReader->hasError()){
+			QXmlStreamReader::TokenType token = xmlReader->readNext();
+			if (token == QXmlStreamReader::StartDocument)
+				continue;
+
+			if (xmlReader->hasError()){
+				ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str());
+				break;
+			}
+
+			QString elementName = xmlReader->name().toString();
+
+			if (token == QXmlStreamReader::StartElement){
+				if (xmlReader->name() == "Mask"){
+					int number = 0;
+					cv::Mat mask;
+					QXmlStreamAttributes attributes = xmlReader->attributes();
+					if (attributes.hasAttribute("filename")){
+						QString maskpath = attributes.value("filename").toString();
+						//printf("mask filename: %s\n",(sweep_folder+maskpath.toStdString()).c_str());
+						mask = cv::imread(sweep_folder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED);
+						//mask = cv::imread((maskpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED);
+					}else{break;}
+
+
+					if (attributes.hasAttribute("image_number")){
+						QString depthpath = attributes.value("image_number").toString();
+						number = atoi(depthpath.toStdString().c_str());
+						//printf("number: %i\n",number);
+					}else{break;}
+
+					mod->frames.push_back(frames[number]->clone());
+					mod->relativeposes.push_back(poses[number]);
+					mod->modelmasks.push_back(new reglib::ModelMask(mask));
+				}
+			}
+		}
+
+		delete xmlReader;
+		models.push_back(mod);
+	}
+
+	for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];}
+	return models;
+}
+
+void addModelToModelServer(reglib::Model * model){
+	printf("addModelToModelServer\n");
+	for(unsigned int i = 0; i < model_pubs.size(); i++){model_pubs[i].publish(quasimodo_brain::getModelMSG(model));}
+	ros::spinOnce();
+}
+
+void sendMetaroomToServer(std::string path){
+	std::vector<reglib::Model *> models = loadModels(path);
+	for(unsigned int i = 0; i < models.size(); i++){
+		printf("%i\n",i);
+		addModelToModelServer(models[i]);
+		models[i]->fullDelete();
+		delete models[i];
+	}
+}
+
+void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(msg->data);}
+
+void processAndSendCallback(const std_msgs::String::ConstPtr& msg){
+	printf("================================================================================\n");
+	printf("============================processAndSendCallback==============================\n");
+	printf("================================================================================\n");
+	CloudPtr dyncloud (new Cloud());
+	processMetaroom(dyncloud,msg->data);
+	sendCallback(msg);
+}
+
+bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){
+	printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n");
+	std::string current_waypointid = req.waypoint_id;
+
+	printf("current_waypointid: %i\n",current_waypointid.c_str());
+
+
+	if(overall_folder.back() == '/'){overall_folder.pop_back();}
+
+	SimpleXMLParser<pcl::PointXYZRGB> parser;
+	int prevind = -1;
+	std::vector<std::string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<pcl::PointXYZRGB>(overall_folder);
+	for (unsigned int i = 0; i < sweep_xmls.size(); i++){
+		SimpleXMLParser<pcl::PointXYZRGB>::RoomData other_roomData  = parser.loadRoomFromXML(sweep_xmls[i],std::vector<std::string>(),false,false);
+		std::string other_waypointid = other_roomData.roomWaypointId;
+		if(other_waypointid.compare(current_waypointid) == 0){prevind = i;}
+	}
+	if(prevind == -1){return false;}
+	std::string path = sweep_xmls[prevind];
+	//processMetaroom(path);
+
+	printf("path: %s\n",path.c_str());
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+	printf("sweep_folder: %s\n",sweep_folder.c_str());
+
+	QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml"));
+	for (auto objectFile : objectFiles){
+		std::string object = sweep_folder+objectFile.toStdString();
+		printf("object: %s\n",object.c_str());
+
+		QFile file(object.c_str());
+
+		if (!file.exists()){
+			ROS_ERROR("Could not open file %s to masks.",object.c_str());
+			continue;
+		}
+
+		file.open(QIODevice::ReadOnly);
+		ROS_INFO_STREAM("Parsing xml file: "<<object.c_str());
+
+		QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+
+		while (!xmlReader->atEnd() && !xmlReader->hasError()){
+			QXmlStreamReader::TokenType token = xmlReader->readNext();
+			if (token == QXmlStreamReader::StartDocument)
+				continue;
+
+			if (xmlReader->hasError()){
+				ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str());
+				break;
+			}
+
+			QString elementName = xmlReader->name().toString();
+			if (token == QXmlStreamReader::StartElement){
+				if (xmlReader->name() == "Mean"){
+					QXmlStreamAttributes attributes = xmlReader->attributes();
+					double x = atof(attributes.value("x").toString().toStdString().c_str());
+					double y = atof(attributes.value("y").toString().toStdString().c_str());
+					double z = atof(attributes.value("z").toString().toStdString().c_str());
+					printf("mean: %f %f %f\n",x,y,z);
+
+					std::string objectpcd = object.substr(0,object.size()-4);
+					std::cout << objectpcd << std::endl;
+
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+					pcl::io::loadPCDFile<pcl::PointXYZRGB> (objectpcd+".pcd", *cloud);
+
+					printf("cloud->points.size() = %i\n",cloud->points.size());
+
+					sensor_msgs::PointCloud2 msg_objects;
+					pcl::toROSMsg(*cloud, msg_objects);
+					msg_objects.header.frame_id="/map";
+
+					geometry_msgs::Point p;
+					p.x = x;
+					p.y = y;
+					p.z = z;
+
+					res.object_id.push_back(object);
+					res.objects.push_back(msg_objects);
+					res.centroids.push_back(p);
+				}
+			}
+		}
+		delete xmlReader;
+	}
+
+	return true;
+}
+
+bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){
+	std::string current_waypointid = req.waypoint_id;
+
+	if(overall_folder.back() == '/'){overall_folder.pop_back();}
+
+	SimpleXMLParser<pcl::PointXYZRGB> parser;
+	int prevind = -1;
+	std::vector<std::string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<pcl::PointXYZRGB>(overall_folder);
+	for (unsigned int i = 0; i < sweep_xmls.size(); i++){
+		SimpleXMLParser<pcl::PointXYZRGB>::RoomData other_roomData  = parser.loadRoomFromXML(sweep_xmls[i],std::vector<std::string>(),false,false);
+		std::string other_waypointid = other_roomData.roomWaypointId;
+		if(other_waypointid.compare(current_waypointid) == 0){prevind = i;}
+	}
+	if(prevind == -1){return false;}
+	std::string path = sweep_xmls[prevind];
+
+	printf("path: %s\n",path.c_str());
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+	printf("sweep_folder: %s\n",sweep_folder.c_str());
+	printf("req.object_id: %s\n",req.object_id.c_str());
+	char buf [1024];
+	std::string fullpath = std::string(buf);
+
+	return true;
+}
+
+bool segmentRaresFiles(std::string path, bool resegment){
+	printf("bool segmentRaresFiles(%s)\n",path.c_str());
+
+	vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(path);
+	for (auto sweep_xml : sweep_xmls) {
+		printf("sweep_xml: %s\n",sweep_xml.c_str());
+
+		quasimodo_brain::cleanPath(sweep_xml);
+		int slash_pos = sweep_xml.find_last_of("/");
+		std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/";
+		QStringList segoutput = QDir(sweep_folder.c_str()).entryList(QStringList("segoutput.txt"));
+
+		printf("segoutput %i\n",segoutput.size());
+		if(resegment || segoutput.size() == 0){
+
+			std::ofstream myfile;
+			myfile.open (sweep_folder+"segoutput.txt");
+			myfile << "dummy";
+			myfile.close();
+
+			CloudPtr dyncloud (new Cloud());
+			processMetaroom(dyncloud,sweep_xml);
+		}
+
+	}
+	return false;
+}
+
+bool testDynamicObjectServiceCallback(std::string path){
+	printf("bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res)\n");
+	DynamicObjectsServiceRequest req;
+	req.waypoint_id = path;
+	DynamicObjectsServiceResponse res;
+	return dynamicObjectsServiceCallback(req,res);
+}
+
+void sendRoomToSomaLLSD(std::string path){
+	printf("sendRoomToSomaLLSD(%s)\n",path.c_str());
+
+
+	if ( ! boost::filesystem::exists( path ) ){return;}
+
+
+	SimpleXMLParser<pcl::PointXYZRGB> parser;
+	SimpleXMLParser<pcl::PointXYZRGB>::RoomData roomData  = parser.loadRoomFromXML(path,std::vector<std::string>(),false,false);
+	std::string waypoint = roomData.roomWaypointId;
+	std::string episode_id = roomData.roomLogName;
+
+
+
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+
+	QFile file((sweep_folder+"ViewGroup.xml").c_str());
+	if (!file.exists()){
+		ROS_ERROR("Could not open file %s to load room.",(sweep_folder+"ViewGroup.xml").c_str());
+		return;
+	}
+
+	file.open(QIODevice::ReadOnly);
+	ROS_INFO_STREAM("Parsing xml file: "<<(sweep_folder+"ViewGroup.xml").c_str());
+
+	std::vector< soma_llsd_msgs::Scene > scenes;
+	std::vector< Eigen::Matrix4d > regposes;
+	std::vector< cv::Mat > rgbs;
+	std::vector< cv::Mat > depths;
+
+
+	bool allOk = true;
+	QXmlStreamReader* xmlReader = new QXmlStreamReader(&file);
+	while (!xmlReader->atEnd() && !xmlReader->hasError())
+	{
+		QXmlStreamReader::TokenType token = xmlReader->readNext();
+		if (token == QXmlStreamReader::StartDocument)
+			continue;
+
+		if (xmlReader->hasError())
+		{
+			ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str());
+			return;
+		}
+
+		QString elementName = xmlReader->name().toString();
+
+		if (token == QXmlStreamReader::StartElement)
+		{
+
+			if (xmlReader->name() == "View")
+			{
+				cv::Mat rgb;
+				cv::Mat depth;
+				//printf("elementName: %s\n",elementName.toStdString().c_str());
+				QXmlStreamAttributes attributes = xmlReader->attributes();
+				if (attributes.hasAttribute("RGB"))
+				{
+					std::string imgpath = attributes.value("RGB").toString().toStdString();
+					printf("rgb filename:   %s\n",(sweep_folder+"/"+imgpath).c_str());
+					rgb = cv::imread(sweep_folder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED);
+				}else{break;}
+
+
+				if (attributes.hasAttribute("DEPTH"))
+				{
+					std::string imgpath = attributes.value("DEPTH").toString().toStdString();
+					printf("depth filename: %s\n",(sweep_folder+"/"+imgpath).c_str());
+					depth = cv::imread(sweep_folder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED);
+				}else{break;}
+
+
+				token = xmlReader->readNext();//Stamp
+				elementName = xmlReader->name().toString();
+
+				token = xmlReader->readNext();//sec
+				elementName = xmlReader->name().toString();
+				int sec = atoi(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//nsec
+				elementName = xmlReader->name().toString();
+				int nsec = atoi(xmlReader->readElementText().toStdString().c_str());
+				token = xmlReader->readNext();//end stamp
+
+				token = xmlReader->readNext();//Camera
+				elementName = xmlReader->name().toString();
+
+				token = xmlReader->readNext();//fx
+				float fx = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//fy
+				float fy = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//cx
+				float cx = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//cy
+				float cy = atof(xmlReader->readElementText().toStdString().c_str());
+
+				token = xmlReader->readNext();//Camera
+				elementName = xmlReader->name().toString();
+
+				double time = double(sec)+double(nsec)/double(1e9);
+
+				token = xmlReader->readNext();//RegisteredPose
+				elementName = xmlReader->name().toString();
+
+				Eigen::Matrix4d regpose = quasimodo_brain::getPose(xmlReader);
+				regposes.push_back(regpose);
+
+				token = xmlReader->readNext();//RegisteredPose
+				elementName = xmlReader->name().toString();
+
+
+				token = xmlReader->readNext();//Pose
+				elementName = xmlReader->name().toString();
+
+				Eigen::Matrix4d pose = quasimodo_brain::getPose(xmlReader);
+
+				token = xmlReader->readNext();//Pose
+				elementName = xmlReader->name().toString();
+
+				rgbs.push_back(rgb);
+				depths.push_back(depth);
+
+
+				unsigned char *		rgbdata		= (unsigned char *)rgb.data;
+				unsigned short *	depthdata	= (unsigned short *)depth.data;
+				const unsigned int width	= rgb.cols;
+				const unsigned int height	= rgb.rows;
+
+				const double idepth			= 0.001/5.0;
+				const double ifx			= 1.0/fx;
+				const double ify			= 1.0/fy;
+
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
+				cloud->width	= width;
+				cloud->height	= height;
+				cloud->points.resize(width*height);
+				for(unsigned int w = 0; w < width; w++){
+					for(unsigned int h = 0; h < height;h++){
+						int ind = h*width+w;
+						double z = idepth*double(depthdata[ind]);
+						pcl::PointXYZRGB p;
+						p.b = rgbdata[3*ind+0];
+						p.g = rgbdata[3*ind+1];
+						p.r = rgbdata[3*ind+2];
+						if(z > 0){
+							p.x = (double(w) - cx) * z * ifx;
+							p.y = (double(h) - cy) * z * ify;
+							p.z = z;
+						}else{
+							p.x = NAN;
+							p.y = NAN;
+							p.z = NAN;
+						}
+						cloud->points[ind] = p;
+					}
+				}
+
+				Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
+				K(0,0) = fx;
+				K(1,1) = fy;
+				K(0,2) = cx;
+				K(1,2) = cy;
+				soma_llsd_msgs::Scene scene;
+				allOk = allOk && quasimodo_conversions::raw_frames_to_soma_scene(*np,rgb,depth,cloud, pose, K,waypoint, episode_id,scene);
+				scenes.push_back(scene);
+			}
+		}
+	}
+	delete xmlReader;
+
+	if(!allOk){printf("failed to add all scenes to soma, not adding segments\n");return;}
+	//Scenes added to database...
+
+	std::ofstream sceneidfile;
+	sceneidfile.open (sweep_folder+"/sceneids.txt");
+	for(unsigned int i = 0; i < scenes.size(); i++){
+		sceneidfile << scenes[i].id;
+		std::cout << "-------------------------------------------------\n" << scenes[i].id << std::endl;
+	}
+	sceneidfile.close();
+
+	//		string id
+	//		string meta_data
+	//		string[] related_scenes
+	//		Observation[] observations
+	QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml"));
+	for (auto objectFile : objectFiles){
+
+		std::string object = sweep_folder+objectFile.toStdString();
+		printf("object: %s\n",object.c_str());
+
+		QFile objfile(object.c_str());
+
+		if (!objfile.exists()){
+			ROS_ERROR("Could not open file %s to masks.",object.c_str());
+			continue;
+		}
+
+		objfile.open(QIODevice::ReadOnly);
+		ROS_INFO_STREAM("Parsing xml file: "<<object.c_str());
+
+		std::vector< std::string > scid;
+		std::vector< cv::Mat > masks;
+		std::vector< Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix<double, 4, 4> > > poses;
+
+
+		QXmlStreamReader* xmlReader = new QXmlStreamReader(&objfile);
+
+		while (!xmlReader->atEnd() && !xmlReader->hasError()){
+			QXmlStreamReader::TokenType token = xmlReader->readNext();
+			if (token == QXmlStreamReader::StartDocument)
+				continue;
+
+			if (xmlReader->hasError()){
+				ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str());
+				break;
+			}
+
+			QString elementName = xmlReader->name().toString();
+
+			if (token == QXmlStreamReader::StartElement){
+				if (xmlReader->name() == "Mask"){
+					int number = 0;
+					cv::Mat mask;
+					QXmlStreamAttributes attributes = xmlReader->attributes();
+					if (attributes.hasAttribute("filename")){
+						QString maskpath = attributes.value("filename").toString();
+						mask = cv::imread(sweep_folder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED);
+					}else{break;}
+
+
+					if (attributes.hasAttribute("image_number")){
+						QString depthpath = attributes.value("image_number").toString();
+						number = atoi(depthpath.toStdString().c_str());
+						//printf("number: %i\n",number);
+					}else{break;}
+
+					scid.push_back(scenes[number].id);
+					poses.push_back(regposes[number]);
+					masks.push_back(mask);
+				}
+			}
+		}
+		delete xmlReader;
+
+		soma_llsd_msgs::Segment segment;
+		if(quasimodo_conversions::add_masks_to_soma_segment(*np,scid, masks, poses,segment)){
+			std::string id = segment.id;
+			for(unsigned int i = 0; i < soma_segment_id_pubs.size(); i++){
+				std_msgs::String msg;
+				msg.data = id;
+				soma_segment_id_pubs[i].publish(msg);
+			}
+		}
+	}
+}
+
+void processSweep(std::string path, std::string savePath){
+
+
+	CloudPtr dyncloud (new Cloud());
+	int ret = processMetaroom(dyncloud,path);
+
+	std_msgs::String msg;
+	if(ret == 0){
+		ROS_ERROR_STREAM("Xml file does not exist. Aborting.");
+		msg.data	= "error_processing_observation";
+	}else{msg.data	= "finished_processing_observation";}
+
+	if(ret == 1){ROS_ERROR_STREAM("First metaroom.");}
+	if(ret == 2){ROS_ERROR_STREAM("No moving objects found.");}
+	if(ret == 3){ROS_ERROR_STREAM("Moving objects found.");}
+
+	sensor_msgs::PointCloud2 input;
+	pcl::toROSMsg (*dyncloud,input);
+	input.header.frame_id = "/map";
+	roomObservationCallback_pubs.publish(input);
+
+	for(unsigned int i = 0; i < m_PublisherStatuss.size(); i++){m_PublisherStatuss[i].publish(msg);}
+
+	//Send the previous room to the modelserver...
+	path = quasimodo_brain::replaceAll(path, "//", "/");
+	int prevind = -1;
+	std::vector<std::string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<pcl::PointXYZRGB>(overall_folder);
+	for (unsigned int i = 0; i < sweep_xmls.size(); i++){
+		sweep_xmls[i] = quasimodo_brain::replaceAll(sweep_xmls[i], "//", "/");
+		if(sweep_xmls[i].compare(path) == 0){break;}
+		prevind = i;
+	}
+
+	if(prevind >= 0){//Submit last metaroom results
+		//Add previous metaroom to soma llsd
+		sendRoomToSomaLLSD(sweep_xmls[prevind]);
+		sendMetaroomToServer(sweep_xmls[prevind]);
+	}
+}
+
+
+
+void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg) {
+
+	std::cout<<"Room obs message received"<<std::endl;
+	processSweep(obs_msg->xml_file_name,"");
+	//processMetaroom
+}
+
+void processSweepForDatabase(std::string path, std::string savePath){
+	path = quasimodo_brain::replaceAll(path, "//", "/");
+	if ( ! boost::filesystem::exists( path ) ){return;}
+
+	int slash_pos = path.find_last_of("/");
+	std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+
+	SimpleXMLParser<pcl::PointXYZRGB> parser;
+	SimpleXMLParser<pcl::PointXYZRGB>::RoomData roomData  = parser.loadRoomFromXML(sweep_folder+"/room.xml",std::vector<std::string>(),false,false);
+
+	std::string current_waypointid = roomData.roomWaypointId;
+
+
+	if(current_waypointid.compare("ReceptionDesk") != 0){return;}
+	//if(current_waypointid.compare("WayPoint5") != 0){return;}
+
+
+	printf("------------------------START--------------------------\n");
+	printf("processSweepForDatabase(%s)\n",path.c_str());
+	//printf("sweep_folder: %s\n",sweep_folder.c_str());
+	printf("current_waypointid: %s\n",current_waypointid.c_str());
+
+
+	//Send the previous room to the modelserver...
+	int firstind = -1;
+	int prevind = -1;
+	std::vector<std::string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<pcl::PointXYZRGB>(overall_folder,false);
+	for (unsigned int i = 0; i < sweep_xmls.size(); i++){
+		sweep_xmls[i] = quasimodo_brain::replaceAll(sweep_xmls[i], "//", "/");
+		SimpleXMLParser<pcl::PointXYZRGB>::RoomData other_roomData  = parser.loadRoomFromXML(sweep_xmls[i],std::vector<std::string>(),false,false);
+
+		//printf("sweep: %s\n",sweep_xmls[i].c_str());
+
+		if(sweep_xmls[i].compare(path) == 0){break;}
+		std::string other_waypointid = other_roomData.roomWaypointId;
+
+		std::string other_path = sweep_xmls[i];
+		int slash_pos = other_path.find_last_of("/");
+		std::string other_sweep_folder = other_path.substr(0, slash_pos) + "/";
+		std::ifstream file (other_sweep_folder+"/superpoints.bin", ios::in|ios::binary|ios::ate);
+		if (file.is_open())	{
+			file.close();
+
+			//Check if it has the superpoint file
+			if(other_waypointid.compare(current_waypointid) == 0){
+				if(firstind == -1){firstind = i;}
+				prevind = i;
+			}
+		}
+	}
+
+	reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath);
+	Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
+	Eigen::Matrix4d change = Eigen::Matrix4d::Identity();
+	if(firstind != -1){
+		std::vector<reglib::superpoint> backgroundsp = quasimodo_brain::getRoomSuperPoints(sweep_xmls[firstind],savePath);
+		if(prevind != -1 && prevind != firstind){
+			std::vector<reglib::superpoint> prev_vec = quasimodo_brain::getRoomSuperPoints(sweep_xmls[prevind],savePath);
+			backgroundsp.insert(backgroundsp.end(), prev_vec.begin(), prev_vec.end());
+		}
+
+		Eigen::Matrix4d currentpose = Eigen::Matrix4d::Identity();
+		roomData  = parser.loadRoomFromXML(sweep_folder+"/room.xml");
+		if(roomData.vIntermediateRoomCloudTransforms.size() != 0){
+			currentpose = quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransforms[0]);
+		}
+
+
+		Eigen::Matrix4d firstpose = Eigen::Matrix4d::Identity();
+		SimpleXMLParser<pcl::PointXYZRGB>::RoomData first_roomData  = parser.loadRoomFromXML(sweep_xmls[firstind]);
+		if(first_roomData.vIntermediateRoomCloudTransforms.size() != 0){
+			firstpose = quasimodo_brain::getMat(first_roomData.vIntermediateRoomCloudTransforms[0]);
+		}
+
+		std::vector<Eigen::Matrix4d> poses;
+		poses.push_back(Eigen::Matrix4d::Identity());
+		poses.push_back(sweep->frames.front()->pose);//Eigen::Matrix4d::Identity());
+
+		reglib::Model * bgdata = new reglib::Model();
+		bgdata->points = backgroundsp;
+
+		reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.1);
+		bgmassreg->timeout = 600;
+		bgmassreg->viewer = viewer;
+		bgmassreg->use_surface = true;
+		bgmassreg->use_depthedge = true;
+		bgmassreg->visualizationLvl = visualization_lvl;//0;
+		bgmassreg->maskstep = 5;
+		bgmassreg->nomaskstep = 5;
+		bgmassreg->nomask = true;
+		bgmassreg->stopval = 0.0005;
+		bgmassreg->addModel(bgdata);
+		bgmassreg->addModel(sweep);
+		reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(poses);
+		change = bgmfr.poses.back() * poses.back().inverse();
+		pose = bgmfr.poses.back();
+	}else{//this is the first room, set to whatever odometry is
+		printf("this is the first room, set to whatever odometry is!\n");
+		roomData  = parser.loadRoomFromXML(sweep_folder+"/room.xml");
+		if(roomData.vIntermediateRoomCloudTransforms.size() != 0){
+			pose = quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransforms[0]);
+		}
+	}
+
+	quasimodo_brain::saveSuperPoints(sweep_folder+"/superpoints.bin",sweep->points,pose,1);
+	for(unsigned int i = 0; i < sweep->frames.size(); i++){
+		sweep->frames[i]->pose *= change;
+	}
+
+	//	string meta_data
+	//	uint32 timestamp
+	//	tf/tfMessage transform
+	//	---
+	//	bool result
+	//	soma_llsd_msgs/Scene response
+
+	ros::ServiceClient client = np->serviceClient<soma_llsd::InsertScene>("/soma_llsd/insert_scene");
+	ROS_INFO("Waiting for /soma_llsd/insert_scene service...");
+	if (!client.waitForExistence(ros::Duration(1.0))) {
+		ROS_INFO("Failed to get /soma_llsd/insert_scene service!");
+		//return;
+	}else{
+		ROS_INFO("Got /soma_llsd/insert_scene service");
+	}
+
+	soma_llsd_msgs::Segment sweepsegment;
+	sweepsegment.id = "sweep"+roomData.roomRunNumber;
+
+	for(unsigned int i = 0; i < sweep->frames.size(); i++){
+		reglib::RGBDFrame * frame = sweep->frames[i];
+		geometry_msgs::Pose		pose;
+		tf::poseEigenToMsg (Eigen::Affine3d(frame->pose), pose);
+
+		geometry_msgs::Pose		segpose;
+		tf::poseEigenToMsg (Eigen::Affine3d(sweep->relativeposes[i]), segpose);
+
+		std::cout << sweep->relativeposes[i] << std::endl;
+
+		cv_bridge::CvImage rgbBridgeImage;
+		rgbBridgeImage.image = frame->rgb;
+		rgbBridgeImage.encoding = "bgr8";
+
+		cv_bridge::CvImage depthBridgeImage;
+		depthBridgeImage.image = frame->depth;
+		depthBridgeImage.encoding = "mono16";
+
+		sensor_msgs::PointCloud2 input;
+		pcl::toROSMsg (*(roomData.vIntermediateRoomClouds[i]),input);//, *transformed_cloud);
+		input.header.frame_id = "/map";
+
+		soma_llsd::InsertScene scene;
+		scene.request.rgb_img = *(rgbBridgeImage.toImageMsg());
+		scene.request.depth_img = *(depthBridgeImage.toImageMsg());
+		scene.request.camera_info.K[0] = frame->camera->fx;
+		scene.request.camera_info.K[4] = frame->camera->fy;
+		scene.request.camera_info.K[2] = frame->camera->cx;
+		scene.request.camera_info.K[5] = frame->camera->cy;
+		scene.request.robot_pose = pose;
+		scene.request.cloud = input;
+		scene.request.waypoint = current_waypointid;
+		scene.request.episode_id = roomData.roomRunNumber;
+
+		//		if (!client.call(scene)) {
+		//			ROS_ERROR("Failed to call service /soma_llsd/insert_scene");
+		//			return;
+		//		}
+
+
+		cv_bridge::CvImage maskBridgeImage;
+		maskBridgeImage.image		= sweep->modelmasks[i]->getMask();
+		maskBridgeImage.encoding	= "mono8";
+
+		soma_llsd_msgs::Observation obs;
+		obs.scene_id = scene.response.response.id;
+		//obs.camera_cloud = model.clouds[counter]; //Dont add clouds...
+		obs.image_mask = *(maskBridgeImage.toImageMsg());
+		obs.pose = segpose;
+		obs.id = "frame"+std::to_string(frame->id);
+		//        obs.timestamp = ros::Time::now().nsec;
+		sweepsegment.observations.push_back(obs);
+	}
+
+
+	//	int startsize = msg.local_poses.size();
+	//	msg.local_poses.resize(startsize+model->relativeposes.size());
+	//	msg.frames.resize(startsize+model->frames.size());
+	//	msg.masks.resize(startsize+model->modelmasks.size());
+	//	for(unsigned int i = 0; i < model->relativeposes.size(); i++){
+	//		geometry_msgs::Pose		pose1;
+	//		tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1);
+	//		geometry_msgs::Pose		pose2;
+	//		tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2);
+	//		cv_bridge::CvImage rgbBridgeImage;
+	//		rgbBridgeImage.image = model->frames[i]->rgb;
+	//		rgbBridgeImage.encoding = "bgr8";
+	//		cv_bridge::CvImage depthBridgeImage;
+	//		depthBridgeImage.image = model->frames[i]->depth;
+	//		depthBridgeImage.encoding = "mono16";
+	//		cv_bridge::CvImage maskBridgeImage;
+	//		maskBridgeImage.image			= model->modelmasks[i]->getMask();
+	//		maskBridgeImage.encoding		= "mono8";
+	//		msg.local_poses[startsize+i]			= pose1;
+	//		msg.frames[startsize+i].capture_time	= ros::Time();
+	//		msg.frames[startsize+i].pose			= pose2;
+	//		msg.frames[startsize+i].frame_id		= model->frames[i]->id;
+	//		msg.frames[startsize+i].rgb				= *(rgbBridgeImage.toImageMsg());
+	//		msg.frames[startsize+i].depth			= *(depthBridgeImage.toImageMsg());
+	//		msg.masks[startsize+i]					= *(maskBridgeImage.toImageMsg());
+
+	//		msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx;
+	//		msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy;
+	//		msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx;
+	//		msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy;
+
+	//		sensor_msgs::PointCloud2 output;
+	//		if(addClouds){pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output);}
+	//		msg.clouds.push_back(output);
+	//	}
+	//	for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){
+	//		addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp,addClouds);
+	//	}
+
+
+	sweep->fullDelete();
+	delete sweep;
+
+	//		int additional_nrviews = 0;
+	//		QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml"));
+	//		for (auto objectFile : objectFiles){
+	//			printf("objectFile: %s\n",(sweep_folder+objectFile.toStdString()).c_str());
+	//			auto object = loadDynamicObjectFromSingleSweep<PointType>(sweep_folder+objectFile.toStdString(),false);
+	//			additional_nrviews += object.vAdditionalViews.size();
+	//		}
+
+	//		SimpleXMLParser<pcl::PointXYZRGB> parser;
+	//		SimpleXMLParser<pcl::PointXYZRGB>::RoomData current_roomData  = parser.loadRoomFromXML(path,std::vector<std::string>{"RoomIntermediateCloud","IntermediatePosition"});
+	//		int metaroom_nrviews = current_roomData.vIntermediateRoomClouds.size();
+
+
+	//		printf("viewgroup_nrviews: %i\n",viewgroup_nrviews);
+	//		printf("additional_nrviews: %i\n",additional_nrviews);
+	//		printf("metaroom_nrviews: %i\n",metaroom_nrviews);
+
+	//		reglib::Model * fullmodel;
+	//		if(viewgroup_nrviews == (additional_nrviews+metaroom_nrviews) && !recomputeRelativePoses){
+	//			printf("time to read old\n");
+	//			fullmodel = new reglib::Model();
+	//			fullmodel->savePath = saveVisuals_sp+"/";
+
+	//			for(unsigned int i = 0; i < viewgroup_nrviews; i++){
+	//				cv::Mat fullmask;
+	//				fullmask.create(480,640,CV_8UC1);
+	//				unsigned char * maskdata = (unsigned char *)fullmask.data;
+	//				for(int j = 0; j < 480*640; j++){maskdata[j] = 255;}
+	//				fullmodel->modelmasks.push_back(new reglib::ModelMask(fullmask));
+	//			}
+
+	//			readViewXML(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes,compute_edges,saveVisuals_sp);
+	//			fullmodel->recomputeModelPoints();
+	//		}else{
+	//			fullmodel = processAV(path,compute_edges,saveVisuals_sp);
+	//			writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes);
+	//		}
+
+	//		return fullmodel;
+	//	}
+
+
+
+	//	quasimodo_msgs::model	model;
+	//	soma_llsd_msgs::Segment segment;
+	//model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment);
+	printf("------------------------STOP--------------------------\n");
+}
+
+bool testPath(std::string path){
+	printf("bool testPath(%s)\n",path.c_str());
+
+	vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(path);
+	for (auto sweep_xml : sweep_xmls) {
+		//printf("sweep_xml: %s\n",sweep_xml.c_str());
+		//processSweep(sweep_xml,"");
+		//processSweepForDatabase(sweep_xml,"");
+
+		sendRoomToSomaLLSD(sweep_xml);
+	}
+	return false;
+}
+
+
+
+void addSceneToLastMetaroom(std::string soma_id){
+	printf("void addSceneToLastMetaroom(%s)\n",soma_id.c_str());
+	vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(overall_folder);
+	std::string lastSweep = sweep_xmls.back();
+
+	int slash_pos = lastSweep.find_last_of("/");
+	std::string sweep_folder = lastSweep.substr(0, slash_pos) + "/";
+
+	printf("last_sweep: %s\n",sweep_folder.c_str());
+
+	ros::ServiceClient client = np->serviceClient<soma_llsd::GetScene>("/soma_llsd/get_scene");
+	ROS_INFO("Waiting for /soma_llsd/get_scene service...");
+	if (!client.waitForExistence(ros::Duration(1.0))) {
+		ROS_INFO("Failed to get /soma_llsd/get_scene service!");
+		return;
+	}
+	ROS_INFO("Got /soma_llsd/get_scene service");
+
+	soma_llsd::GetScene srv;
+	srv.request.scene_id = soma_id;
+	if (!client.call(srv)) {
+		ROS_ERROR("Failed to call service /soma_llsd/get_scene");
+		return;
+	}
+	ROS_INFO("Got /soma_llsd/get_scene");
+
+	reglib::RGBDFrame * frame = quasimodo_brain::getFrame(srv.response.response);
+	printf("frame->soma_id: %s\n",frame->soma_id.c_str());
+//	soma_llsd_msgs::Scene sc = quasimodo_brain::getScene(*np,frame,"","");
+//	printf("sc.id: %s\n",sc.id.c_str());
+
+	char buf [1024];
+	int counter = 0;
+	while(true){
+		sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),counter);
+		string tmp = string(buf)+"_data.txt";
+		QFile file(tmp.c_str());
+		if (!file.exists()){break;}
+		counter++;
+	}
+	printf("save to %s\n",buf);
+	frame->save(std::string(buf));
+	frame->camera->save(std::string(buf)+"_camera");
+	delete frame->camera;
+	delete frame;
+}
+
+void add_soma_id_callback(const std_msgs::String::ConstPtr& msg){
+	printf("================================================================================\n");
+	printf("=============================add_soma_id_callback===============================\n");
+	printf("================================================================================\n");
+	if(msg->data.compare("done") == 0){
+		vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(overall_folder);
+		if(sweep_xmls.size() == 0){return ;}
+		std::string lastSweep = sweep_xmls.back();
+		//if(prevind >= 0){
+			sendRoomToSomaLLSD(lastSweep);
+			sendMetaroomToServer(lastSweep);
+		//}
+	}else{
+		addSceneToLastMetaroom(msg->data);
+	}
+}
+
+
+void setLargeStack(){
+	const rlim_t kStackSize = 256 * 1024 * 1024;   // min stack size = 256 MB
+	struct rlimit rl;
+	unsigned long result;
+
+	result = getrlimit(RLIMIT_STACK, &rl);
+	if (result == 0){
+		if (rl.rlim_cur < kStackSize){
+			rl.rlim_cur = kStackSize;
+			result = setrlimit(RLIMIT_STACK, &rl);
+			if (result != 0){fprintf(stderr, "setrlimit returned result = %d\n", int(result));}
+		}
+	}
+}
+
+int main(int argc, char** argv){
+
+	bool baseSetting = true;
+	bool once = false;
+
+	overall_folder	= std::string(getenv ("HOME"))+"/.semanticMap/";
+	outtopic		= "/some/topic";
+	modelouttopic	= "/model/out/topic";
+	posepath		= "testposes.xml";
+
+	ros::init(argc, argv, "metaroom_additional_view_processing");
+	ros::NodeHandle n;
+	np = &n;
+
+	std::vector<ros::Subscriber> segsubs;
+	std::vector<ros::Subscriber> sendsubs;
+	std::vector<ros::Subscriber> roomObservationSubs;
+	std::vector<ros::Subscriber> processAndSendsubs;
+	std::vector<ros::Subscriber> soma_id_subs;
+
+
+	std::vector<std::string> trainMetarooms;
+	std::vector<std::string> sendMetaroomToServers;
+	std::vector<std::string> processMetarooms;
+	std::vector<std::string> testpaths;
+
+	std::vector<bool>		 raresfiles_resegment;
+	std::vector<std::string> raresfiles;
+
+	bool resegment					= false;
+	int inputstate = 0;
+	for(int i = 1; i < argc;i++){
+		printf("input: %s\n",argv[i]);
+		if(		std::string(argv[i]).compare("-intopic") == 0){						inputstate = 0;}
+		else if(std::string(argv[i]).compare("-outtopic") == 0){					inputstate = 1;}
+		else if(std::string(argv[i]).compare("-file") == 0){						inputstate = 2;}
+		else if(std::string(argv[i]).compare("-v") == 0){	visualization_lvl = 1;	inputstate = 3;}
+		else if(std::string(argv[i]).compare("-folder") == 0){						inputstate	= 4;}
+		else if(std::string(argv[i]).compare("-train") == 0){						inputstate	= 5;}
+		else if(std::string(argv[i]).compare("-posepath") == 0){					inputstate	= 6;}
+		else if(std::string(argv[i]).compare("-loadposes") == 0){					inputstate	= 7;}
+		else if(std::string(argv[i]).compare("-sendModel") == 0){					inputstate	= 8;}
+		else if(std::string(argv[i]).compare("-sendSub") == 0)	{					inputstate	= 9;}
+		else if(std::string(argv[i]).compare("-sendTopic") == 0){					inputstate	= 10;}
+		else if(std::string(argv[i]).compare("-roomObservationTopic") == 0){		inputstate	= 11;}
+		else if(std::string(argv[i]).compare("-DynamicObjectsService") == 0){		inputstate	= 12;}
+		else if(std::string(argv[i]).compare("-GetDynamicObjectService") == 0){		inputstate	= 13;}
+		else if(std::string(argv[i]).compare("-statusmsg") == 0){					inputstate	= 14;}
+		else if(std::string(argv[i]).compare("-files") == 0){						inputstate	= 15;}
+		else if(std::string(argv[i]).compare("-baseSweep") == 0){					inputstate	= 16;}
+		else if(std::string(argv[i]).compare("-resegment") == 0){					resegment	= true;}
+		else if(std::string(argv[i]).compare("-once") == 0){						once		= true;}
+		else if(std::string(argv[i]).compare("-nobase") == 0){						baseSetting = false;}
+		else if(std::string(argv[i]).compare("-recomputeRelativePoses") == 0){		recomputeRelativePoses = true;}
+		else if(std::string(argv[i]).compare("-v_init") == 0){	visualization_lvl_regini = 1;inputstate = 17;}
+		else if(std::string(argv[i]).compare("-v_reg") == 0){	visualization_lvl_regref = 1;inputstate = 18;}
+		else if(std::string(argv[i]).compare("-saveVisuals") == 0){					inputstate = 19;}
+		else if(std::string(argv[i]).compare("-testpaths") == 0 || std::string(argv[i]).compare("-testpath") == 0 || std::string(argv[i]).compare("-testPaths") == 0 || std::string(argv[i]).compare("-testPath") == 0){					inputstate = 20;}
+		else if(std::string(argv[i]).compare("-minClusterSize") == 0){					inputstate = 21;}
+		else if(inputstate == 0){
+			segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback));
+		}else if(inputstate == 1){
+			out_pubs.push_back(n.advertise<std_msgs::String>(std::string(argv[i]), 1000));
+		}else if(inputstate == 2){
+			processMetarooms.push_back(std::string(argv[i]));
+		}else if(inputstate == 3){
+			visualization_lvl = atoi(argv[i]);
+		}else if(inputstate == 4){
+			overall_folder = std::string(argv[i]);
+		}else if(inputstate == 5){
+			trainMetarooms.push_back(std::string(argv[i]));
+		}else if(inputstate == 6){
+			posepath = std::string(argv[i]);
+		}else if(inputstate == 7){
+			sweepPoses = quasimodo_brain::readPoseXML(std::string(argv[i]));
+		}else if(inputstate == 8){
+			sendMetaroomToServers.push_back(std::string(argv[i]));
+		}else if(inputstate == 9){
+			sendsubs.push_back(n.subscribe(std::string(argv[i]), 1000, sendCallback));
+		}else if(inputstate == 10){
+			model_pubs.push_back(n.advertise<quasimodo_msgs::model>(std::string(argv[i]), 1000));
+		}else if(inputstate == 11){
+			roomObservationSubs.push_back(n.subscribe(std::string(argv[i]), 1000, roomObservationCallback));
+		}else if(inputstate == 12){
+			m_DynamicObjectsServiceServers.push_back(n.advertiseService(std::string(argv[i]), dynamicObjectsServiceCallback));
+		}else if(inputstate == 13){
+			m_GetDynamicObjectServiceServers.push_back(n.advertiseService(std::string(argv[i]), getDynamicObjectServiceCallback));
+		}else if(inputstate == 14){
+			m_PublisherStatuss.push_back(n.advertise<std_msgs::String>(std::string(argv[i]), 1000));
+		}else if(inputstate == 15){
+			raresfiles.push_back(std::string(argv[i]));
+			raresfiles_resegment.push_back(resegment);
+		}else if(inputstate == 16){
+			setBaseSweep(std::string(argv[i]));
+		}else if(inputstate == 17){
+			visualization_lvl_regini = atoi(argv[i]);
+		}else if(inputstate == 18){
+			visualization_lvl_regref = atoi(argv[i]);
+		}else if(inputstate == 19){
+			saveVisuals = std::string(argv[i]);
+		}else if(inputstate == 20){
+			testpaths.push_back(std::string(argv[i]));
+		}else if(inputstate == 21){
+			minClusterSize  = atoi(argv[i]);
+		}
+	}
+
+	printf("done reading commands.\n");
+
+	if(visualization_lvl != 0 || visualization_lvl_regref != 0 || visualization_lvl_regini != 0){
+		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
+		viewer->setBackgroundColor (0.5, 0, 0.5);
+		viewer->addCoordinateSystem (1.0);
+		viewer->initCameraParameters ();
+	}
+
+	if(baseSetting){
+		if(m_PublisherStatuss.size() == 0){
+			m_PublisherStatuss.push_back(n.advertise<std_msgs::String>("/local_metric_map/status", 1000));
+		}
+
+		if(segsubs.size() == 0){
+			segsubs.push_back(n.subscribe("/quasimodo/segmentation/in", 1000, chatterCallback));
+		}
+
+		if(out_pubs.size() == 0){
+			out_pubs.push_back(n.advertise<std_msgs::String>("/quasimodo/segmentation/out/path", 1000));
+		}
+
+		if(model_pubs.size() == 0){
+			model_pubs.push_back(n.advertise<quasimodo_msgs::model>("/quasimodo/segmentation/out/model", 1000));
+		}
+
+		if(sendsubs.size() == 0){
+			sendsubs.push_back(n.subscribe("/quasimodo/segmentation/send", 1000, sendCallback));
+		}
+
+		if(roomObservationSubs.size() == 0){
+			roomObservationSubs.push_back(n.subscribe("/local_metric_map/room_observations", 1000, roomObservationCallback));
+		}
+
+		if(processAndSendsubs.size() == 0){
+			processAndSendsubs.push_back(n.subscribe("/object_learning/learned_object_xml", 1000, chatterCallback));//processAndSendCallback));
+		}
+
+		if(soma_segment_id_pubs.size() == 0){
+			soma_segment_id_pubs.push_back(n.advertise<std_msgs::String>("/quasimodo/segmentation/out/soma_segment_id", 1000));
+		}
+
+		if(soma_id_subs.size() == 0){
+			soma_id_subs.push_back(n.subscribe("/quasimodo/segmentation/in/soma_segment_id", 1000,add_soma_id_callback));
+			soma_id_subs.push_back(n.subscribe("/surface_based_object_learning/scenes", 1000,add_soma_id_callback));
+		}
+	}
+
+
+	roomObservationCallback_pubs = n.advertise<sensor_msgs::PointCloud2>("/quasimodo/segmentation/roomObservation/dynamic_clusters", 1000);
+
+	printf("overall_folder: %s\n",overall_folder.c_str());
+
+//	addSceneToLastMetaroom("ed5f22eb-e6c0-426c-b905-4800780ca596");
+
+	printf("testpaths: %i\n",testpaths.size());
+	for(unsigned int i = 0; i < testpaths.size(); i++){
+		testPath(testpaths[i]);
+	}
+
+
+	for(unsigned int i = 0; i < raresfiles.size(); i++){segmentRaresFiles(		raresfiles[i], raresfiles_resegment[i]);}
+	for(unsigned int i = 0; i < trainMetarooms.size(); i++){		trainMetaroom(			trainMetarooms[i]);}
+	for(unsigned int i = 0; i < processMetarooms.size(); i++){
+		CloudPtr dyncloud (new Cloud());
+		processMetaroom(dyncloud,processMetarooms[i]);
+	}
+	for(unsigned int i = 0; i < sendMetaroomToServers.size(); i++){
+		vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(sendMetaroomToServers[i]);
+		for (auto sweep_xml : sweep_xmls) {
+			printf("sweep_xml: %s\n",sweep_xml.c_str());
+			quasimodo_brain::cleanPath(sweep_xml);
+			int slash_pos = sweep_xml.find_last_of("/");
+			std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/";
+			sendMetaroomToServer(sweep_folder);
+		}
+	}
+
+	if(!once){ros::spin();}
+	printf("done...\n");
+	return 0;
+}
diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp
index 10b95d35..4b627df3 100644
--- a/quasimodo/quasimodo_brain/src/modelserver.cpp
+++ b/quasimodo/quasimodo_brain/src/modelserver.cpp
@@ -15,12 +15,11 @@
 #include "quasimodo_msgs/retrieval_query_result.h"
 #include "quasimodo_msgs/retrieval_query.h"
 #include "quasimodo_msgs/retrieval_result.h"
-#include "soma2_msgs/SOMA2Object.h"
+#include "quasimodo_msgs/recognize.h"
+#include "soma_msgs/SOMAObject.h"
 
-#include "soma_manager/SOMA2InsertObjs.h"
+#include "soma_manager/SOMAInsertObjs.h"
 
-//#include "modelupdater/ModelUpdater.h"
-//#include "/home/johane/catkin_ws_dyn/src/quasimodo_models/include/modelupdater/ModelUpdater.h"
 #include "modelupdater/ModelUpdater.h"
 #include "core/RGBDFrame.h"
 #include <sensor_msgs/PointCloud2.h>
@@ -34,15 +33,10 @@
 #include <sensor_msgs/image_encodings.h>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/highgui/highgui.hpp>
-
 #include <map>
-
 #include "ModelDatabase/ModelDatabase.h"
-
 #include <thread>
-
 #include <sys/time.h>
-
 #include <sys/types.h>
 #include <dirent.h>
 #include <errno.h>
@@ -50,7 +44,25 @@
 #include <string>
 #include <iostream>
 
+#include "Util/Util.h"
+
+bool addToDB(ModelDatabase * database, reglib::Model * model, bool add);
+bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Model * model2);
+void addNewModel(reglib::Model * model);
+
+using namespace quasimodo_brain;
+
 bool visualization = false;
+bool show_db = false;//Full database show
+bool save_db = false;//Full database save
+int save_db_counter = 0;
+
+int show_init_lvl = 0;//init show
+int show_refine_lvl = 0;//refine show
+int show_reg_lvl = 0;//registration show
+bool show_scoring = false;//fuse scoring show
+bool show_search = false;
+bool show_modelbuild = false;
 
 
 std::map<int , reglib::Camera *>		cameras;
@@ -58,9 +70,11 @@ std::map<int , reglib::RGBDFrame *>		frames;
 
 std::map<int , reglib::Model *>			models;
 std::map<int , reglib::ModelUpdater *>	updaters;
-reglib::RegistrationRandom *				registration;
-ModelDatabase * 						modeldatabase;
 
+std::set<std::string>					framekeys;
+reglib::RegistrationRandom *			registration;
+ModelDatabase * 						modeldatabase;
+ros::NodeHandle *						nh;
 std::string								savepath = ".";
 
 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
@@ -68,26 +82,25 @@ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
 ros::Publisher models_new_pub;
 ros::Publisher models_updated_pub;
 ros::Publisher models_deleted_pub;
-
 ros::Publisher model_pcd_pub;
 ros::Publisher database_pcd_pub;
+ros::Publisher model_history_pub;
+ros::Publisher model_places_pub;
+ros::Publisher model_last_pub;
 ros::Publisher chatter_pub;
 
-ros::ServiceClient soma2add;
-
-double occlusion_penalty = 15;
-double massreg_timeout = 60;
+ros::ServiceClient retrieval_client;
+ros::ServiceClient conversion_client;
+ros::ServiceClient insert_client;
 
-bool run_search = false;
-double search_timeout = 30;
+double occlusion_penalty	= 10;
+double massreg_timeout		= 120;
+bool run_search				= false;
+double search_timeout		= 30;
+int sweepid_counter			= 0;
+int current_model_update	= 0;
 
-bool myfunction (reglib::Model * i,reglib::Model * j) { return i->frames.size() > j->frames.size(); }
-
-double getTime(){
-	struct timeval start1;
-	gettimeofday(&start1, NULL);
-	return double(start1.tv_sec+(start1.tv_usec/1000000.0));
-}
+bool myfunction (reglib::Model * i,reglib::Model * j) { return (i->frames.size() + i->submodels.size())  > (j->frames.size() + j->submodels.size()); }
 
 quasimodo_msgs::retrieval_result sresult;
 bool new_search_result = false;
@@ -102,7 +115,7 @@ void publishDatabasePCD(bool original_colors = false){
 
 	float maxx = 0;
 	for(unsigned int i = 0; i < results.size(); i++){
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = results[i]->getPCLcloud(1, original_colors);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = results[i]->getPCLcloud(1, false);
 		float meanx = 0;
 		float meany = 0;
 		float meanz = 0;
@@ -122,66 +135,64 @@ void publishDatabasePCD(bool original_colors = false){
 		}
 
 		float minx = 100000000000;
-
 		for(unsigned int j = 0; j < cloud->points.size(); j++){minx = std::min(cloud->points[j].x , minx);}
 		for(unsigned int j = 0; j < cloud->points.size(); j++){cloud->points[j].x += maxx-minx + 0.15;}
 		for(unsigned int j = 0; j < cloud->points.size(); j++){maxx = std::max(cloud->points[j].x,maxx);}
-
 		for(unsigned int j = 0; j < cloud->points.size(); j++){conccloud->points.push_back(cloud->points[j]);}
 	}
 
 	sensor_msgs::PointCloud2 input;
 	pcl::toROSMsg (*conccloud,input);//, *transformed_cloud);
-	input.header.frame_id = "/db_frame";
+	input.header.frame_id = "/map";
 	database_pcd_pub.publish(input);
 }
 
-void publishObject(int id){
-
+void publish_history(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> history){
+	for(unsigned int i = 0; i < history.size(); i++){
+		sensor_msgs::PointCloud2 input;
+		pcl::toROSMsg (*history[i],input);//, *transformed_cloud);
+		input.header.frame_id = "/map";
+		model_history_pub.publish(input);
+	}
 }
 
 void dumpDatabase(std::string path = "."){
+	printf("%i\n",__LINE__);
 	char command [1024];
-	sprintf(command,"rm -r -f %s/quasimodo_model*",path.c_str());
-	printf("%s\n",command);
+	sprintf(command,"rm -r -f %s/database_tmp",path.c_str());
 	int r = system(command);
+	printf("%i\n",__LINE__);
 
-	sprintf(command,"rm -f %s/quasimodo_camera*",path.c_str());
-	printf("%s\n",command);
+	sprintf(command,"mkdir %s/database_tmp",path.c_str());
 	r = system(command);
+	printf("%i\n",__LINE__);
 
-	cameras[0]->save(path+"/quasimodo_camera0");
 	for(unsigned int m = 0; m < modeldatabase->models.size(); m++){
+		printf("%i\n",__LINE__);
 		char buf [1024];
-		sprintf(buf,"%s/quasimodo_model%i",path.c_str(),m);
+		sprintf(buf,"%s/database_tmp/model_%08i",path.c_str(),m);
 		sprintf(command,"mkdir -p %s",buf);
+		printf("%i\n",__LINE__);
 		r = system(command);
-
-		sprintf(command,"mkdir -p %s/views",buf);
-		r = system(command);
-
+		printf("%i\n",__LINE__);
 		modeldatabase->models[m]->save(std::string(buf));
+		printf("%i\n",__LINE__);
 	}
+	//exit(0);
 }
 
 void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){
-	printf("retrievalCallback\n");
-	sresult = qr.result;
-	new_search_result = true;
-	last_search_result_time = getTime();
+    printf("retrievalCallback\n");
+    sresult = qr.result;
+    new_search_result = true;
+    last_search_result_time = getTime();
 }
 
-
-int savecounter = 0;
-void show_sorted(){
-	if(!visualization){return;}
-	std::vector<reglib::Model *> results;
-	for(unsigned int i = 0; i < modeldatabase->models.size(); i++){results.push_back(modeldatabase->models[i]);}
-	std::sort (results.begin(), results.end(), myfunction);
-	viewer->removeAllPointClouds();
+void showModels(std::vector<reglib::Model *> mods){
 	float maxx = 0;
-	for(unsigned int i = 0; i < results.size(); i++){
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = results[i]->getPCLcloud(1, false);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr	conccloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
+	for(unsigned int i = 0; i < mods.size(); i++){
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = mods[i]->getPCLcloud(1, false);
 		float meanx = 0;
 		float meany = 0;
 		float meanz = 0;
@@ -205,170 +216,56 @@ void show_sorted(){
 		for(unsigned int j = 0; j < cloud->points.size(); j++){minx = std::min(cloud->points[j].x , minx);}
 		for(unsigned int j = 0; j < cloud->points.size(); j++){cloud->points[j].x += maxx-minx + 0.15;}
 		for(unsigned int j = 0; j < cloud->points.size(); j++){maxx = std::max(cloud->points[j].x,maxx);}
-		char buf [1024];
-		sprintf(buf,"cloud%i",i);
-		viewer->addPointCloud<pcl::PointXYZRGB> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(cloud), buf);
-	}
-	viewer->spin();
-	//char buf [1024];
-	//sprintf(buf,"globalimg%i.png",savecounter++);
-	//viewer->saveScreenshot(std::string(buf));
-	//viewer->spinOnce();
-}
-
-quasimodo_msgs::model getModelMSG(reglib::Model * model){
-	quasimodo_msgs::model msg;
-	msg.model_id = model->id;
-	msg.local_poses.resize(model->relativeposes.size());
-	msg.frames.resize(model->frames.size());
-	msg.masks.resize(model->modelmasks.size());
-	for(unsigned int i = 0; i < model->relativeposes.size(); i++){
-		geometry_msgs::Pose		pose1;
-		tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i]), pose1);
-		geometry_msgs::Pose		pose2;
-		tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose), pose2);
-		cv_bridge::CvImage rgbBridgeImage;
-		rgbBridgeImage.image = model->frames[i]->rgb;
-		rgbBridgeImage.encoding = "bgr8";
-		cv_bridge::CvImage depthBridgeImage;
-		depthBridgeImage.image = model->frames[i]->depth;
-		depthBridgeImage.encoding = "mono16";
-		cv_bridge::CvImage maskBridgeImage;
-		maskBridgeImage.image			= model->modelmasks[i]->getMask();
-		maskBridgeImage.encoding		= "mono8";
-		msg.local_poses[i]			= pose1;
-		msg.frames[i].capture_time	= ros::Time();
-		msg.frames[i].pose			= pose2;
-		msg.frames[i].frame_id		= model->frames[i]->id;
-		msg.frames[i].rgb			= *(rgbBridgeImage.toImageMsg());
-		msg.frames[i].depth			= *(depthBridgeImage.toImageMsg());
-		msg.masks[i]				= *(maskBridgeImage.toImageMsg());//getMask()
+		*conccloud += *cloud;
 	}
-	return msg;
-}
 
-std::vector<soma2_msgs::SOMA2Object> getSOMA2ObjectMSGs(reglib::Model * model){
-	std::vector<soma2_msgs::SOMA2Object> msgs;
-	for(unsigned int i = 0; i < model->frames.size(); i++){
-		soma2_msgs::SOMA2Object msg;
+	if( save_db ){
+		printf("save_db\n");
 		char buf [1024];
-		sprintf(buf,"id_%i_%i",int(model->id),int(model->frames[i]->id));
-		msg.id				= std::string(buf);
-		msg.map_name		= "whatevermapname";				//	#### the global map that the object belongs. Automatically filled by SOMA2 insert service
-		msg.map_unique_id	= "";								// 	#### the unique db id of the map. Automatically filled by SOMA2 insert service
-		msg.config			= "configid";						//	#### the corresponding world configuration. This could be incremented as config1, config2 at each meta-room observation
-		msg.mesh			= "";								//	#### mesh model of the object. Could be left blank
-		sprintf(buf,"type_%i",int(model->id));
-		msg.type			= std::string(buf);					//	#### type of the object. For higher level objects, this could chair1, chair2. For middle or lower level segments it could be segment1101, segment1102, etc.
-		//		msg.waypoint		= "";								//	#### the waypoint id. Could be left blank
-		msg.timestep 		= 0;								//	#### this is the discrete observation instance. This could be incremented at each meta-room observation as 1,2,3,etc...
-		msg.logtimestamp	= 1000000.0 * double(model->frames[i]->capturetime);	//	#### this is the unix timestamp information that holds the time when the object is logged. SOMA2 uses UTC time values.
-
-		Eigen::Affine3f transform = Eigen::Affine3f(model->frames[i]->pose.cast<float>());//Eigen::Affine3f::Identity();
-
-		bool * maskdata = model->modelmasks[i]->maskvec;
-		unsigned char * rgbdata = (unsigned char *) model->frames[i]->rgb.data;
-		unsigned short * depthdata = (unsigned short *) model->frames[i]->depth.data;
-
-		const unsigned int width	= model->frames[i]->camera->width; const unsigned int height	= model->frames[i]->camera->height;
-		const double idepth			= model->frames[i]->camera->idepth_scale;
-		const double cx				= model->frames[i]->camera->cx;		const double cy				= model->frames[i]->camera->cy;
-		const double ifx			= 1.0/model->frames[i]->camera->fx;	const double ify			= 1.0/model->frames[i]->camera->fy;
-
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr	transformed_cloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
-		for(unsigned int w = 0; w < width; w++){
-			for(unsigned int h = 0; h < height;h++){
-				int ind = h*width+w;
-				double z = idepth*double(depthdata[ind]);
-				if(z > 0 && maskdata[ind]){
-					pcl::PointXYZRGB p;
-					p.x = (double(w) - cx) * z * ifx;
-					p.y = (double(h) - cy) * z * ify;
-					p.z = z;
-					p.b = rgbdata[3*ind+0];
-					p.g = rgbdata[3*ind+1];
-					p.r = rgbdata[3*ind+2];
-					cloud->points.push_back(p);
-				}
-			}
-		}
-
-		pcl::transformPointCloud (*cloud, *transformed_cloud, transform);
-		sensor_msgs::PointCloud2ConstPtr input;
-		pcl::fromROSMsg (*input, *transformed_cloud);
-
-
-		msg.cloud = *input; //#### The 3D cloud of the object		(with respect to /map frame)
-
-		Eigen::Quaterniond q = Eigen::Quaterniond(Eigen::Affine3d(model->frames[i]->pose).rotation());
-
-		geometry_msgs::Pose		pose;
-		pose.orientation.x	= q.x();
-		pose.orientation.y	= q.y();
-		pose.orientation.z	= q.z();
-		pose.orientation.w	= q.w();
-		pose.position.x		= model->frames[i]->pose(0,3);
-		pose.position.y		= model->frames[i]->pose(1,3);
-		pose.position.z		= model->frames[i]->pose(2,3);
-		msg.pose = pose;		//	#### Object pose in 3D			(with respect to /map frame)
-		//		geometry_msgs/Pose sweepCenter 	#### The pose of the robot during sweep (with respect to /map frame)
-		msgs.push_back(msg);
+		sprintf(buf,"quasimodoDB_%i.pcd",save_db_counter);
+		pcl::io::savePCDFileBinaryCompressed(buf, *conccloud);
+		save_db_counter++;
 	}
 
-	//	msg.model_id = model->id;
-	//	msg.local_poses.resize(model->relativeposes.size());
-	//	msg.frames.resize(model->frames.size());
-	//	msg.masks.resize(model->modelmasks.size());
-	//	for(unsigned int i = 0; i < model->relativeposes.size(); i++){
-	//		geometry_msgs::Pose		pose1;
-	//		tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i]), pose1);
-	//		geometry_msgs::Pose		pose2;
-	//		tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose), pose2);
-	//		cv_bridge::CvImage rgbBridgeImage;
-	//		rgbBridgeImage.image = model->frames[i]->rgb;
-	//		rgbBridgeImage.encoding = "bgr8";
-	//		cv_bridge::CvImage depthBridgeImage;
-	//		depthBridgeImage.image = model->frames[i]->depth;
-	//		depthBridgeImage.encoding = "mono16";
-	//		cv_bridge::CvImage maskBridgeImage;
-	//		maskBridgeImage.image			= model->modelmasks[i]->getMask();
-	//		maskBridgeImage.encoding		= "mono8";
-	//		msg.local_poses[i]			= pose1;
-	//		msg.frames[i].capture_time	= ros::Time();
-	//		msg.frames[i].pose			= pose2;
-	//		msg.frames[i].frame_id		= model->frames[i]->id;
-	//		msg.frames[i].rgb			= *(rgbBridgeImage.toImageMsg());
-	//		msg.frames[i].depth			= *(depthBridgeImage.toImageMsg());
-	//		msg.masks[i]				= *(maskBridgeImage.toImageMsg());//getMask()
-	//	}
-	return msgs;
+	if(show_db && visualization){
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGB> (conccloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(conccloud), "conccloud");
+		viewer->spin();
+	}
 }
 
-void publishModel(reglib::Model * model){
-	//	std::vector<soma2_msgs::SOMA2Object> somamsgs = getSOMA2ObjectMSGs(model);
-	soma_manager::SOMA2InsertObjs objs;
-	objs.request.objects = getSOMA2ObjectMSGs(model);
-	if (soma2add.call(objs)){//Add frame to model server
-		////			int frame_id = ifsrv.response.frame_id;
-	}else{ROS_ERROR("Failed to call service soma2add");}
-
-	//	for(int i = 0; i < somamsgs.size(); i++){
-	//		if (soma2add.call(somamsgs[i])){//Add frame to model server
-	////			int frame_id = ifsrv.response.frame_id;
-	//		 }else{ROS_ERROR("Failed to call service soma2add");}
-	//	 }
+int savecounter = 0;
+void show_sorted(){
+	if(!show_db && !save_db ){return;}
+	//if(!visualization){return;}
+	std::vector<reglib::Model *> results;
+	for(unsigned int i = 0; i < modeldatabase->models.size(); i++){results.push_back(modeldatabase->models[i]);}
+	std::sort (results.begin(), results.end(), myfunction);
+	showModels(results);
 }
 
+
 bool getModel(quasimodo_msgs::get_model::Request  & req, quasimodo_msgs::get_model::Response & res){
-	int model_id			= req.model_id;
-	reglib::Model * model	= models[model_id];
-	res.model = getModelMSG(model);
-	return true;
+    int model_id			= req.model_id;
+    reglib::Model * model	= models[model_id];
+    res.model = getModelMSG(model);
+    return true;
+}
+
+bool recognizeService(quasimodo_msgs::recognize::Request  & req, quasimodo_msgs::recognize::Response & res){
+    reglib::Model * mod = quasimodo_brain::getModelFromSegment(*nh, req.id);
+	addNewModel(mod);
+	reglib::Model * p = mod->parrent;
+	if((p == 0) || ((p->frames.size() == 0) && (p->submodels.size() == 1))){
+		req.id = "";
+		return false;
+	}else{
+		req.id = p->soma_id;
+		return true;
+	}
 }
 
 bool indexFrame(quasimodo_msgs::index_frame::Request  & req, quasimodo_msgs::index_frame::Response & res){
-	//printf("indexFrame\n");
 	sensor_msgs::CameraInfo		camera			= req.frame.camera;
 	ros::Time					capture_time	= req.frame.capture_time;
 	geometry_msgs::Pose			pose			= req.frame.pose;
@@ -386,495 +283,215 @@ bool indexFrame(quasimodo_msgs::index_frame::Request  & req, quasimodo_msgs::ind
 	Eigen::Affine3d epose;
 	tf::poseMsgToEigen(pose, epose);
 
-	//printf("%s LINE:%i\n",__FILE__,__LINE__);
-	reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix());
-	//printf("%s LINE:%i\n",__FILE__,__LINE__);
+	reglib::Camera * cam		= new reglib::Camera();//TODO:: ADD TO CAMERAS
+	cam->fx = camera.K[0];
+	cam->fy = camera.K[4];
+	cam->cx = camera.K[2];
+	cam->cy = camera.K[5];
+
+	reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix());
 	frames[frame->id] = frame;
 	res.frame_id = frame->id;
 	return true;
 }
 
-
-class SearchResult{
-public:
-	double score;
-	reglib::Model * model;
-	Eigen::Affine3d pose;
-
-	SearchResult(double score_, reglib::Model * model_, Eigen::Affine3d pose_){
-		score	= score_;
-		model	= model_;
-		pose	= pose_;
-	}
-	~SearchResult(){}
-};
-
-bool removeModel(int id){
-	//printf("removeModel: %i\n",id);
-	delete models[id];
-	models.erase(id);
-	delete updaters[id];
-	updaters.erase(id);
-}
-
-
-reglib::Model * mod;
-std::vector<reglib::Model * > res;
-std::vector<reglib::FusionResults > fr_res;
-
-void call_from_thread(int i) {
-	reglib::Model * model2 = res[i];
-
-	printf("testreg %i to %i\n",int(mod->id),int(model2->id));
-	reglib::RegistrationRandom *	reg		= new reglib::RegistrationRandom();
+bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Model * model2){
+	reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
 	reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( model2, reg);
 	mu->occlusion_penalty               = occlusion_penalty;
 	mu->massreg_timeout                 = massreg_timeout;
 	mu->viewer							= viewer;
-	reg->visualizationLvl				= 0;
-
-	reglib::FusionResults fr = mu->registerModel(mod);
-	fr_res[i] = fr;
+	mu->show_init_lvl					= show_init_lvl;//init show
+	mu->show_refine_lvl					= show_refine_lvl;//refine show
+	mu->show_scoring					= show_scoring;//fuse scoring show
+	reg->visualizationLvl				= show_reg_lvl;
+
+	reglib::FusionResults fr = mu->registerModel(model);
+	if(fr.score > 100){
+		reglib::UpdatedModels ud = mu->fuseData(&fr, model2, model);
+		delete mu;
+		delete reg;
 
-	delete mu;
-	delete reg;
+		if(ud.deleted_models.size() > 0 || ud.updated_models.size() > 0 || ud.new_models.size() > 0){
+			for(unsigned int j = 0; j < ud.deleted_models.size();	j++){
+				database->remove(ud.deleted_models[j]);
+				delete ud.deleted_models[j];
+			}
+			for(unsigned int j = 0; j < ud.updated_models.size();	j++){
+				database->remove(ud.updated_models[j]);
+				models_deleted_pub.publish(getModelMSG(ud.updated_models[j]));
+			}
+			for(unsigned int j = 0; j < ud.updated_models.size();	j++){	addToDB(database, ud.updated_models[j], true);}
+			for(unsigned int j = 0; j < ud.new_models.size();	j++){		addToDB(database, ud.new_models[j],		true);}
+			return true;
+		}
+	}else{
+		delete mu;
+		delete reg;
+	}
+	return false;
 }
 
-
-int current_model_update = 0;
-void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, bool deleteIfFail = false){
-	printf("addToDB %i %i\n",int(add),int(deleteIfFail));
+bool addToDB(ModelDatabase * database, reglib::Model * model, bool add){// = true){, bool deleteIfFail = false){
+printf("start: %s\n",__PRETTY_FUNCTION__);
 	if(add){
-
-		if(model->frames.size() > 2){
+		if(model->submodels.size() > 2){
 			reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
 			reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( model, 0);
 			mu->occlusion_penalty               = occlusion_penalty;
 			mu->massreg_timeout                 = massreg_timeout;
 			mu->viewer							= viewer;
 			reg->visualizationLvl				= 0;
-			mu->refine(0.001,true);
+			mu->show_init_lvl = show_init_lvl;//init show
+			mu->show_refine_lvl = show_refine_lvl;//refine show
+			mu->show_scoring = show_scoring;//fuse scoring show
+			mu->refine(0.001,false,0);
 			delete mu;
 			delete reg;
 		}
 		database->add(model);
-		//models_new_pub.publish(getModelMSG(model));
 		model->last_changed = ++current_model_update;
-		//		show_sorted();
 	}
 
-	mod = model;
-	res = modeldatabase->search(model,150);
-	fr_res.resize(res.size());
+	std::vector<reglib::Model * > res = modeldatabase->search(model,1);
+	if(show_search){showModels(res);}
 
-	if(res.size() == 0){
-		printf("no candidates found in database!\n");
-		return;
-	}
-
-	bool changed = false;
-	std::map<int , reglib::Model *>	new_models;
-	std::map<int , reglib::Model *>	updated_models;
-	std::vector<reglib::Model * > models2merge;
-	std::vector<reglib::FusionResults > fr2merge;
-	bool multi_thread = false;
-	if(multi_thread){
-		const int num_threads = res.size();
-		std::thread t[num_threads];
-		for (int i = 0; i < num_threads; ++i) {t[i] = std::thread(call_from_thread,i);}
-		for (int i = 0; i < num_threads; ++i) {t[i].join();}
-		for (int i = 0; i < num_threads; ++i) {
-			reglib::Model * model2 = res[i];
-			reglib::FusionResults fr = fr_res[i];
-			if(fr.score > 100){
-				fr.guess = fr.guess.inverse();
-				fr2merge.push_back(fr);
-				models2merge.push_back(model2);
-				printf("%i could be registered\n",i);
-			}
+	for(unsigned int i = 0; i < res.size(); i++){
+		if(addIfPossible(database,model,res[i])){
+			printf("stop: %s\n",__PRETTY_FUNCTION__);
+			return true;
 		}
-
-		for(unsigned int i = 0; i < models2merge.size(); i++){
-			reglib::Model * model2 = models2merge[i];
-
-			reglib::RegistrationRandom *	reg		= new reglib::RegistrationRandom();
-			reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( model2, reg);
-			mu->occlusion_penalty               = occlusion_penalty;
-			mu->massreg_timeout                 = massreg_timeout;
-			mu->viewer							= viewer;
-			reg->visualizationLvl				= 0;
-
-			reglib::UpdatedModels ud = mu->fuseData(&(fr2merge[i]), model, model2);
-			printf("merge %i to %i\n",int(model->id),int(model2->id));
-			printf("new_models:     %i\n",int(ud.new_models.size()));
-			printf("updated_models: %i\n",int(ud.updated_models.size()));
-			printf("deleted_models: %i\n",int(ud.deleted_models.size()));
-
-			delete mu;
-			delete reg;
-
-			for(unsigned int j = 0; j < ud.new_models.size(); j++){		new_models[ud.new_models[j]->id]			= ud.new_models[j];}
-			for(unsigned int j = 0; j < ud.updated_models.size(); j++){	updated_models[ud.updated_models[j]->id]	= ud.updated_models[j];}
-
-			for(unsigned int j = 0; j < ud.deleted_models.size(); j++){
-				database->remove(ud.deleted_models[j]);
-				delete ud.deleted_models[j];
-			}
-			if(ud.deleted_models.size() > 0){changed = true; break;}
-		}
-	}else{
-		for(unsigned int i = 0; i < res.size(); i++){
-			reglib::Model * model2 = res[i];
-			reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
-			reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( model2, reg);
-			mu->occlusion_penalty               = occlusion_penalty;
-			mu->massreg_timeout                 = massreg_timeout;
-			mu->viewer							= viewer;
-			reg->visualizationLvl				= 0;
-			reglib::FusionResults fr = mu->registerModel(model);
-
-			if(fr.score > 100){
-				fr.guess = fr.guess.inverse();
-
-				reglib::UpdatedModels ud = mu->fuseData(&fr, model, model2);
-				printf("merge %i to %i\n",int(model->id),int(model2->id));
-				printf("new_models:     %i\n",int(ud.new_models.size()));
-				printf("updated_models: %i\n",int(ud.updated_models.size()));
-				printf("deleted_models: %i\n",int(ud.deleted_models.size()));
-
-				for(unsigned int j = 0; j < ud.new_models.size(); j++){		new_models[ud.new_models[j]->id]			= ud.new_models[j];}
-				for(unsigned int j = 0; j < ud.updated_models.size(); j++){	updated_models[ud.updated_models[j]->id]	= ud.updated_models[j];}
-
-				for(unsigned int j = 0; j < ud.deleted_models.size(); j++){
-					database->remove(ud.deleted_models[j]);
-					delete ud.deleted_models[j];
-				}
-				if(ud.deleted_models.size() > 0){changed = true; break;}
-			}
-
-			delete mu;
-			delete reg;
-		}
-	}
-
-
-
-	for (std::map<int,reglib::Model *>::iterator it=updated_models.begin(); it!=updated_models.end();	++it) {
-		database->remove(it->second);
-		models_deleted_pub.publish(getModelMSG(it->second));
-	}
-	for (std::map<int,reglib::Model *>::iterator it=updated_models.begin(); it!=updated_models.end();	++it) {
-		if (deleteIfFail) { // deleteIfFail means that this is coming from search results, we delete them if failing to recognize
-			std_msgs::String msg;
-			msg.data = "Added search result to DB!";
-			chatter_pub.publish(msg);
-			publishDatabasePCD();
-		}
-		addToDB(database, it->second);
-	}
-	for (std::map<int,reglib::Model *>::iterator it=new_models.begin();		it!=new_models.end();		++it) {
-		if (deleteIfFail) { // deleteIfFail means that this is coming from search results, we delete them if failing to recognize
-			std_msgs::String msg;
-			msg.data = "Added search result to DB!";
-			chatter_pub.publish(msg);
-			publishDatabasePCD();
-		}
-		addToDB(database, it->second);
 	}
+printf("stop: %s\n",__PRETTY_FUNCTION__);
+	return false;
+}
 
-	printf("end of addToDB: %i %i",int(add),int(deleteIfFail));
-	if(deleteIfFail){
-		if(!changed){
-			printf("didnt manage to integrate searchresult\n");
-			database->remove(model);
-			for(unsigned int i = 0; i < model->frames.size(); i++){
-				delete model->frames[i];
-				delete model->modelmasks[i];
+bool runSearch(ModelDatabase * database, reglib::Model * model, int number_of_searches = 5){
+	quasimodo_msgs::model_to_retrieval_query m2r;
+	m2r.request.model = quasimodo_brain::getModelMSG(model,true);
+	if (conversion_client.call(m2r)){
+		quasimodo_msgs::query_cloud qc;
+		qc.request.query = m2r.response.query;
+		qc.request.query.query_kind = qc.request.query.METAROOM_QUERY;
+		qc.request.query.number_query = number_of_searches+10;
+
+		if (retrieval_client.call(qc)){
+			quasimodo_msgs::retrieval_result result = qc.response.result;
+
+			for(unsigned int i = 0; i < result.retrieved_images.size(); i++){
+				for(unsigned int j = 0; j < result.retrieved_images[i].images.size(); j++){
+					cv_bridge::CvImagePtr ret_image_ptr;
+					try {ret_image_ptr = cv_bridge::toCvCopy(result.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);}
+					catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
+
+					cv_bridge::CvImagePtr ret_mask_ptr;
+					try {ret_mask_ptr = cv_bridge::toCvCopy(result.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);}
+					catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
+
+					cv_bridge::CvImagePtr ret_depth_ptr;
+					try {ret_depth_ptr = cv_bridge::toCvCopy(result.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);}
+					catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
+
+					cv::Mat rgbimage	= ret_image_ptr->image;
+					cv::Mat maskimage	= ret_mask_ptr->image;
+					cv::Mat depthimage	= ret_depth_ptr->image;
+
+					cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE );			cv::imshow( "rgbimage", rgbimage );
+					cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE );		cv::imshow( "maskimage", maskimage );
+					cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE );		cv::imshow( "depthimage", depthimage );
+					cv::waitKey( 0 );
+				}
 			}
-			delete model;
 		}else{
-			printf("integrateing searchresult\n");
+			ROS_ERROR("retrieval_client service FAIL in %s :: %i !",__FILE__,__LINE__);
 		}
+	}else{
+		ROS_ERROR("model_to_retrieval_query service FAIL in %s :: %i !",__FILE__,__LINE__);
 	}
+	return true;
 }
 
-bool nextNew = true;
-reglib::Model * newmodel = 0;
-
-int sweepid_counter = 0;
+std::set<int> searchset;
 
-//std::vector<cv::Mat> newmasks;
-std::vector<cv::Mat> allmasks;
-
-int modaddcount = 0;
-bool modelFromFrame(quasimodo_msgs::model_from_frame::Request  & req, quasimodo_msgs::model_from_frame::Response & res){
-	printf("======================================\nmodelFromFrame\n======================================\n");
-	uint64 frame_id = req.frame_id;
-	uint64 isnewmodel = req.isnewmodel;
+void addNewModel(reglib::Model * model){
+printf("start: %s\n",__PRETTY_FUNCTION__);
+	reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+	reg->visualizationLvl				= show_reg_lvl;
+	reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( model, reg);
+	mu->occlusion_penalty               = occlusion_penalty;
+	mu->massreg_timeout                 = massreg_timeout;
+	mu->viewer							= viewer;
+	mu->show_init_lvl					= show_init_lvl;//init show
+	mu->show_refine_lvl					= show_refine_lvl;//refine show
+	mu->show_scoring					= show_scoring;//fuse scoring show
+	mu->makeInitialSetup();
 
-	printf("%i and %i\n",int(frame_id),int(isnewmodel));
-	cv_bridge::CvImagePtr			mask_ptr;
-	try{							mask_ptr = cv_bridge::toCvCopy(req.mask, sensor_msgs::image_encodings::MONO8);}
-	catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());return false;}
+	delete mu;
+	delete reg;
 
-	cv::Mat mask					= mask_ptr->image;
-	allmasks.push_back(mask);
-	std::cout << frames[frame_id]->pose << std::endl << std::endl;
-
-	cv::Mat fullmask;
-	fullmask.create(480,640,CV_8UC1);
-	unsigned char * maskdata = (unsigned char *)fullmask.data;
-	for(unsigned int j = 0; j < 640*480; j++){maskdata[j] = 255;}
-	if(newmodel == 0){
-		newmodel					= new reglib::Model(frames[frame_id],mask);
-		modeldatabase->add(newmodel);
-		//		newmodel->masks.back() = fullmask;
-		//		newmodel->recomputeModelPoints();
+	reglib::Model * newmodelHolder = new reglib::Model();
+	model->parrent = newmodelHolder;
+	newmodelHolder->submodels.push_back(model);
+	newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity());
+	newmodelHolder->last_changed = ++current_model_update;
+	if(show_modelbuild){
+		newmodelHolder->recomputeModelPoints(Eigen::Matrix4d::Identity(),viewer);
 	}else{
-		//		reglib::Model * inputmodel					= new reglib::Model(frames[frame_id],mask);
-		//		inputmodel->masks.back() = fullmask;
-		//		inputmodel->recomputeModelPoints();
-
-		//		reglib::RegistrationRefinement *	reg		= new reglib::RegistrationRefinement();
-		//		//RegistrationRefinement
-		//		reg->target_points							= 3000;
-		//		reglib::ModelUpdaterBasicFuse * mu			= new reglib::ModelUpdaterBasicFuse( inputmodel, reg);
-		//		mu->viewer									= viewer;
-		//		reg->visualizationLvl						= 4;
-		//		reglib::FusionResults fr = mu->registerModel(newmodel,(newmodel->frames.front()->pose.inverse() * frames[frame_id]->pose).inverse());
-		//		fr.guess = fr.guess.inverse();
-		//		delete inputmodel;
-		//		delete mu;
-		//		delete reg;
-
-		newmodel->frames.push_back(frames[frame_id]);
-		//newmodel->masks.push_back(mask);
-		newmodel->relativeposes.push_back(newmodel->frames.front()->pose.inverse() * frames[frame_id]->pose);
-		//newmodel->relativeposes.push_back(frames[frame_id]->pose);
-		//newmodel->relativeposes.push_back(newmodel->frames.front()->pose.inverse() * fr.guess);
-		newmodel->modelmasks.push_back(new reglib::ModelMask(mask));
-		newmodel->recomputeModelPoints();
-
+		newmodelHolder->recomputeModelPoints();
 	}
-	newmodel->modelmasks.back()->sweepid = sweepid_counter;
-	//printf("LINE: %i\n",__LINE__);
-
-	//show_sorted();
 
-	res.model_id					= newmodel->id;
-
-	if(isnewmodel != 0){
-		//newmodel->masks = newmasks;
-		//newmasks.clear();
-		newmodel->recomputeModelPoints();
-		//show_sorted();
-		reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
-		reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( newmodel, reg);
-		mu->occlusion_penalty               = occlusion_penalty;
-		mu->massreg_timeout                 = massreg_timeout;
-		mu->viewer							= viewer;
-		//		reg->visualizationLvl				= 0;
-		mu->refine(0.01,true);
-		mu->recomputeScores();
-		delete mu;
-		delete reg;
-		int current_model_update_before = current_model_update;
-		newmodel->recomputeModelPoints();
-		newmodel->last_changed = ++current_model_update;
-		newmodel->print();
-		addToDB(modeldatabase, newmodel,false);
-		//if(modaddcount % 1 == 0){show_sorted();}
-		show_sorted();
-		publishDatabasePCD();
-		modaddcount++;
-		dumpDatabase(savepath);
-
-		//		cameras[0]->save("./camera0");
-		//		for(unsigned int m = 0; m < modeldatabase->models.size(); m++){
-		//			char buf [1024];
-		//			sprintf(buf,"./model%i",m);
-		//			char command [1024];
-		//			sprintf(command,"mkdir -p %s",buf);
-		//			system(command);
-		//			modeldatabase->models[m]->save(std::string(buf));
-		//		}
-		//exit(0);
-
-		for(unsigned int m = 0; run_search && m < modeldatabase->models.size(); m++){
-			printf("looking at: %i\n",int(modeldatabase->models[m]->last_changed));
-			reglib::Model * currentTest = modeldatabase->models[m];
-			if(currentTest->last_changed > current_model_update_before){
-				printf("changed: %i\n",int(m));
-
-				double start = getTime();
-
-				new_search_result = false;
-				models_new_pub.publish(getModelMSG(currentTest));
-
-				while(getTime()-start < search_timeout){
-					ros::spinOnce();
-					if(new_search_result){
-
-						for(unsigned int i = 0; i < sresult.retrieved_images.size(); i++){
-
-							int maxval = 0;
-							int maxind = 0;
-
-							for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){
-								cv_bridge::CvImagePtr ret_image_ptr;
-								try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);}
-								catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
-
-								cv_bridge::CvImagePtr ret_mask_ptr;
-								try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);}
-								catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
-
-								cv_bridge::CvImagePtr ret_depth_ptr;
-								try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);}
-								catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
-
-								cv::Mat rgbimage	= ret_image_ptr->image;
-								cv::Mat maskimage	= ret_mask_ptr->image;
-								cv::Mat depthimage	= ret_depth_ptr->image;
-
-
-								unsigned short * depthdata = (unsigned short * )depthimage.data;
-								unsigned char * rgbdata = (unsigned char * )rgbimage.data;
-								unsigned char * maskdata = (unsigned char * )maskimage.data;
-								int count = 0;
-								for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){
-									if(depthdata[pixel] > 0 && maskdata[pixel] > 0){
-										count++;
-									}
-								}
-								if(count > maxval){
-									maxval = count;
-									maxind = j;
-								}
-							}
-
-							//for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){
-							if(maxval > 100){//Atleast some pixels has to be in the mask...
-								int j =  maxind;
-
-								cv_bridge::CvImagePtr ret_image_ptr;
-								try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);}
-								catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
-
-								cv_bridge::CvImagePtr ret_mask_ptr;
-								try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);}
-								catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
-
-								cv_bridge::CvImagePtr ret_depth_ptr;
-								try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);}
-								catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);}
-
-								cv::Mat rgbimage	= ret_image_ptr->image;
-								cv::Mat maskimage	= ret_mask_ptr->image;
-								cv::Mat depthimage	= ret_depth_ptr->image;
-								for (int ii = 0; ii < depthimage.rows; ++ii) {
-									for (int jj = 0; jj < depthimage.cols; ++jj) {
-										depthimage.at<uint16_t>(ii, jj) *= 5;
-									}
-								}
-
-//								cv::Mat overlap	= rgbimage.clone();
-//								unsigned short * depthdata = (unsigned short * )depthimage.data;
-//								unsigned char * rgbdata = (unsigned char * )rgbimage.data;
-//								unsigned char * maskdata = (unsigned char * )maskimage.data;
-//								unsigned char * overlapdata = (unsigned char * )overlap.data;
-//								for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){
-//									if(depthdata[pixel] > 0 && maskdata[pixel] > 0){
-//										overlapdata[3*pixel+0] = rgbdata[3*pixel+0];
-//										overlapdata[3*pixel+1] = rgbdata[3*pixel+1];
-//										overlapdata[3*pixel+2] = rgbdata[3*pixel+2];
-//									}else{
-//										overlapdata[3*pixel+0] = 0;
-//										overlapdata[3*pixel+1] = 0;
-//										overlapdata[3*pixel+2] = 0;
-//									}
-//								}
-//								cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE );			cv::imshow( "rgbimage", rgbimage );
-//								cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE );		cv::imshow( "maskimage", maskimage );
-//								cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE );		cv::imshow( "depthimage", 100*depthimage );
-//								cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE );			cv::imshow( "overlap", overlap );
-
-								Eigen::Affine3d epose = Eigen::Affine3d::Identity();
-								reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix());
-								reglib::Model * searchmodel = new reglib::Model(frame,maskimage);
-								bool res = searchmodel->testFrame(0);
-
-//								if(cv::waitKey(0) != 'n'){
-
-									//Todo:: check new model is not flat or L shape
-
-									printf("--- trying to add serach results, if more then one addToDB: results added-----\n");
-									addToDB(modeldatabase, searchmodel,false,true);
-									show_sorted();
-//								}
-							}
-						}
-
-						break;
-					}else{
-						printf("searching... timeout in %3.3f\n", +start +search_timeout - getTime());
-						usleep(100000);
-					}
+	modeldatabase->add(newmodelHolder);
+	addToDB(modeldatabase, newmodelHolder,false);
+	printf("%i\n",__LINE__);
+	show_sorted();
+
+	printf("%i\n",__LINE__);
+	bool do_next = true;
+	while(do_next && run_search){
+		printf("running search loop\n");
+		do_next = false;
+		for(unsigned int i = 0; i < modeldatabase->models.size(); i++){
+			reglib::Model * current = modeldatabase->models[i];
+			if(searchset.count(current->id)==0){
+				searchset.insert(current->id);
+				printf("new search %i\n",current->id);
+
+				if(runSearch(modeldatabase, current)){
+					do_next = true;
+					break;
 				}
+			}else{
+				printf("already searched %i\n",current->id);
 			}
-			//exit(0);
 		}
-		newmodel = 0;
-		sweepid_counter++;
 	}
-	return true;
-}
 
-bool fuseModels(quasimodo_msgs::fuse_models::Request  & req, quasimodo_msgs::fuse_models::Response & res){}
+	printf("%i\n",__LINE__);
+	for(unsigned int i = 0; i < modeldatabase->models.size(); i++){publish_history(modeldatabase->models[i]->getHistory());}
+	printf("%i\n",__LINE__);
+	publishDatabasePCD();
+	printf("%i\n",__LINE__);
+	dumpDatabase(savepath);
+	printf("stop: %s\n",__PRETTY_FUNCTION__);
+}
 
-int getdir (std::string dir, std::vector<std::string> & files){
-	DIR *dp;
-	struct dirent *dirp;
-	if((dp  = opendir(dir.c_str())) == NULL) {
-		cout << "Error(" << errno << ") opening " << dir << endl;
-		return errno;
-	}
+void somaCallback(const std_msgs::String & m){printf("somaCallback(%s)\n",m.data.c_str());}
 
-	while ((dirp = readdir(dp)) != NULL) {
-		files.push_back(std::string(dirp->d_name));
-	}
-	closedir(dp);
-	return 0;
+void modelCallback(const quasimodo_msgs::model & m){
+	quasimodo_msgs::model mod = m;
+	addNewModel(quasimodo_brain::getModelFromMSG(mod,false));
 }
 
 void clearMem(){
-
-
-	for(auto iterator = cameras.begin(); iterator != cameras.end(); iterator++) {
-		delete iterator->second;
-	}
-
-	for(auto iterator = frames.begin(); iterator != frames.end(); iterator++) {
-		delete iterator->second;
-	}
-
-	for(auto iterator = models.begin(); iterator != models.end(); iterator++) {
+	for(auto iterator = cameras.begin();	iterator != cameras.end();	iterator++) {delete iterator->second;}
+	for(auto iterator = frames.begin();		iterator != frames.end();	iterator++) {delete iterator->second;}
+	for(auto iterator = models.begin();		iterator != models.end();	iterator++) {
 		reglib::Model * model = iterator->second;
-		for(unsigned int i = 0; i < model->frames.size(); i++){
-			delete model->frames[i];
-		}
-
-		for(unsigned int i = 0; i < model->modelmasks.size(); i++){
-			delete model->modelmasks[i];
-		}
+		for(unsigned int i = 0; i < model->frames.size()	; i++){delete model->frames[i];}
+		for(unsigned int i = 0; i < model->modelmasks.size(); i++){delete model->modelmasks[i];}
 		delete model;
 	}
-
-	for(auto iterator = updaters.begin(); iterator != updaters.end(); iterator++) {
-		delete iterator->second;
-	}
-
-
+	for(auto iterator = updaters.begin(); iterator != updaters.end(); iterator++) {delete iterator->second;}
 	if(registration != 0){delete registration;}
 	if(modeldatabase != 0){delete modeldatabase;}
 }
@@ -882,33 +499,42 @@ void clearMem(){
 int main(int argc, char **argv){
 	cameras[0]		= new reglib::Camera();
 	registration	= new reglib::RegistrationRandom();
-	modeldatabase	= new ModelDatabaseRGBHistogram(5);
+	modeldatabase	= 0;
 
 	ros::init(argc, argv, "quasimodo_model_server");
 	ros::NodeHandle n;
+    nh = &n;
 	models_new_pub		= n.advertise<quasimodo_msgs::model>("/models/new",		1000);
 	models_updated_pub	= n.advertise<quasimodo_msgs::model>("/models/updated", 1000);
 	models_deleted_pub	= n.advertise<quasimodo_msgs::model>("/models/deleted", 1000);
 
-	ros::ServiceServer service1 = n.advertiseService("model_from_frame", modelFromFrame);
-	ROS_INFO("Ready to add use model_from_frame.");
+	//ros::ServiceServer service1 = n.advertiseService("model_from_frame",	modelFromFrame);
+	ros::ServiceServer service2 = n.advertiseService("index_frame",			indexFrame);
+	ros::ServiceServer service4 = n.advertiseService("get_model",			getModel);
+    ros::ServiceServer service5 = n.advertiseService("quasimodo/recognize", recognizeService);
+	ROS_INFO("Ready to add use services.");
 
-	ros::ServiceServer service2 = n.advertiseService("index_frame", indexFrame);
-	ROS_INFO("Ready to add use index_frame.");
+	ros::Subscriber sub = n.subscribe("/retrieval_result", 1, retrievalCallback);
+	ROS_INFO("Ready to add recieve search results.");
 
-	ros::ServiceServer service3 = n.advertiseService("fuse_models", fuseModels);
-	ROS_INFO("Ready to add use fuse_models.");
+	database_pcd_pub    = n.advertise<sensor_msgs::PointCloud2>("modelserver/databasepcd", 1000);
+	model_history_pub   = n.advertise<sensor_msgs::PointCloud2>("modelserver/model_history", 1000);
+	model_last_pub      = n.advertise<sensor_msgs::PointCloud2>("modelserver/last", 1000);
+	model_places_pub    = n.advertise<sensor_msgs::PointCloud2>("modelserver/model_places", 1000);
 
-	ros::ServiceServer service4 = n.advertiseService("get_model", getModel);
-	ROS_INFO("Ready to add use get_model.");
+	std::string retrieval_name	= "/quasimodo_retrieval_service";
+	std::string conversion_name = "/models/server";
+	std::string insert_name		= "/insert_model_service";
+	retrieval_client	= n.serviceClient<quasimodo_msgs::query_cloud>				(retrieval_name);
+	conversion_client	= n.serviceClient<quasimodo_msgs::model_to_retrieval_query>	(conversion_name);
+	insert_client		= n.serviceClient<quasimodo_msgs::insert_model>				(insert_name);
 
-	soma2add = n.serviceClient<soma_manager::SOMA2InsertObjs>(		"soma2/insert_objs");
-	ROS_INFO("Ready to add use soma2add.");
 
-	ros::Subscriber sub = n.subscribe("/retrieval_result", 1, retrievalCallback);
-	ROS_INFO("Ready to add recieve search results.");
+	std::vector<ros::Subscriber> input_model_subs;
+	std::vector<ros::Subscriber> soma_input_model_subs;
+	std::vector<std::string> modelpcds;
 
-	database_pcd_pub  = n.advertise<sensor_msgs::PointCloud2>("modelserver/databasepcd", 1000);
+	bool clearQDB = false;
 
 	int inputstate = -1;
 	for(int i = 1; i < argc;i++){
@@ -916,7 +542,7 @@ int main(int argc, char **argv){
 		if(		std::string(argv[i]).compare("-c") == 0){	printf("camera input state\n"); inputstate = 1;}
 		else if(std::string(argv[i]).compare("-l") == 0){	printf("reading all models at %s (the path defined from -p)\n",savepath.c_str());
 			std::vector<std::string> folderdata;
-			int r = getdir(savepath+"/",folderdata);
+			int r = quasimodo_brain::getdir(savepath+"/",folderdata);
 			for(unsigned int fnr = 0; fnr < folderdata.size(); fnr++){
 				printf("%s\n",folderdata[fnr].c_str());
 			}
@@ -926,16 +552,27 @@ int main(int argc, char **argv){
 		else if(std::string(argv[i]).compare("-occlusion_penalty") == 0){printf("occlusion_penalty input state\n");inputstate = 4;}
 		else if(std::string(argv[i]).compare("-massreg_timeout") == 0){printf("massreg_timeout input state\n");inputstate = 5;}
 		else if(std::string(argv[i]).compare("-search") == 0){printf("pointcloud search input state\n");run_search = true; inputstate = 6;}
-		else if(std::string(argv[i]).compare("-v") == 0){	printf("visualization turned on\n");	visualization = true;}
+		else if(std::string(argv[i]).compare("-v") == 0){           printf("visualization turned on\n");                visualization = true;}
+		else if(std::string(argv[i]).compare("-v_init") == 0){      printf("visualization of init turned on\n");        visualization = true; inputstate = 8;}
+		else if(std::string(argv[i]).compare("-v_refine") == 0 || std::string(argv[i]).compare("-v_ref") == 0){	printf("visualization refinement turned on\n");     visualization = true; inputstate = 9;}
+		else if(std::string(argv[i]).compare("-v_register") == 0 || std::string(argv[i]).compare("-v_reg") == 0){	printf("visualization registration turned on\n");	visualization = true; inputstate = 10;}
+		else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){	printf("visualization scoring turned on\n");        visualization = true; show_scoring = true;}
+		else if(std::string(argv[i]).compare("-v_db") == 0){        printf("visualization db turned on\n");             visualization = true; show_db = true;}
+		else if(std::string(argv[i]).compare("-s_db") == 0){        printf("save db turned on\n"); save_db = true;}
+		else if(std::string(argv[i]).compare("-intopic") == 0){	printf("intopic input state\n");	inputstate = 11;}
+		else if(std::string(argv[i]).compare("-mdb") == 0){	printf("intopic input state\n");		inputstate = 12;}
+		else if(std::string(argv[i]).compare("-show_search") == 0){	printf("show_search\n");		show_search = true;}
+		else if(std::string(argv[i]).compare("-show_modelbuild") == 0){	printf("show_modelbuild\n");	visualization = true; show_modelbuild = true;}
+		else if(std::string(argv[i]).compare("-loadModelsPCDs") == 0){								inputstate = 13;}
+		else if(std::string(argv[i]).compare("-clearQDB") == 0){									clearQDB = true;}
 		else if(inputstate == 1){
 			reglib::Camera * cam = reglib::Camera::load(std::string(argv[i]));
 			delete cameras[0];
-			cameras[0] = cam;
+            cameras[0] = cam;
 		}else if(inputstate == 2){
 			reglib::Model * model = reglib::Model::load(cameras[0],std::string(argv[i]));
 			sweepid_counter = std::max(int(model->modelmasks[0]->sweepid + 1), sweepid_counter);
 			modeldatabase->add(model);
-			//addToDB(modeldatabase, model,false);
 			model->last_changed = ++current_model_update;
 			show_sorted();
 		}else if(inputstate == 3){
@@ -946,26 +583,105 @@ int main(int argc, char **argv){
 			massreg_timeout = atof(argv[i]); printf("massreg_timeout set to %f\n",massreg_timeout);
 		}else if(inputstate == 6){
 			search_timeout = atof(argv[i]); printf("search_timeout set to %f\n",search_timeout);
-			if(search_timeout == 0){
-				run_search = false;
+			if(search_timeout == 0){run_search = false;}
+		}else if(inputstate == 8){
+			show_init_lvl = atoi(argv[i]);
+		}else if(inputstate == 9){
+			show_refine_lvl = atoi(argv[i]);
+		}else if(inputstate == 10){
+			show_reg_lvl = atoi(argv[i]);
+		}else if(inputstate == 11){
+			printf("adding %s to input_model_subs\n",argv[i]);
+			input_model_subs.push_back(n.subscribe(std::string(argv[i]), 100, modelCallback));
+		}else if(inputstate == 12){
+			if(atoi(argv[i]) == 0){
+				if(modeldatabase != 0){delete modeldatabase;}
+				modeldatabase	= new ModelDatabaseBasic();
+			}
+
+			if(atoi(argv[i]) == 1){
+				if(modeldatabase != 0){delete modeldatabase;}
+				modeldatabase	= new ModelDatabaseRGBHistogram(5);
+			}
+
+			if(atoi(argv[i]) == 2){
+				if(modeldatabase != 0){delete modeldatabase;}
+				modeldatabase	= new ModelDatabaseRetrieval(n);
 			}
+		}else if(inputstate == 13){
+			modelpcds.push_back( std::string(argv[i]) );
 		}
 	}
 
+//	if(clearQDB){
+//		printf("rm -r -f %s\n",(savepath+"/database_tmp").c_str());
+//		char command [1024];
+//		printf(command,"rm -r -f %s",(savepath+"/database_tmp").c_str());
+//		int r = system(command);
+//	}
+
+
+//	if(fileExists(savepath+"/database_tmp")){
+//		printf("file exists\n");
+
+//	}else{
+//		printf("making savedi: %s\n",(savepath+"/database_tmp").c_str());
+//		char command [1024];
+//		sprintf(command,"mkdir %s",(savepath+"/database_tmp").c_str());
+//		int r = system(command);
+//	}
+//	//exit(0);
+
 	if(visualization){
-		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("viewer"));
+		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("Modelserver Viewer"));
 		viewer->addCoordinateSystem(0.1);
-		viewer->setBackgroundColor(0.9,0.9,0.9);
+		viewer->setBackgroundColor(1.0,0.0,1.0);
+	}
+
+	if(modeldatabase == 0){modeldatabase	= new ModelDatabaseRetrieval(n);}
+
+	for(unsigned int i = 0; i < modelpcds.size(); i++){
+		std::vector<reglib::Model *> mods = quasimodo_brain::loadModelsPCDs(modelpcds[i]);
+		for(unsigned int j = 0; j < mods.size(); j++){
+			//modeldatabase->add(mods[j]);
+			//addNewModel(mods[j]);
+
+			reglib::Model * model = mods[j];
+
+			reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+			reg->visualizationLvl				= show_reg_lvl;
+			reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( model, reg);
+			mu->occlusion_penalty               = occlusion_penalty;
+			mu->massreg_timeout                 = massreg_timeout;
+			mu->viewer							= viewer;
+			mu->show_init_lvl					= show_init_lvl;//init show
+			mu->show_refine_lvl					= show_refine_lvl;//refine show
+			mu->show_scoring					= show_scoring;//fuse scoring show
+			mu->makeInitialSetup();
+
+			delete mu;
+			delete reg;
+
+			reglib::Model * newmodelHolder = new reglib::Model();
+			model->parrent = newmodelHolder;
+			newmodelHolder->submodels.push_back(model);
+			newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity());
+			newmodelHolder->last_changed = ++current_model_update;
+			if(show_modelbuild){
+				newmodelHolder->recomputeModelPoints(Eigen::Matrix4d::Identity(),viewer);
+			}else{
+				newmodelHolder->recomputeModelPoints();
+			}
+			modeldatabase->add(newmodelHolder);
+		}
+
+		//show_sorted();
 	}
 
-	ros::Duration(1.0).sleep();
-	chatter_pub = n.advertise<std_msgs::String>("modelserver", 1000);
-	sleep(1);
-	std_msgs::String msg;
-	msg.data = "starting";
-	chatter_pub.publish(msg);
-	ros::spinOnce();
-	sleep(1);
+	if(input_model_subs.size()		== 0){input_model_subs.push_back(		n.subscribe("/quasimodo/segmentation/out/model", 100, modelCallback));}
+	if(soma_input_model_subs.size() == 0){soma_input_model_subs.push_back(	n.subscribe("/quasimodo/segmentation/out/soma_segment_id", 10000, somaCallback));}
+
+
 
 	ros::spin();
 	return 0;
diff --git a/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp b/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp
new file mode 100644
index 00000000..0c8f88af
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp
@@ -0,0 +1,106 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include <pcl_ros/point_cloud.h>
+
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include <sys/time.h>
+#include <sys/resource.h>
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <signal.h>
+
+
+
+
+int main(int argc, char** argv){
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+	viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
+	viewer->setBackgroundColor (1.0, 1.0, 1.0);
+	//viewer->addCoordinateSystem (1.0);
+	viewer->initCameraParameters ();
+
+	std::vector< std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > > clouds;
+	for(int i = 1; i < argc;i++){
+		printf("input: %s\n",argv[i]);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+		if (pcl::io::loadPCDFile<pcl::PointXYZRGBNormal> (argv[i], *cloud) != -1){
+			printf("%i\n",cloud->points.size());
+			clouds.push_back(std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr >());
+			clouds.back().push_back(cloud);
+		}
+	}
+	printf("nr: %i ->",clouds.size());
+	double square = sqrt(double(clouds.size())/1.5);
+	double wgrid = std::ceil(square*1.5);
+	double hgrid = std::ceil(double(clouds.size())/double(wgrid));
+	printf("square: %f ->  w: %f h: %f\n",square,wgrid,hgrid);
+
+	std::vector<int> inds;
+	for( double w = 0; w+0.5 < wgrid; w++ ){
+		for( double h = 0; h+0.5 < hgrid; h++ ){
+			int v1(0);
+			viewer->createViewPort ( double(w+0)/wgrid, double(h+0)/hgrid, double(w+1)/wgrid,  double(h+1)/hgrid, v1);
+			inds.push_back(v1);
+		}
+	}
+
+	for( int i = 0; i < clouds.size(); i++ ){
+		for( int j = 0; j < clouds[i].size(); j++ ){
+			viewer->addPointCloud<pcl::PointXYZRGBNormal> (clouds[i][j], pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(clouds[i][j]), std::to_string(i)+"_"+std::to_string(j),inds[i]);
+		}
+
+		viewer->addText (argv[1+i], 10, 10,0,0,0, std::to_string(i), inds[i]);
+		printf("%i -> %i\n",i,inds[i]);
+		viewer->setBackgroundColor (1.0,1.0,1.0, inds[i]+1);
+
+	}
+
+	viewer->spin();
+
+
+	for( int i = 0; i < clouds.size(); i++ ){
+		viewer->removeAllPointClouds(inds[i]);
+	}
+
+	int v1(0);
+	viewer->createViewPort ( 0, 0, 1, 1, v1);
+	viewer->setBackgroundColor (255.0, 255.0, 255.0,v1+1);
+
+	for( int i = 0; i < clouds.size(); i++ ){
+		for( int j = 0; j < clouds[i].size(); j++ ){
+			viewer->addPointCloud<pcl::PointXYZRGBNormal> (clouds[i][j], pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(clouds[i][j]), std::to_string(i)+"_"+std::to_string(j),v1);
+			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3,  std::to_string(i)+"_"+std::to_string(j),v1);
+		}
+		viewer->spinOnce();
+		std::string path = std::string(argv[1+i]);
+		path.pop_back();
+		path.pop_back();
+		path.pop_back();
+		path += "png";
+		printf("path: %s\n",path.c_str());
+
+		viewer->saveScreenshot(path);
+		viewer->spinOnce();
+		//viewer->spin();
+		viewer->removeAllPointClouds(v1);
+	}
+	viewer->spin();
+
+
+	printf("done...\n");
+	return 0;
+}
diff --git a/quasimodo/quasimodo_brain/src/preload_object_data.cpp b/quasimodo/quasimodo_brain/src/preload_object_data.cpp
index 05638eb5..abc0e789 100644
--- a/quasimodo/quasimodo_brain/src/preload_object_data.cpp
+++ b/quasimodo/quasimodo_brain/src/preload_object_data.cpp
@@ -78,6 +78,15 @@ using LabelT = semantic_map_load_utilties::LabelledData<PointT>;
 #include "quasimodo_msgs/fuse_models.h"
 #include "quasimodo_msgs/get_model.h"
 
+#include "metaroom_xml_parser/simple_xml_parser.h"
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+
+
 using namespace std;
 
 typedef pcl::PointXYZRGB PointType;
@@ -98,8 +107,10 @@ std::vector< std::vector<cv::Mat> > depths;
 std::vector< std::vector<cv::Mat> > masks;
 std::vector< std::vector<tf::StampedTransform > >tfs;
 std::vector< std::vector<Eigen::Matrix4f> > initposes;
+std::vector< std::vector< image_geometry::PinholeCameraModel > > cams;
 
 pcl::visualization::PCLVisualizer* pg;
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
 
 
 
@@ -112,7 +123,7 @@ std::vector<Eigen::Matrix4f> getRegisteredViewPoses(const std::string& poses_fil
 	}
 	cout<<"Loading additional view registered poses from "<<poses_file<<endl;
 
-    for (int i=0; i<no_transforms+1; i++){
+	for (int i=0; i<no_transforms+1; i++){
 		Eigen::Matrix4f transform;
 		float temp;
 		for (size_t j=0; j<4; j++){
@@ -128,244 +139,483 @@ std::vector<Eigen::Matrix4f> getRegisteredViewPoses(const std::string& poses_fil
 
 void chatterCallback(const std_msgs::String::ConstPtr& msg)
 {
-  ROS_INFO("I heard: [%s]", msg->data.c_str());
+	ROS_INFO("I heard: [%s]", msg->data.c_str());
 
-  for(unsigned int i = 0; i < rgbs.size(); i++){
-      std::vector<int> fid;
-      std::vector<int> fadded;
-      for(unsigned int j = 0; j < rgbs[i].size(); j++){
-          printf("%i %i\n",i,j);
+    for(unsigned int i = 0; i < rgbs.size(); i++){
+        printf("feeding system...\n");
+		std::vector<int> fid;
+		std::vector<int> fadded;
+		for(unsigned int j = 0; j < rgbs[i].size(); j++){
+			printf("%i %i\n",i,j);
 
-          //cv::Mat maskimage		= masks[i][j];
-		  cv::Mat rgbimage			= rgbs[i][j];
-          cv::Mat depthimage		= depths[i][j];
-          tf::StampedTransform tf	= tfs[i][j];
-		  Eigen::Matrix4f m			= initposes[i][j];
-          geometry_msgs::TransformStamped tfstmsg;
-          tf::transformStampedTFToMsg (tf, tfstmsg);
+			//cv::Mat maskimage			= masks[i][j];
+			cv::Mat rgbimage			= rgbs[i][j];
+			cv::Mat depthimage			= depths[i][j];
+			tf::StampedTransform tf		= tfs[i][j];
+			Eigen::Matrix4f m			= initposes[i][j];
+			geometry_msgs::TransformStamped tfstmsg;
+			tf::transformStampedTFToMsg (tf, tfstmsg);
 
-          geometry_msgs::Transform tfmsg = tfstmsg.transform;
+			geometry_msgs::Transform tfmsg = tfstmsg.transform;
 
-          printf("start adding frame %i\n",i);
+			printf("start adding frame %i\n",i);
 
 			Eigen::Quaternionf q = Eigen::Quaternionf(Eigen::Affine3f(m).rotation());
 
-          geometry_msgs::Pose		pose;
-//		  pose.orientation		= tfmsg.rotation;
-//		  pose.position.x		= tfmsg.translation.x;
-//		  pose.position.y		= tfmsg.translation.y;
-//		  pose.position.z		= tfmsg.translation.z;
-		  pose.orientation.x	= q.x();
-		  pose.orientation.y	= q.y();
-		  pose.orientation.z	= q.z();
-		  pose.orientation.w	= q.w();
-		  pose.position.x		= m(0,3);
-		  pose.position.y		= m(1,3);
-		  pose.position.z		= m(2,3);
-
-
-          cv_bridge::CvImage rgbBridgeImage;
-          rgbBridgeImage.image = rgbimage;
-          rgbBridgeImage.encoding = "bgr8";
-
-          cv_bridge::CvImage depthBridgeImage;
-          depthBridgeImage.image = depthimage;
-          depthBridgeImage.encoding = "mono16";
-
-          quasimodo_msgs::index_frame ifsrv;
-          ifsrv.request.frame.capture_time = ros::Time();
-          ifsrv.request.frame.pose		= pose;
-          ifsrv.request.frame.frame_id	= -1;
-          ifsrv.request.frame.rgb			= *(rgbBridgeImage.toImageMsg());
-          ifsrv.request.frame.depth		= *(depthBridgeImage.toImageMsg());
-
-          if (index_frame_client.call(ifsrv)){//Add frame to model server
-              int frame_id = ifsrv.response.frame_id;
-              fadded.push_back(j);
-              fid.push_back(frame_id);
-              ROS_INFO("frame_id%i", frame_id );
-           }else{ROS_ERROR("Failed to call service index_frame");}
-
-          printf("stop adding frame %i\n",i);
-      }
-
-      for(unsigned int j = 0; j < fadded.size(); j++){
-          printf("start adding mask %i\n",i);
-          cv::Mat mask	= masks[i][fadded[j]];
-
-          cv_bridge::CvImage maskBridgeImage;
-          maskBridgeImage.image = mask;
-          maskBridgeImage.encoding = "mono8";
-
-          quasimodo_msgs::model_from_frame mff;
-          mff.request.mask		= *(maskBridgeImage.toImageMsg());
-          mff.request.isnewmodel	= (j == (fadded.size()-1));
-          mff.request.frame_id	= fid[j];
-
-          if (model_from_frame_client.call(mff)){//Build model from frame
-              int model_id = mff.response.model_id;
-              if(model_id > 0){
-                  ROS_INFO("model_id%i", model_id );
-              }
-          }else{ROS_ERROR("Failed to call service index_frame");}
-
-          printf("stop adding mask %i\n",i);
-
-      }
-  }
+			geometry_msgs::Pose		pose;
+//			pose.orientation		= tfmsg.rotation;
+//			pose.position.x		= tfmsg.translation.x;
+//			pose.position.y		= tfmsg.translation.y;
+//			pose.position.z		= tfmsg.translation.z;
+					  pose.orientation.x	= q.x();
+					  pose.orientation.y	= q.y();
+					  pose.orientation.z	= q.z();
+					  pose.orientation.w	= q.w();
+					  pose.position.x		= m(0,3);
+					  pose.position.y		= m(1,3);
+					  pose.position.z		= m(2,3);
+
+
+			cv_bridge::CvImage rgbBridgeImage;
+			rgbBridgeImage.image = rgbimage;
+			rgbBridgeImage.encoding = "bgr8";
+
+			cv_bridge::CvImage depthBridgeImage;
+			depthBridgeImage.image = depthimage;
+			depthBridgeImage.encoding = "mono16";
+
+			quasimodo_msgs::index_frame ifsrv;
+			ifsrv.request.frame.capture_time = ros::Time();
+			ifsrv.request.frame.pose		= pose;
+			ifsrv.request.frame.frame_id	= -1;
+			ifsrv.request.frame.rgb		= *(rgbBridgeImage.toImageMsg());
+			ifsrv.request.frame.depth		= *(depthBridgeImage.toImageMsg());
+
+
+			//314.458000 242.038000 536.458000 537.422000
+			ifsrv.request.frame.camera.K[0] = 536.458000;//cams[i][j].fx();
+			ifsrv.request.frame.camera.K[4] = 537.422000;//cams[i][j].fy();
+			ifsrv.request.frame.camera.K[2] = 314.458000;//cams[i][j].cx();
+			ifsrv.request.frame.camera.K[5] = 242.038000;//cams[i][j].cy();
+
+			printf("camera: %f %f %f %f\n",cams[i][j].cx(),cams[i][j].cy(),cams[i][j].fx(),cams[i][j].fy());
+
+			if (index_frame_client.call(ifsrv)){//Add frame to model server
+				int frame_id = ifsrv.response.frame_id;
+				fadded.push_back(j);
+				fid.push_back(frame_id);
+				ROS_INFO("frame_id%i", frame_id );
+			}else{ROS_ERROR("Failed to call service index_frame");}
+
+			printf("stop adding frame %i\n",i);
+		}
+
+		for(unsigned int j = 0; j < fadded.size(); j++){
+			printf("start adding mask %i\n",i);
+			cv::Mat mask	= masks[i][fadded[j]];
+
+			cv_bridge::CvImage maskBridgeImage;
+			maskBridgeImage.image = mask;
+			maskBridgeImage.encoding = "mono8";
+
+			quasimodo_msgs::model_from_frame mff;
+			mff.request.mask		= *(maskBridgeImage.toImageMsg());
+			mff.request.isnewmodel	= (j == (fadded.size()-1));
+			mff.request.frame_id	= fid[j];
+
+			if (model_from_frame_client.call(mff)){//Build model from frame
+				int model_id = mff.response.model_id;
+				if(model_id > 0){
+					ROS_INFO("model_id%i", model_id );
+				}
+			}else{ROS_ERROR("Failed to call service index_frame");}
+
+			printf("stop adding mask %i\n",i);
+
+		}
+	}
 }
 
-int main(int argc, char** argv){
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformCloud(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud, Eigen::Matrix4d p){
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ret (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	ret->points.resize(cloud->points.size());
 
-    ros::init(argc, argv, "use_rares_client");
-    ros::NodeHandle n;
-    ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback);
+	const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3);
+	const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3);
+	const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3);
 
-	pg = new pcl::visualization::PCLVisualizer (argc, argv, "global_transform");
-	pg->addCoordinateSystem();
+	for(unsigned int i = 0; i < cloud->points.size(); i++){
+		pcl::PointXYZRGBNormal & p1 = cloud->points[i];
+		pcl::PointXYZRGBNormal & p2 = ret->points[i];
 
-    ros::NodeHandle pn("~");
-	for(int ar = 1; ar < argc; ar++){
-		string overall_folder = std::string(argv[ar]);
-		//string overall_folder = "/media/nbore/My Passport/icra_data_surfels/controlled_experiments/object_2_fire_extinguisher";
-		//overall_folder = "/media/johane/SSDstorage/icra_data_surfels/controlled_experiments/object_7_microwave/";
-		//overall_folder = "/media/johane/SSDstorage/icra_data_surfels/controlled_experiments/object_6_router_box/";
-		//overall_folder = "/media/johane/SSDstorage/icra_data_surfels/controlled_experiments/";
-		//pn.param<string>("folder", overall_folder, "");
+		p2.r = p1.r;
+		p2.g = p1.g;
+		p2.b = p1.b;
 
-		vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(overall_folder);
+		const double & src_x = p1.x;
+		const double & src_y = p1.y;
+		const double & src_z = p1.z;
 
-		for (auto sweep_xml : sweep_xmls) {
-			//if(viewrgbs.size() > 0){break;}
-	//if(rgbs.size() > 0){break;}
-			int slash_pos = sweep_xml.find_last_of("/");
-			std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/";
-			printf("folder: %s\n",sweep_folder.c_str());
-
-			QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml"));
-			vector<ObjectData> objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep<PointType>(sweep_folder+"room.xml");
-
-			int object_id;
-			for (unsigned int i=0; i< objects.size(); i++){
-				if (objects[i].objectScanIndices.size() != 0){object_id = i;}
-			}
-			int index = objectFiles[object_id].toStdString().find_last_of(".");
-			string object_root_name = objectFiles[object_id].toStdString().substr(0,index);
-
-			for (auto object : objects){
-				if (!object.objectScanIndices.size()){continue;}
-
-	//if(rgbs.size() > 0){break;}
-	pg->removeAllPointClouds();
-                printf("%i\n",int(object.vAdditionalViews.size()));
-
-				std::vector<cv::Mat> viewrgbs;
-				std::vector<cv::Mat> viewdepths;
-				std::vector<cv::Mat> viewmasks;
-				std::vector<tf::StampedTransform > viewtfs;
-				std::vector<Eigen::Matrix4f> viewposes;
-				//std::stringstream ss_pose_file; ss_pose_file<<sweep_folder<<"/object_"<<object_id<<"/";
-				char buf [1024];
-				sprintf(buf,"%s/object_%i/poses.txt",sweep_folder.c_str(),object_id);
-				std::vector<Eigen::Matrix4f> poses = getRegisteredViewPoses(std::string(buf), object.vAdditionalViews.size());
-				cout<<"Loaded "<<poses.size()<<" registered poses."<<endl;
-
-				//std::vector<Eigen::Matrix4f> rp = getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms)
-				// view AV clouds and masks
-
-				int step = std::max(1,int(0.5+double(object.vAdditionalViews.size())/20.0));
-				for (unsigned int i=0; i < 2000 && i<object.vAdditionalViews.size(); i+=step){
-					CloudPtr cloud = object.vAdditionalViews[i];
-
-					cv::Mat mask;
-					mask.create(cloud->height,cloud->width,CV_8UC1);
-					unsigned char * maskdata = (unsigned char *)mask.data;
-
-					cv::Mat rgb;
-					rgb.create(cloud->height,cloud->width,CV_8UC3);
-					unsigned char * rgbdata = (unsigned char *)rgb.data;
-
-					cv::Mat depth;
-					depth.create(cloud->height,cloud->width,CV_16UC1);
-					unsigned short * depthdata = (unsigned short *)depth.data;
-
-					unsigned int nr_data = cloud->height * cloud->width;
-					for(unsigned int j = 0; j < nr_data; j++){
-						maskdata[j] = 0;
-						PointType p = cloud->points[j];
-						rgbdata[3*j+0]	= p.b;
-						rgbdata[3*j+1]	= p.g;
-						rgbdata[3*j+2]	= p.r;
-						depthdata[j]	= short(5000.0 * p.z);
-					}
-
-					cout<<"Processing AV "<<i<<endl;
-
-					stringstream av_ss; av_ss<<object_root_name; av_ss<<"_additional_view_"; av_ss<<i; av_ss<<".txt";
-					string complete_av_mask_name = sweep_folder + av_ss.str();
-					ifstream av_mask_in(complete_av_mask_name);
-					if (!av_mask_in.is_open()){
-						cout<<"COULD NOT FIND AV MASK FILE "<<complete_av_mask_name<<endl;
-						continue;
-					}
-
-					CloudPtr av_mask(new Cloud);
-
-					int av_index;
-					while (av_mask_in.is_open() && !av_mask_in.eof()){
-						av_mask_in>>av_index;
-						av_mask->push_back(object.vAdditionalViews[i]->points[av_index]);
-						maskdata[av_index] = 255;
-					}
-
-					viewrgbs.push_back(rgb);
-					viewdepths.push_back(depth);
-					viewmasks.push_back(mask);
-					viewtfs.push_back(object.vAdditionalViewsTransforms[i]);
-					viewposes.push_back(poses[i+1]);
-
-					cv::namedWindow("rgbimage",	cv::WINDOW_AUTOSIZE);
-					cv::imshow(		"rgbimage",	rgb);
-					cv::namedWindow("depthimage",	cv::WINDOW_AUTOSIZE);
-					cv::imshow(		"depthimage",	depth);
-					cv::namedWindow("mask",	cv::WINDOW_AUTOSIZE);
-					cv::imshow(		"mask",	mask);
-					cv::waitKey(30);
-	//				{
-	//					std::cout << poses[i+1] << std::endl << std::endl;
-	//					pcl::PointCloud<PointType>::Ptr transformed_cloud (new pcl::PointCloud<PointType> ());
-	//					pcl::transformPointCloud (*(av_mask), *transformed_cloud, poses[i+1]);
-	//					PointCloudColorHandlerCustom<PointType> intermediate_h (transformed_cloud, 255, 0, 0);
-	//					pg->addPointCloud(transformed_cloud, intermediate_h, (complete_av_mask_name+"av").c_str());
-	//					pg->spin();
-    //				}
-				}
+		const double & src_nx = p1.normal_x;
+		const double & src_ny = p1.normal_y;
+		const double & src_nz = p1.normal_z;
 
-				if(viewrgbs.size() > 0){
-					//pg->spin();
-					rgbs.push_back(viewrgbs);
-					depths.push_back(viewdepths);
-					masks.push_back(viewmasks);
-					tfs.push_back(viewtfs);
-					initposes.push_back(viewposes);
-				}
-			}
-		}
+		p2.x = m00*src_x + m01*src_y + m02*src_z + m03;
+		p2.y = m10*src_x + m11*src_y + m12*src_z + m13;
+		p2.z = m20*src_x + m21*src_y + m22*src_z + m23;
+
+		p2.normal_x = m00*src_nx + m01*src_ny + m02*src_nz;
+		p2.normal_y = m10*src_nx + m11*src_ny + m12*src_nz;
+		p2.normal_z = m20*src_nx + m21*src_ny + m22*src_nz;
+
+		p2.r = p1.r;
+		p2.g = p1.g;
+		p2.b = p1.b;
 	}
 
-    for(unsigned int i = 0; i < rgbs.size(); i++){
-        int ind = rand()%rgbs.size();
-        std::iter_swap(rgbs.begin()+i,rgbs.begin()+ind);
-        std::iter_swap(depths.begin()+i,depths.begin()+ind);
-        std::iter_swap(masks.begin()+i,masks.begin()+ind);
-        std::iter_swap(tfs.begin()+i,tfs.begin()+ind);
-        std::iter_swap(initposes.begin()+i,initposes.begin()+ind);
+	return ret;
+}
+
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr getNC(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int nr_data = 5000000){
+	int MaxDepthChangeFactor = 20;
+	int NormalSmoothingSize = 7;
+	int depth_dependent_smoothing = 1;
+
+	pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
+	ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor));
+	ne.setNormalSmoothingSize(NormalSmoothingSize);
+	ne.setDepthDependentSmoothing (depth_dependent_smoothing);
+	ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
+
+	pcl::PointCloud<pcl::Normal>::Ptr	normals (new pcl::PointCloud<pcl::Normal>);
+	ne.setInputCloud(cloud);
+	ne.compute(*normals);
+
+	int rc = rand()%256;
+	int gc = rand()%256;
+	int bc = rand()%256;
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ret (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::concatenateFields(*cloud,*normals,*ret);
+
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+	viewer->setBackgroundColor (0.5, 0, 0.5);
+	viewer->addCoordinateSystem (1.0);
+	viewer->initCameraParameters ();
+
+	viewer->removeAllPointClouds();
+	viewer->addPointCloud<pcl::PointXYZRGB> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(cloud), "sample cloud");
+	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+	viewer->spin();
+
+	viewer->removeAllPointClouds();
+	viewer->addPointCloud<pcl::PointXYZRGBNormal> (ret, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(ret), "sample cloud");
+	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+	viewer->addPointCloudNormals<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> (ret, ret, 15, 0.10, "normals");
+	viewer->spin();
+
+
+//	ret->points.resize(cloud->points.size());
+//	for(int i = 0; i < cloud->points.size(); i++){
+//		pcl::PointXYZRGBNormal & p1 = ret->points[i];
+//		pcl::PointXYZRGB & p = cloud->points[i];
+//		pcl::Normal & pn = normals->points[i];
+
+//		p1.x = p.x;
+//		p1.y = p.y;
+//		p1.z = p.z;
+////		p1.r = rc;
+////		p1.g = gc;
+////		p1.b = bc;
+//		p1.r = p.r;
+//		p1.g = p.g;
+//		p1.b = p.b;
+//		p1.normal_x = pn.normal_x;
+//		p1.normal_y = pn.normal_y;
+//		p1.normal_z = pn.normal_z;
+//	}
+/*
+
+*/
+
+//	for(int i = 0; i < ret->points.size(); i++){
+//		pcl::PointXYZRGBNormal & p1 = ret->points[i];
+//		p1.r = rc;
+//		p1.g = gc;
+//		p1.b = bc;
+//	}
+	return ret;
+}
+
+int nr_vAdditionalViews = 4000;
+
+void load(std::string sweep_xml){
+
+    int slash_pos = sweep_xml.find_last_of("/");
+    std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/";
+    printf("folder: %s\n",sweep_folder.c_str());
+
+    SimpleXMLParser<PointType> parser;
+    SimpleXMLParser<PointType>::RoomData roomData  = parser.loadRoomFromXML(sweep_folder+"/room.xml");
+
+    QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml"));
+    vector<ObjectData> objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep<PointType>(sweep_folder+"room.xml");
+
+    int object_id;
+    for (unsigned int i=0; i< objects.size(); i++){
+        if (objects[i].objectScanIndices.size() != 0){object_id = i;}
     }
+    int index = objectFiles[object_id].toStdString().find_last_of(".");
+    string object_root_name = objectFiles[object_id].toStdString().substr(0,index);
+
+    for (auto object : objects){
+        if (!object.objectScanIndices.size()){continue;}
+
+        //nr_vAdditionalViews = 1 + rand()%5;
+
+        std::vector<cv::Mat> viewrgbs;
+        std::vector<cv::Mat> viewdepths;
+        std::vector<cv::Mat> viewmasks;
+        std::vector<tf::StampedTransform > viewtfs;
+        std::vector<Eigen::Matrix4f> viewposes;
+        std::vector< image_geometry::PinholeCameraModel > viewcams;
+        char buf [1024];
+        sprintf(buf,"%s/object_%i/poses.txt",sweep_folder.c_str(),object_id);
+        std::vector<Eigen::Matrix4f> poses = getRegisteredViewPoses(std::string(buf), object.vAdditionalViews.size());
+        cout<<"Loaded "<<poses.size()<<" registered poses."<<endl;
+
+		for (unsigned int i=0; i < nr_vAdditionalViews && i<object.vAdditionalViews.size(); i++){
+            CloudPtr cloud = object.vAdditionalViews[i];
+
+            cv::Mat mask;
+            mask.create(cloud->height,cloud->width,CV_8UC1);
+            unsigned char * maskdata = (unsigned char *)mask.data;
+
+            cv::Mat rgb;
+            rgb.create(cloud->height,cloud->width,CV_8UC3);
+            unsigned char * rgbdata = (unsigned char *)rgb.data;
+
+            cv::Mat depth;
+            depth.create(cloud->height,cloud->width,CV_16UC1);
+            unsigned short * depthdata = (unsigned short *)depth.data;
+
+            unsigned int nr_data = cloud->height * cloud->width;
+            for(unsigned int j = 0; j < nr_data; j++){
+                maskdata[j] = 0;
+                PointType p = cloud->points[j];
+                rgbdata[3*j+0]	= p.b;
+                rgbdata[3*j+1]	= p.g;
+                rgbdata[3*j+2]	= p.r;
+                depthdata[j]	= short(5000.0 * p.z);
+            }
+
+            cout<<"Processing AV "<<i<<endl;
+
+            stringstream av_ss; av_ss<<object_root_name; av_ss<<"_additional_view_"; av_ss<<i; av_ss<<".txt";
+            string complete_av_mask_name = sweep_folder + av_ss.str();
+            ifstream av_mask_in(complete_av_mask_name);
+            if (!av_mask_in.is_open()){
+                cout<<"COULD NOT FIND AV MASK FILE "<<complete_av_mask_name<<endl;
+                continue;
+            }
+
+            CloudPtr av_mask(new Cloud);
+
+            int av_index;
+            while (av_mask_in.is_open() && !av_mask_in.eof()){
+                av_mask_in>>av_index;
+                av_mask->push_back(object.vAdditionalViews[i]->points[av_index]);
+                maskdata[av_index] = 255;
+            }
+
+            viewrgbs.push_back(rgb);
+            viewdepths.push_back(depth);
+            viewmasks.push_back(mask);
+            viewtfs.push_back(object.vAdditionalViewsTransforms[i]);
+            viewposes.push_back(poses[i+1]);
+            viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front());
+
+            cv::namedWindow("rgbimage",     cv::WINDOW_AUTOSIZE);
+            cv::imshow(		"rgbimage",     rgb);
+            cv::namedWindow("depthimage",	cv::WINDOW_AUTOSIZE);
+            cv::imshow(		"depthimage",	depth);
+            cv::namedWindow("mask",         cv::WINDOW_AUTOSIZE);
+            cv::imshow(		"mask",         mask);
+            cv::waitKey(30);
+        }
+
+        if(viewrgbs.size() > 0){
+
+			rgbs.push_back(viewrgbs);
+			depths.push_back(viewdepths);
+			masks.push_back(viewmasks);
+			tfs.push_back(viewtfs);
+			initposes.push_back(viewposes);
+			cams.push_back(viewcams);
+/*
+            tf::StampedTransform tf	= roomData.vIntermediateRoomCloudTransforms.front();
+            geometry_msgs::TransformStamped tfstmsg;
+            tf::transformStampedTFToMsg (tf, tfstmsg);
+            geometry_msgs::Transform tfmsg = tfstmsg.transform;
+            geometry_msgs::Pose		pose;
+            pose.orientation		= tfmsg.rotation;
+            pose.position.x		= tfmsg.translation.x;
+            pose.position.y		= tfmsg.translation.y;
+            pose.position.z		= tfmsg.translation.z;
+            Eigen::Affine3d localthing;
+            tf::poseMsgToEigen(pose, localthing);
+
+            reglib::Camera * cam		= new reglib::Camera();//TODO:: ADD TO CAMERAS
+            cam->fx = viewcams.front().fx();
+            cam->fy = viewcams.front().fy();
+            cam->cx = viewcams.front().cx();
+            cam->cy = viewcams.front().cy();
+
+
+            cv::Mat fullmask;
+            fullmask.create(480,640,CV_8UC1);
+            unsigned char *take maskdata = (unsigned char *)fullmask.data;
+            for(int j = 0; j < 480*640; j++){maskdata[j] = 0;}
+
+
+            reglib::Model * objectModel = 0;//new reglib::Model(frames[frame_id],mask);
+
+            std::vector<reglib::RGBDFrame * > current_frames;
+			for (unsigned int i=0;i < nr_vAdditionalViews && i<object.vAdditionalViews.size(); i++){
+                tf::StampedTransform tf	= object.vAdditionalViewsTransforms[i];
+                geometry_msgs::TransformStamped tfstmsg;
+                tf::transformStampedTFToMsg (tf, tfstmsg);
+                geometry_msgs::Transform tfmsg = tfstmsg.transform;
+                geometry_msgs::Pose		pose;
+                pose.orientation		= tfmsg.rotation;
+                pose.position.x		= tfmsg.translation.x;
+                pose.position.y		= tfmsg.translation.y;
+                pose.position.z		= tfmsg.translation.z;
+                Eigen::Affine3d epose;
+                tf::poseMsgToEigen(pose, epose);
+                epose = localthing.inverse()*epose;
+
+                reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],0, epose.matrix());
+                current_frames.push_back(frame);
+
+                if(i == 0){
+                    objectModel = new reglib::Model(frame,viewmasks[i]);
+                }else{
+                    objectModel->frames.push_back(frame);
+                    objectModel->relativeposes.push_back(current_frames.front()->pose.inverse() * frame->pose);
+                    objectModel->modelmasks.push_back(new reglib::ModelMask(viewmasks[i]));//fullmask));
+                }
+            }
+
+			reglib::Model * sweepmodel = 0;
+            std::vector<reglib::RGBDFrame * > current_room_frames;
+            for (size_t i=0; i<roomData.vIntermediateRoomClouds.size(); i++)
+            {
+                cout<<"Intermediate cloud size "<<roomData.vIntermediateRoomClouds[i]->points.size()<<endl;
+
+                //Transform
+                tf::StampedTransform tf	= roomData.vIntermediateRoomCloudTransformsRegistered[i];
+                geometry_msgs::TransformStamped tfstmsg;
+                tf::transformStampedTFToMsg (tf, tfstmsg);
+                geometry_msgs::Transform tfmsg = tfstmsg.transform;
+                geometry_msgs::Pose		pose;
+                pose.orientation		= tfmsg.rotation;
+                pose.position.x		= tfmsg.translation.x;
+                pose.position.y		= tfmsg.translation.y;
+                pose.position.z		= tfmsg.translation.z;
+                Eigen::Affine3d epose;
+                tf::poseMsgToEigen(pose, epose);
+
+                reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, epose.matrix());
+                current_room_frames.push_back(frame);
+                if(i == 0){
+                    sweepmodel = new reglib::Model(frame,fullmask);
+                }else{
+                    sweepmodel->frames.push_back(frame);
+                    sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose);
+                    sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask));
+                }
+            }
+            sweepmodel->recomputeModelPoints();
+
+//            objectModel->submodels.push_back(sweepmodel);
+//            objectModel->submodels_relativeposes.push_back(objectModel->frames.front()->pose.inverse() * sweepmodel->frames.front()->pose);
+
+
+            reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+            reglib::ModelUpdaterBasicFuse * mu	= new reglib::ModelUpdaterBasicFuse( objectModel, reg);
+            mu->occlusion_penalty               = 15;
+            mu->massreg_timeout                 = 60*4;
+            mu->viewer							= viewer;
+
+            objectModel->print();
+            mu->makeInitialSetup();
+            objectModel->print();
+            delete mu;
+
+            objectModel->recomputeModelPoints();
+
+            reglib::Model * newmodelHolder = new reglib::Model();
+            newmodelHolder->submodels.push_back(objectModel);
+            newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity());
+            newmodelHolder->recomputeModelPoints();
+
+            pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cn1 = objectModel->getPCLnormalcloud(1, false);
+            viewer->removeAllPointClouds();
+            viewer->addPointCloud<pcl::PointXYZRGBNormal> (cn1, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cn1), "sample cloud");
+            viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+            viewer->spin();
+
+            pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cn = sweepmodel->getPCLnormalcloud(1, false);
+            viewer->removeAllPointClouds();
+            viewer->addPointCloud<pcl::PointXYZRGBNormal> (cn, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cn), "sample cloud");
+            viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+            viewer->spin();
+*/
+//            rgbs.push_back(viewrgbs);
+//            depths.push_back(viewdepths);
+//            masks.push_back(viewmasks);
+//            tfs.push_back(viewtfs);
+//            initposes.push_back(viewposes);
+//            cams.push_back(viewcams);
+        }
+    }
+}
+
+int main(int argc, char** argv){
+
+	ros::init(argc, argv, "use_rares_client");
+	ros::NodeHandle n;
+	ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback);
 
-    model_from_frame_client	= n.serviceClient<quasimodo_msgs::model_from_frame>("model_from_frame");
-    fuse_models_client		= n.serviceClient<quasimodo_msgs::fuse_models>(		"fuse_models");
-    get_model_client			= n.serviceClient<quasimodo_msgs::get_model>(		"get_model");
-    index_frame_client		= n.serviceClient<quasimodo_msgs::index_frame>(		"index_frame");
+    viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
+	viewer->setBackgroundColor (0.5, 0, 0.5);
+	viewer->addCoordinateSystem (1.0);
+	viewer->initCameraParameters ();
+
+	ros::NodeHandle pn("~");
+	for(int ar = 1; ar < argc; ar++){
+		string overall_folder = std::string(argv[ar]);
+
+        vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(overall_folder);
+		for (auto sweep_xml : sweep_xmls) {
+            load(sweep_xml);
+		}
+	}
 
-    ros::spin();
+//	for(unsigned int i = 0; i < rgbs.size(); i++){
+//		int ind = rand()%rgbs.size();
+//		std::iter_swap(rgbs.begin()+i,rgbs.begin()+ind);
+//		std::iter_swap(depths.begin()+i,depths.begin()+ind);
+//		std::iter_swap(masks.begin()+i,masks.begin()+ind);
+//		std::iter_swap(tfs.begin()+i,tfs.begin()+ind);
+//		std::iter_swap(initposes.begin()+i,initposes.begin()+ind);
+//	}
+
+	model_from_frame_client	= n.serviceClient<quasimodo_msgs::model_from_frame>("model_from_frame");
+	fuse_models_client		= n.serviceClient<quasimodo_msgs::fuse_models>(		"fuse_models");
+	get_model_client		= n.serviceClient<quasimodo_msgs::get_model>(		"get_model");
+	index_frame_client		= n.serviceClient<quasimodo_msgs::index_frame>(		"index_frame");
+printf("ready to give data\n");
+	ros::spin();
 }
diff --git a/quasimodo/quasimodo_brain/src/robot_listener.cpp b/quasimodo/quasimodo_brain/src/robot_listener.cpp
index ebac8c53..ac2ba99e 100644
--- a/quasimodo/quasimodo_brain/src/robot_listener.cpp
+++ b/quasimodo/quasimodo_brain/src/robot_listener.cpp
@@ -99,6 +99,8 @@ std::vector< std::vector<cv::Mat> > masks;
 std::vector< std::vector<tf::StampedTransform > >tfs;
 std::vector< std::vector<Eigen::Matrix4f> > initposes;
 
+ros::Publisher metaroom_pub;
+
 std::vector<Eigen::Matrix4f> getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms){
     std::vector<Eigen::Matrix4f> toRet;
     ifstream in(poses_file);
@@ -127,12 +129,24 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg)
 
     ROS_INFO("I heard: [%s]", msg->data.c_str());
 
+    std::string str = msg->data.c_str();
+
     ObjectData object = semantic_map_load_utilties::loadDynamicObjectFromSingleSweep<PointType>(msg->data.c_str());
 
     printf("number of inds: %i",int(object.objectScanIndices.size()));
     printf("AVs: %i\n",int(object.vAdditionalViews.size()));
     if (!object.vAdditionalViews.size()){return;}
 
+    std::cout << "Splitting: " << str << '\n';
+    std::size_t found = str.find_last_of("/\\");
+    std::cout << " path: " << str.substr(0,found) << '\n';
+    std::cout << " file: " << str.substr(found+1) << '\n';
+
+    //SimpleXMLParser<PointType> parser;
+    //SimpleXMLParser<PointType>::RoomData roomData  = parser.loadRoomFromXML(str.substr(0,found)+"/room.xml");
+
+    //roomData.completeRoomCloud();
+
 
 
     std::vector<cv::Mat> viewrgbs;
@@ -141,7 +155,7 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg)
     std::vector<tf::StampedTransform > viewtfs;
     std::vector<Eigen::Matrix4f> viewposes;
 
-    int step = std::max(1,int(0.5+double(object.vAdditionalViews.size())/10.0));
+    int step = 1;//std::max(1,int(0.5+double(object.vAdditionalViews.size())/10.0));
 	printf("step: %i\n",step);
 
     for (unsigned int i=0; i < 2000 && i<object.vAdditionalViews.size(); i+=step){
@@ -184,7 +198,8 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg)
         viewrgbs.push_back(rgb);
         viewdepths.push_back(depth);
         viewmasks.push_back(mask);
-        viewtfs.push_back(object.vAdditionalViewsTransformsRegistered[i]);
+        //viewtfs.push_back(object.vAdditionalViewsTransformsRegistered[i]);
+        viewtfs.push_back(object.vAdditionalViewsTransforms[i]);
 
 //		cv::namedWindow("rgbimage",	cv::WINDOW_AUTOSIZE);
 //		cv::imshow(		"rgbimage",	rgb);
@@ -216,9 +231,9 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg)
 
             geometry_msgs::Pose		pose;
             pose.orientation		= tfmsg.rotation;
-            pose.position.x		= tfmsg.translation.x;
-            pose.position.y		= tfmsg.translation.y;
-            pose.position.z		= tfmsg.translation.z;
+            pose.position.x         = tfmsg.translation.x;
+            pose.position.y         = tfmsg.translation.y;
+            pose.position.z         = tfmsg.translation.z;
             //            pose.orientation.x	= q.x();
             //            pose.orientation.y	= q.y();
             //            pose.orientation.z	= q.z();
@@ -228,6 +243,8 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg)
             //            pose.position.z		= m(2,3);
 
 
+
+
             cv_bridge::CvImage rgbBridgeImage;
             rgbBridgeImage.image = rgbimage;
             rgbBridgeImage.encoding = "bgr8";
@@ -243,6 +260,15 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg)
             ifsrv.request.frame.rgb			= *(rgbBridgeImage.toImageMsg());
             ifsrv.request.frame.depth		= *(depthBridgeImage.toImageMsg());
 
+//			ifsrv.request.frame.camera.K[0] = 536.458000;//cams[i][j].fx();
+//			ifsrv.request.frame.camera.K[4] = 537.422000;//cams[i][j].fy();
+//			ifsrv.request.frame.camera.K[2] = 314.458000;//cams[i][j].cx();
+//			ifsrv.request.frame.camera.K[5] = 242.038000;//cams[i][j].cy();
+//533.796,0,314.863,0,0,533.113,241.271,0,0,0,1,0,
+            ifsrv.request.frame.camera.K[0] = 533.796;
+            ifsrv.request.frame.camera.K[4] = 533.113;
+            ifsrv.request.frame.camera.K[2] = 314.863;
+            ifsrv.request.frame.camera.K[5] = 241.271;
             if (index_frame_client.call(ifsrv)){//Add frame to model server
                 int frame_id = ifsrv.response.frame_id;
                 fadded.push_back(j);
@@ -451,7 +477,7 @@ void chatterCallback2(const std_msgs::String::ConstPtr& msg)
 }
 
 int main(int argc, char** argv){
-    ros::init(argc, argv, "use_rares_client");
+    ros::init(argc, argv, "robot_listener");
     ros::NodeHandle n;
 
     std::string listentopic = "/some/stupid/topic";
diff --git a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp
new file mode 100644
index 00000000..6e4debf7
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp
@@ -0,0 +1,172 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+#include "metaroom_xml_parser/simple_summary_parser.h"
+
+#include <tf_conversions/tf_eigen.h>
+
+#include "ros/ros.h"
+#include <metaroom_xml_parser/load_utilities.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "metaroom_xml_parser/load_utilities.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include <tf_conversions/tf_eigen.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+#include "quasimodo_msgs/segment_model.h"
+#include "quasimodo_msgs/metaroom_pair.h"
+
+#include "ros/ros.h"
+#include <quasimodo_msgs/query_cloud.h>
+#include <quasimodo_msgs/visualize_query.h>
+#include <metaroom_xml_parser/load_utilities.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "metaroom_xml_parser/load_utilities.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+#include "Util/Util.h"
+
+ros::ServiceClient segmentation_client;
+
+
+using namespace std;
+
+bool segment_metaroom(quasimodo_msgs::metaroom_pair::Request  & req, quasimodo_msgs::metaroom_pair::Response & res){
+	printf("segment_metaroom\n");
+
+	printf("background: %s\n",req.background.c_str());
+	printf("foreground: %s\n",req.foreground.c_str());
+
+	reglib::Model * bg_model = quasimodo_brain::load_metaroom_model(req.background);
+	reglib::Model * fg_model = quasimodo_brain::load_metaroom_model(req.foreground);
+
+	quasimodo_msgs::segment_model sm;
+	sm.request.backgroundmodel = quasimodo_brain::getModelMSG(bg_model);
+	sm.request.models.push_back(quasimodo_brain::getModelMSG(fg_model));
+
+	bool status;
+
+	if (segmentation_client.call(sm)){
+		if(sm.response.dynamicmasks.size() > 0){
+			res.dynamicmasks	= sm.response.dynamicmasks.front().images;
+			res.movingmasks		= sm.response.movingmasks.front().images;
+		}
+		status = true;
+	}else{
+		ROS_ERROR("Failed to call service segment_model");
+		status = false;
+	}
+
+	bg_model->fullDelete();
+	delete bg_model;
+	fg_model->fullDelete();
+	delete fg_model;
+	return status;
+}
+
+int main(int argc, char** argv){
+	ROS_INFO("starting segment_room_pairs");
+	ros::init(argc, argv, "segmentationserver_metaroom");
+	ros::NodeHandle n;
+	segmentation_client = n.serviceClient<quasimodo_msgs::segment_model>("segment_model");
+	ros::ServiceServer service = n.advertiseService("segment_metaroom", segment_metaroom);
+
+	ROS_INFO("running segment_room_pairs");
+	ros::spin();
+	/*
+    reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+
+	viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
+	viewer->setBackgroundColor (0.5, 0, 0.5);
+	viewer->addCoordinateSystem (1.0);
+	viewer->initCameraParameters ();
+
+    for(int ar = 1; ar < argc; ar++){
+        string overall_folder = std::string(argv[ar]);
+        vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(overall_folder);
+        printf("sweep_xmls\n");
+        for (auto sweep_xml : sweep_xmls) {
+            printf("sweep_xml: %s\n",sweep_xml.c_str());
+            load2(sweep_xml);
+        }
+    }
+
+	//Not needed if metaroom well calibrated
+    reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01);
+    bgmassreg->timeout = 20;
+    bgmassreg->viewer = viewer;
+    bgmassreg->use_surface = true;
+    bgmassreg->use_depthedge = false;
+    bgmassreg->visualizationLvl = 0;
+    bgmassreg->maskstep = 10;
+    bgmassreg->nomaskstep = 10;
+    bgmassreg->nomask = true;
+    bgmassreg->stopval = 0.0005;
+    bgmassreg->setData(models.front()->frames,models.front()->modelmasks);
+    reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes);
+    delete bgmassreg;
+
+
+	ros::init(argc, argv, "test_segment");
+	ros::NodeHandle n;
+	ros::ServiceClient segmentation_client = n.serviceClient<quasimodo_msgs::segment_model>("segment_model");
+
+	for(unsigned int i = 1; i < models.size(); i++){
+		quasimodo_msgs::segment_model sm;
+		sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i]));
+
+		if(i > 0){
+			sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]);
+		}
+
+		if (segmentation_client.call(sm)){//Build model from frame
+			//int model_id = mff.response.model_id;
+			printf("segmented: %i\n",i);
+		}else{ROS_ERROR("Failed to call service segment_model");}
+	}
+	//ros::spin();
+
+    delete reg;
+    for(size_t j = 0; j < models.size(); j++){
+        models[j]->fullDelete();
+        delete models[j];
+    }
+	printf("done\n");
+	*/
+	return 0;
+}
diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp
new file mode 100644
index 00000000..03ad3461
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp
@@ -0,0 +1,194 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+
+#include "quasimodo_msgs/segment_model.h"
+
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+
+#include "Util/Util.h"
+
+
+using namespace std;
+
+
+bool visualization = false;
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+
+//Assume internally correctly registered...
+
+//Strategy:
+//Register frames to background+other frames in the same model DONE
+//Determine occlusions inside current model to filter out motion(most likeley people etc) //Done
+    //Remove from current model
+//Determine occlusion of current model to background
+    //These pixels are dynamic objects
+//Update background
+    //How?
+
+bool segment_model(quasimodo_msgs::segment_model::Request  & req, quasimodo_msgs::segment_model::Response & res){
+	printf("segment_model\n");
+
+	std::vector< reglib::Model * > models;
+	for(unsigned int i = 0; i < req.models.size(); i++){
+		models.push_back(quasimodo_brain::getModelFromMSG(req.models[i]));
+	}
+
+	reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel);
+
+	std::vector< std::vector< cv::Mat > > internal;
+	std::vector< std::vector< cv::Mat > > external;
+	std::vector< std::vector< cv::Mat > > dynamic;
+
+	std::vector< reglib::Model * > bgs;
+	bgs.push_back(bg);
+
+	//quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization);
+	quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization);
+	printf("visualization: %i\n",visualization);
+
+	for(unsigned int i = 0; false && visualization && i < models.size(); i++){
+		std::vector<cv::Mat> internal_masks = internal[i];
+		std::vector<cv::Mat> external_masks = external[i];
+		std::vector<cv::Mat> dynamic_masks	= dynamic[i];
+		reglib::Model * model = models[i];
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		for(unsigned int j = 0; j < model->frames.size(); j++){
+			reglib::RGBDFrame * frame = model->frames[j];
+			unsigned char  * rgbdata		= (unsigned char	*)(frame->rgb.data);
+			unsigned short * depthdata		= (unsigned short	*)(frame->depth.data);
+			float		   * normalsdata	= (float			*)(frame->normals.data);
+
+			reglib::Camera * camera = frame->camera;
+
+			unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data);
+			unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data);
+			unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data);
+
+			Eigen::Matrix4d p = model->relativeposes[j];
+			float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+			float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+			float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+			const float idepth			= camera->idepth_scale;
+			const float cx				= camera->cx;
+			const float cy				= camera->cy;
+			const float ifx				= 1.0/camera->fx;
+			const float ify				= 1.0/camera->fy;
+			const unsigned int width	= camera->width;
+			const unsigned int height	= camera->height;
+
+			for(unsigned int w = 0; w < width;w++){
+				for(unsigned int h = 0; h < height;h++){
+					int ind = h*width+w;
+					float z = idepth*float(depthdata[ind]);
+					if(z > 0){
+						float x = (float(w) - cx) * z * ifx;
+						float y = (float(h) - cy) * z * ify;
+						pcl::PointXYZRGBNormal point;
+						point.x = m00*x + m01*y + m02*z + m03;
+						point.y = m10*x + m11*y + m12*z + m13;
+						point.z = m20*x + m21*y + m22*z + m23;
+						point.b = rgbdata[3*ind+0];
+						point.g = rgbdata[3*ind+1];
+						point.r = rgbdata[3*ind+2];
+						if(dynamicmaskdata[ind] != 0){
+							point.b = 0;
+							point.g = 255;
+							point.r = 0;
+						}else if(internalmaskdata[ind] == 0){
+							point.b = 0;
+							point.g = 0;
+							point.r = 255;
+						}
+						cloud->points.push_back(point);
+					}
+				}
+			}
+		}
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud), "scloud");
+		viewer->spin();
+	}
+
+//	for(unsigned int i = 0; visualization && i < models.size(); i++){
+//		std::vector<cv::Mat> internal_masks = internal[i];
+//		std::vector<cv::Mat> external_masks = external[i];
+//		std::vector<cv::Mat> dynamic_masks	= dynamic[i];
+//		reglib::Model * model = models[i];
+//		for(unsigned int j = 0; j < model->frames.size(); j++){
+//			cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE );		cv::imshow( "rgb",		models[i]->frames[j]->rgb);
+//			cv::namedWindow( "moving", cv::WINDOW_AUTOSIZE );	cv::imshow( "moving",	255*(internal[i][j] > 0));
+//			cv::namedWindow( "dynamic", cv::WINDOW_AUTOSIZE );	cv::imshow( "dynamic",	255*(dynamic[i][j] > 0));
+//			cv::waitKey(0);
+//		}
+//	}
+	res.backgroundmodel = req.backgroundmodel;
+
+	res.dynamicmasks.resize(models.size());
+	res.movingmasks.resize(models.size());
+	for(unsigned int i = 0; i < models.size(); i++){
+		for(unsigned int j = 0; j < models[i]->frames.size(); j++){
+			cv_bridge::CvImage dynamicmaskBridgeImage;
+			dynamicmaskBridgeImage.image			= dynamic[i][j];
+			dynamicmaskBridgeImage.encoding			= "mono8";
+			res.dynamicmasks[i].images.push_back( *(dynamicmaskBridgeImage.toImageMsg()) );
+
+			cv_bridge::CvImage internalmaskBridgeImage;
+			internalmaskBridgeImage.image			= internal[i][j];
+			internalmaskBridgeImage.encoding		= "mono8";
+			res.movingmasks[i].images.push_back( *(internalmaskBridgeImage.toImageMsg()) );
+
+//			cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE );		cv::imshow( "rgb",		models[i]->frames[j]->rgb);
+//			cv::namedWindow( "moving", cv::WINDOW_AUTOSIZE );	cv::imshow( "moving",	255*(internal[i][j] > 0));
+//			cv::namedWindow( "dynamic", cv::WINDOW_AUTOSIZE );	cv::imshow( "dynamic",	255*(dynamic[i][j] > 0));
+//			cv::waitKey(0);
+		}
+		res.models.push_back(quasimodo_brain::getModelMSG(models[i]));
+	}
+
+
+	for(unsigned int i = 0; i < models.size(); i++){
+		models[i]->fullDelete();
+		delete models[i];
+	}
+	models.clear();
+
+	bg->fullDelete();
+	delete bg;
+
+	return true;
+}
+
+int main(int argc, char** argv){
+ROS_INFO("starting segmentserver.");
+	ros::init(argc, argv, "segmentationserver");
+	ros::NodeHandle n;
+	for(int i = 1; i < argc;i++){
+		printf("input: %s\n",argv[i]);
+		if(std::string(argv[i]).compare("-v") == 0){           printf("visualization turned on\n");                visualization = true;}
+	}
+
+	if(visualization){
+		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("viewer"));
+		viewer->addCoordinateSystem(0.01);
+		viewer->setBackgroundColor(0.0,0.0,0.0);
+	}
+
+	ros::ServiceServer service = n.advertiseService("segment_model", segment_model);
+ROS_INFO("Ready to add use segment_model.");
+
+	ros::spin();
+}
diff --git a/quasimodo/quasimodo_brain/src/test_align2.cpp b/quasimodo/quasimodo_brain/src/test_align2.cpp
new file mode 100644
index 00000000..c17839eb
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/test_align2.cpp
@@ -0,0 +1,303 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+
+#include <sys/time.h>
+
+#include "../../quasimodo_models/include/modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+#include "core/Util.h"
+#include <random>
+
+#include <omp.h>
+
+#include "ceres/ceres.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "ceres/rotation.h"
+#include "ceres/iteration_callback.h"
+
+using namespace std;
+using namespace Eigen;
+using namespace reglib;
+
+using ceres::NumericDiffCostFunction;
+using ceres::SizedCostFunction;
+using ceres::CENTRAL;
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+using ceres::Solve;
+
+default_random_engine generator;
+Matrix3Xd conc(Matrix3Xd A, Matrix3Xd B){
+	Eigen::Matrix3Xd C	= Eigen::Matrix3Xd::Zero(3,	A.cols()+B.cols());
+	C << A, B;
+	return C;
+}
+Matrix3Xd getPoints(int cols){
+	Eigen::Matrix3Xd points	= Eigen::Matrix3Xd::Zero(3,cols);
+	for(int i = 0; i < 3; i++){
+		for(int j = 0; j < cols; j++){
+			points(i,j) = (rand()%10000)*0.0001;
+		}
+	}
+	return points;
+}
+Matrix3Xd getMeasurements(Matrix3Xd gt, double noise){
+	int cols = gt.cols();
+
+	normal_distribution<double> distribution(0,noise);
+	Eigen::Matrix3Xd points	= Eigen::Matrix3Xd::Zero(3,cols);
+	for(int i = 0; i < 3; i++){
+		for(int j = 0; j < cols; j++){
+			points(i,j) = gt(i,j) + distribution(generator);
+		}
+	}
+	return points;
+}
+
+Matrix3Xd transform_points(Matrix3Xd points, Matrix4d transform){
+	int cols = points.cols();
+
+	float m00 = transform(0,0); float m01 = transform(0,1); float m02 = transform(0,2); float m03 = transform(0,3);
+	float m10 = transform(1,0); float m11 = transform(1,1); float m12 = transform(1,2); float m13 = transform(1,3);
+	float m20 = transform(2,0); float m21 = transform(2,1); float m22 = transform(2,2); float m23 = transform(2,3);
+
+	Eigen::Matrix3Xd points2	= Eigen::Matrix3Xd::Zero(3,cols);
+	for(int i = 0; i < cols; i++){
+		float x	= points(0,i);
+		float y	= points(1,i);
+		float z	= points(2,i);
+		points2(0,i)	= m00*x + m01*y + m02*z + m03;
+		points2(1,i)	= m10*x + m11*y + m12*z + m13;
+		points2(2,i)	= m20*x + m21*y + m22*z + m23;
+	}
+	return points2;
+}
+
+double get_reconst_rms(Matrix3Xd gt, Matrix3Xd points, int nr_points){
+	double rms = 0;
+	for(int i = 0; i < nr_points; i++){
+		float dx	= gt(0,i) - points(0,i);
+		float dy	= gt(1,i) - points(1,i);
+		float dz	= gt(2,i) - points(2,i);
+		rms += dx*dx+dy*dy+dz*dz;
+	}
+	rms/= double(nr_points);
+	rms = sqrt(rms);
+	return rms;
+}
+
+
+#include <pcl/common/transformation_from_correspondences.h>
+
+Matrix4d align1(Matrix3Xd gt, Matrix3Xd points){
+	pcl::TransformationFromCorrespondences tfc;
+	for(int i = 0; i < gt.cols(); i++){
+		tfc.add(Eigen::Vector3f(gt(0,i),gt(1,i),gt(2,i)),Eigen::Vector3f(points(0,i),points(1,i),points(2,i)));
+	}
+	return tfc.getTransformation().matrix().cast<double>().inverse();
+}
+
+
+Eigen::Matrix4d constructTransformationMatrixtest (const double & alpha, const double & beta, const double & gamma, const double & tx,    const double & ty,   const double & tz){
+	// Construct the transformation matrix from rotation and translation
+	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Zero ();
+	transformation_matrix (0, 0) =  cos (gamma) * cos (beta);
+	transformation_matrix (0, 1) = -sin (gamma) * cos (alpha) + cos (gamma) * sin (beta) * sin (alpha);
+	transformation_matrix (0, 2) =  sin (gamma) * sin (alpha) + cos (gamma) * sin (beta) * cos (alpha);
+	transformation_matrix (1, 0) =  sin (gamma) * cos (beta);
+	transformation_matrix (1, 1) =  cos (gamma) * cos (alpha) + sin (gamma) * sin (beta) * sin (alpha);
+	transformation_matrix (1, 2) = -cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * cos (alpha);
+	transformation_matrix (2, 0) = -sin (beta);
+	transformation_matrix (2, 1) =  cos (beta) * sin (alpha);
+	transformation_matrix (2, 2) =  cos (beta) * cos (alpha);
+
+	transformation_matrix (0, 3) = tx;
+	transformation_matrix (1, 3) = ty;
+	transformation_matrix (2, 3) = tz;
+	transformation_matrix (3, 3) = 1;
+	return transformation_matrix;
+}
+
+
+Matrix4d align2(Matrix3Xd gt, Matrix3Xd points){
+	Affine3d p = Affine3d::Identity();
+	for(int l = 0; l < 25; l++){
+
+
+		typedef Eigen::Matrix<double, 6, 1> Vector6d;
+		typedef Eigen::Matrix<double, 6, 6> Matrix6d;
+
+		Matrix6d ATA;
+		Vector6d ATb;
+		ATA.setZero ();
+		ATb.setZero ();
+
+		const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3);
+		const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3);
+		const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3);
+
+		double scoreX = 0;
+		double scoreY = 0;
+		double scoreZ = 0;
+		double scoreALL = 0;
+
+		double wsum = 0;
+		double wsx = 0;
+		double wsy = 0;
+		double wsz = 0;
+		for(int i = 0; i < gt.cols(); i++){
+			const double & src_x = gt(0,i);
+			const double & src_y = gt(1,i);
+			const double & src_z = gt(2,i);
+
+			const double & dx = points(0,i);
+			const double & dy = points(1,i);
+			const double & dz = points(2,i);
+
+			const double & sx = m00*src_x + m01*src_y + m02*src_z + m03;
+			const double & sy = m10*src_x + m11*src_y + m12*src_z + m13;
+			const double & sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+			const double diffX = dx-sx;
+			const double diffY = dy-sy;
+			const double diffZ = dz-sz;
+
+			scoreX += diffX*diffX;
+			scoreY += diffY*diffY;
+			scoreZ += diffZ*diffZ;
+			scoreALL += diffX*diffX+diffY*diffY+diffZ*diffZ;
+
+			double weight = 1;
+			wsum += weight;
+
+			wsx += weight * sx;
+			wsy += weight * sy;
+			wsz += weight * sz;
+
+			double wsxsx = weight * sx*sx;
+			double wsysy = weight * sy*sy;
+			double wszsz = weight * sz*sz;
+
+			ATA.coeffRef (0)  += wsysy + wszsz;//a0 * a0;
+			ATA.coeffRef (1)  -= weight * sx*sy;//a0 * a1;
+			ATA.coeffRef (2)  -= weight * sz*sx;//a0 * a2;
+
+
+			ATA.coeffRef (7)  += wsxsx + wszsz;//a1 * a1;
+			ATA.coeffRef (8)  -= weight * sy*sz;//a1 * a2;
+
+			ATA.coeffRef (14) += wsxsx + wsysy;//a2 * a2;
+
+			ATb.coeffRef (0) += weight * (sy*diffZ -sz*diffY);//a0 * b;
+			ATb.coeffRef (1) += weight * (-sx*diffZ + sz*diffX);//a1 * b;
+			ATb.coeffRef (2) += weight * (sx*diffY -sy*diffX);//a2 * b;
+			ATb.coeffRef (3) += weight * diffX;//a3 * b;
+			ATb.coeffRef (4) += weight * diffY;//a4 * b;
+			ATb.coeffRef (5) += weight * diffZ;//a5 * b;
+		}
+
+
+		ATA.coeffRef (4)  -= wsz;//a0 * a4;
+		ATA.coeffRef (9)  += wsz;//a1 * a3;
+
+		ATA.coeffRef (5)  += wsy;//a0 * a5;
+		ATA.coeffRef (15) -= wsy;//a2 * a3;
+
+		ATA.coeffRef (11) -= wsx;//a1 * a5;
+		ATA.coeffRef (16) += wsx;//a2 * a4;
+
+		ATA.coeffRef (21) += wsum;//a3 * a3;
+
+		ATA.coeffRef (28) += wsum;//a4 * a4;
+
+		ATA.coeffRef (35) += wsum;//a5 * a5;
+
+		ATA.coeffRef (6) = ATA.coeff (1);
+		ATA.coeffRef (12) = ATA.coeff (2);
+		ATA.coeffRef (13) = ATA.coeff (8);
+		ATA.coeffRef (18) = ATA.coeff (3);
+		ATA.coeffRef (19) = ATA.coeff (9);
+		ATA.coeffRef (20) = ATA.coeff (15);
+		ATA.coeffRef (24) = ATA.coeff (4);
+		ATA.coeffRef (25) = ATA.coeff (10);
+		ATA.coeffRef (26) = ATA.coeff (16);
+		ATA.coeffRef (27) = ATA.coeff (22);
+		ATA.coeffRef (30) = ATA.coeff (5);
+		ATA.coeffRef (31) = ATA.coeff (11);
+		ATA.coeffRef (32) = ATA.coeff (17);
+		ATA.coeffRef (33) = ATA.coeff (23);
+		ATA.coeffRef (34) = ATA.coeff (29);
+
+		ATA(0,0) += wsum*0.000001;
+		ATA(1,1) += wsum*0.000001;
+		ATA(2,2) += wsum*0.000001;
+		ATA(3,3) += wsum*0.000001;
+		ATA(4,4) += wsum*0.000001;
+		ATA(5,5) += wsum*0.000001;
+
+		//printf("--------------------\n");
+		printf("scoreX:   %5.5f ",scoreX);
+		printf("scoreY:   %5.5f ",scoreY);
+		printf("scoreZ:   %5.5f ",scoreZ);
+		printf("scoreALL: %5.5f\n",scoreALL);
+
+		// Solve A*x = b
+		Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
+
+		Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrixtest(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)));
+		//		std::cout << ATA << std::endl << std::endl;
+		//		std::cout << ATb.transpose() << std::endl << std::endl;
+		//		std::cout << ATA.inverse () << std::endl << std::endl;
+		//		std::cout << x.transpose() << std::endl << std::endl;
+		//		std::cout << transformation.matrix() << std::endl << std::endl;
+		p = transformation*p;
+
+	}
+	return p.matrix().inverse();
+}
+
+int main(int argc, char **argv){
+	int nr_tests = 1;
+	int nr_frames = 100;
+	int points_per_match = 100;
+	double overlap_prob = 0.1;
+	int nr_points = 5000;
+	double noise = 0.01;
+
+	for(int nt = 0; nt < nr_tests; nt++){
+		Matrix3Xd gt = getPoints(nr_points);
+		Matrix3Xd measurements = getMeasurements(gt,noise);
+
+		double angle1 = double(0.1);double angle2 = 0.1;double angle3 = 0.1;
+		double t1 = double(0.1);double t2 = 0;	double t3 = 0;
+
+		//printf("\%\% transformation %i -> angle: %f %f %f translation: %f %f %f\n",i,angle1,angle2,angle3,t1,t2,t3);
+		Matrix3d rotmat;
+		rotmat = AngleAxisd(angle1, Vector3d::UnitZ()) * AngleAxisd(angle2, Vector3d::UnitY()) * AngleAxisd(angle3, Vector3d::UnitZ());
+		Matrix4d transformation = Matrix4d::Identity();
+		transformation.block(0,0,3,3) = rotmat;
+		transformation(0,3) = t1; transformation(1,3) = t2; transformation(2,3) = t3;
+
+		Matrix3Xd measurements_trans = transform_points(measurements, transformation);
+
+		Matrix4d transformation_est1 = align1(gt,measurements_trans);
+		Matrix3Xd measurements_trans_est1 = transform_points(measurements_trans, transformation_est1);
+
+		Matrix4d transformation_est2 = align2(gt,measurements_trans);
+		Matrix3Xd measurements_trans_est2 = transform_points(measurements_trans, transformation_est2);
+
+		double opt_rms = get_reconst_rms(gt, measurements, nr_points)/sqrt(3.0);
+		double rms1 = get_reconst_rms(gt, measurements_trans_est1, nr_points)/sqrt(3.0);
+		double rms2 = get_reconst_rms(gt, measurements_trans_est2, nr_points)/sqrt(3.0);
+		printf("rms1: %12.12f\n",10000.0*(rms1-opt_rms));
+		printf("rms2: %12.12f\n",10000.0*(rms2-opt_rms));
+
+
+	}
+	return 0;
+}
diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp
new file mode 100644
index 00000000..b39c9ed0
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/test_segment.cpp
@@ -0,0 +1,241 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+#include "metaroom_xml_parser/simple_summary_parser.h"
+
+#include <tf_conversions/tf_eigen.h>
+
+#include "ros/ros.h"
+#include <metaroom_xml_parser/load_utilities.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "metaroom_xml_parser/load_utilities.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include <tf_conversions/tf_eigen.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+#include "quasimodo_msgs/segment_model.h"
+
+#include "ros/ros.h"
+#include <quasimodo_msgs/query_cloud.h>
+#include <quasimodo_msgs/visualize_query.h>
+#include <metaroom_xml_parser/load_utilities.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "metaroom_xml_parser/load_utilities.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+#include "Util/Util.h"
+
+
+
+using namespace std;
+
+typedef pcl::PointXYZRGB PointType;
+typedef pcl::PointCloud<PointType> Cloud;
+typedef typename Cloud::Ptr CloudPtr;
+typedef pcl::search::KdTree<PointType> Tree;
+typedef semantic_map_load_utilties::DynamicObjectData<PointType> ObjectData;
+
+using pcl::visualization::PointCloudColorHandlerCustom;
+
+std::vector< std::vector<cv::Mat> > rgbs;
+std::vector< std::vector<cv::Mat> > depths;
+std::vector< std::vector<cv::Mat> > masks;
+std::vector< std::vector<tf::StampedTransform > >tfs;
+std::vector< std::vector<Eigen::Matrix4f> > initposes;
+std::vector< std::vector< image_geometry::PinholeCameraModel > > cams;
+
+pcl::visualization::PCLVisualizer* pg;
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+
+std::vector<reglib::Model * > models;
+
+reglib::Model * load2(std::string sweep_xml){
+	int slash_pos = sweep_xml.find_last_of("/");
+	std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/";
+	printf("folder: %s\n",sweep_folder.c_str());
+
+	SimpleXMLParser<PointType> parser;
+	SimpleXMLParser<PointType>::RoomData roomData  = parser.loadRoomFromXML(sweep_folder+"/room.xml");
+
+//	projection
+//	532.158936 0.000000 310.514310 0.000000
+//	0.000000 533.819214 236.842039 0.000000
+//	0.000000 0.000000 1.000000 0.000000
+
+	reglib::Model * sweepmodel = 0;
+
+	std::vector<reglib::RGBDFrame * > current_room_frames;
+    for (size_t i=0; i < 1 && i<roomData.vIntermediateRoomClouds.size(); i++)
+	{
+
+		cv::Mat fullmask;
+		fullmask.create(480,640,CV_8UC1);
+		unsigned char * maskdata = (unsigned char *)fullmask.data;
+		for(int j = 0; j < 480*640; j++){maskdata[j] = 255;}
+
+		reglib::Camera * cam		= new reglib::Camera();//TODO:: ADD TO CAMERAS
+		cam->fx = 532.158936;
+		cam->fy = 533.819214;
+		cam->cx = 310.514310;
+		cam->cy = 236.842039;
+
+
+		cout<<"Intermediate cloud size "<<roomData.vIntermediateRoomClouds[i]->points.size()<<endl;
+
+		printf("%i / %i\n",i,roomData.vIntermediateRoomClouds.size());
+
+		//Transform
+		tf::StampedTransform tf	= roomData.vIntermediateRoomCloudTransformsRegistered[i];
+		geometry_msgs::TransformStamped tfstmsg;
+		tf::transformStampedTFToMsg (tf, tfstmsg);
+		geometry_msgs::Transform tfmsg = tfstmsg.transform;
+		geometry_msgs::Pose		pose;
+		pose.orientation		= tfmsg.rotation;
+		pose.position.x		= tfmsg.translation.x;
+		pose.position.y		= tfmsg.translation.y;
+		pose.position.z		= tfmsg.translation.z;
+		Eigen::Affine3d epose;
+		tf::poseMsgToEigen(pose, epose);
+
+		reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, epose.matrix());
+
+        current_room_frames.push_back(frame);
+		if(i == 0){
+			sweepmodel = new reglib::Model(frame,fullmask);
+		}else{
+			sweepmodel->frames.push_back(frame);
+			sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose);
+			sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask));
+		}
+	}
+
+	//sweepmodel->recomputeModelPoints();
+	//printf("nr points: %i\n",sweepmodel->points.size());
+
+    models.push_back(sweepmodel);
+	return sweepmodel;
+}
+
+//segment(reglib::Model * bg, std::vector< reglib::Model * > models, bool debugg)
+
+int main(int argc, char** argv){
+
+//    reglib::RegistrationRandom *	reg	= new reglib::RegistrationRandom();
+
+//    viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
+//    viewer->setBackgroundColor (0.5, 0, 0.5);
+//    viewer->addCoordinateSystem (1.0);
+//    viewer->initCameraParameters ();
+
+    for(int ar = 1; ar < argc; ar++){
+        string overall_folder = std::string(argv[ar]);
+        vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointType>(overall_folder);
+        printf("sweep_xmls\n");
+        for (auto sweep_xml : sweep_xmls) {
+            printf("sweep_xml: %s\n",sweep_xml.c_str());
+            load2(sweep_xml);
+        }
+    }
+
+    //Not needed if metaroom well calibrated
+    reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01);
+    bgmassreg->timeout = 20;
+    bgmassreg->viewer = viewer;
+    bgmassreg->use_surface = true;
+    bgmassreg->use_depthedge = false;
+    bgmassreg->visualizationLvl = 0;
+    bgmassreg->maskstep = 4;
+    bgmassreg->nomaskstep = 4;
+    bgmassreg->nomask = true;
+    bgmassreg->stopval = 0.0005;
+    bgmassreg->setData(models.front()->frames,models.front()->modelmasks);
+    reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes);
+    delete bgmassreg;
+
+    for(unsigned int i = 0; i < models.size(); i++){
+        models[i]->relativeposes = bgmfr.poses;
+    }
+
+
+/*
+	ros::init(argc, argv, "test_segment");
+	ros::NodeHandle n;
+	ros::ServiceClient segmentation_client = n.serviceClient<quasimodo_msgs::segment_model>("segment_model");
+
+
+	for(unsigned int i = 1; i < models.size(); i++){
+		quasimodo_msgs::segment_model sm;
+		sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i]));
+		if(i > 0){sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]);}
+
+		if (segmentation_client.call(sm)){//Build model from frame
+			printf("segmented: %i\n",i);
+		}else{ROS_ERROR("Failed to call service segment_model");}
+	}
+*/
+
+	for(unsigned int i = 1; i < models.size(); i++){
+		std::vector< reglib::Model * > foreground;
+		foreground.push_back(models[i]);
+		std::vector< std::vector< cv::Mat > > internal;
+		std::vector< std::vector< cv::Mat > > external;
+		std::vector< std::vector< cv::Mat > > dynamic;
+
+		std::vector< reglib::Model * > background;
+		background.push_back(models[i-1]);
+
+		quasimodo_brain::segment(background,foreground,internal,external,dynamic,false);
+
+//		quasimodo_msgs::segment_model sm;
+//		sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i]));
+//		if(i > 0){sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]);}
+
+//		if (segmentation_client.call(sm)){//Build model from frame
+//			printf("segmented: %i\n",i);
+//		}else{ROS_ERROR("Failed to call service segment_model");}
+	}
+
+    for(size_t j = 0; j < models.size(); j++){
+        models[j]->fullDelete();
+        delete models[j];
+    }
+
+	//delete reg;
+	printf("done\n");
+	return 0;
+}
diff --git a/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp
new file mode 100644
index 00000000..d335aa96
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp
@@ -0,0 +1,97 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+#include "metaroom_xml_parser/simple_summary_parser.h"
+
+#include <tf_conversions/tf_eigen.h>
+
+#include "ros/ros.h"
+#include <metaroom_xml_parser/load_utilities.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "metaroom_xml_parser/load_utilities.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include <tf_conversions/tf_eigen.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+#include "quasimodo_msgs/segment_model.h"
+#include "quasimodo_msgs/metaroom_pair.h"
+
+#include "ros/ros.h"
+#include <quasimodo_msgs/query_cloud.h>
+#include <quasimodo_msgs/visualize_query.h>
+#include <metaroom_xml_parser/load_utilities.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "metaroom_xml_parser/load_utilities.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+#include "Util/Util.h"
+
+ros::ServiceClient segmentation_client;
+
+
+using namespace std;
+
+
+int main(int argc, char** argv){
+
+
+	vector<string> room_xmls;
+	for(int ar = 1; ar < argc; ar++){
+		string overall_folder = std::string(argv[ar]);
+		vector<string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<pcl::PointXYZRGB>(overall_folder);
+		printf("sweep_xmls\n");
+		for (auto sweep_xml : sweep_xmls) {
+			printf("sweep_xml: %s\n",sweep_xml.c_str());
+			room_xmls.push_back(sweep_xml);
+			//load2(sweep_xml);
+		}
+	}
+
+	ros::init(argc, argv, "test_segmentationserver_metaroom");
+	ros::NodeHandle n;
+	ros::ServiceClient segmentation_client = n.serviceClient<quasimodo_msgs::metaroom_pair>("segment_metaroom");
+	quasimodo_msgs::metaroom_pair sm;
+	sm.request.background = std::string(room_xmls[0]);
+	sm.request.foreground = std::string(room_xmls[1]);
+for(int i = 0; i < 150; i++){
+	if (segmentation_client.call(sm)){
+		//int model_id = mff.response.model_id;
+	}else{ROS_ERROR("Failed to call service segment_model");}
+}
+	return 0;
+}
diff --git a/quasimodo/quasimodo_brain/src/velodyne2.cpp b/quasimodo/quasimodo_brain/src/velodyne2.cpp
index 82b11b2b..eb2df121 100644
--- a/quasimodo/quasimodo_brain/src/velodyne2.cpp
+++ b/quasimodo/quasimodo_brain/src/velodyne2.cpp
@@ -18,63 +18,364 @@
 #include "modelupdater/ModelUpdater.h"
 #include "core/RGBDFrame.h"
 
+
+#include <iostream>
+
+#include <boost/thread/thread.hpp>
+#include <pcl/common/common_headers.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/console/parse.h>
+
 #include <string.h>
 
 using namespace std;
 
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+
 int counterr = 0;
 int counterl = 0;
 std::string path = "./";
 
+//From crossproduct
+void getNormal(float & nx, float & ny, float & nz, float x1, float y1, float z1,float x2, float y2, float z2,float x3, float y3, float z3){
+	float u1 = x1-x2;
+	float u2 = y1-y2;
+	float u3 = z1-z2;
+	//	float unorm = sqrt(u1*u1+u2*u2+u3*u3);
+	//	u1 /= unorm;
+	//	u2 /= unorm;
+	//	u3 /= unorm;
+
+	float v1 = x1-x3;
+	float v2 = y1-y3;
+	float v3 = z1-z3;
+	//	float vnorm = sqrt(v1*v1+v2*v2+v3*v3);
+	//	v1 /= vnorm;
+	//	v2 /= vnorm;
+	//	v3 /= vnorm;
+
+
+	//Corssprod u x v
+	nx = u2*v3-u3*v2;
+	ny = u3*v1-u1*v3;
+	nz = u1*v2-u2*v1;
+
+	float nnorm = sqrt(nx*nx+ny*ny+nz*nz);
+	nx /= nnorm;
+	ny /= nnorm;
+	nz /= nnorm;
+
+	//printf("%f %f %f\n",nx,ny,nz);
+	//Flip direction if point + normal further away than point
+	if(((x1+nx)*(x1+nx) + (y1+ny)*(y1+ny) + (z1+nz)*(z1+nz)) > (x1*x1 + y1*y1 + z1*z1)){
+		nx = -nx;
+		ny = -ny;
+		nz = -nz;
+	}
+
+
+}
+
+double scoren (Eigen::Vector3d & a, Eigen::Vector3d & b){
+	double v = a.dot(b);
+	if(std::isnan(v)){return 0;}
+	return v*v;
+}
+
+unsigned int text_id = 0;
+bool gonext = false;
+void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event,  void* viewer_void) {
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);
+	if (event.getKeySym () == "n" && event.keyDown ()){
+		gonext = true;
+		std::cout << "n was pressed => removing all text" << std::endl;
+	}
+}
+
+void fillNormals(pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, int normaltype = 0){
+	viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);
+	int stepw = 2*16;
+
+	unsigned int nrp = cloud.points.size();
+	for(int i = 0; i < cloud.points.size(); i++){
+		pcl::PointXYZRGBNormal & np = cloud.points[i];
+
+		pcl::PointXYZRGBNormal & right = cloud.points[(i+stepw)%nrp];
+		pcl::PointXYZRGBNormal & left = cloud.points[(nrp+i-stepw)%nrp];
+		pcl::PointXYZRGBNormal up;
+		if(i%16 != 0){up = cloud.points[i-1];}
+
+		pcl::PointXYZRGBNormal down;
+		if(i%16 != 15){down = cloud.points[i+1];}
+
+		Eigen::Vector3d npv (np.x,np.y,np.z);
+		Eigen::Vector3d rv (right.x,right.y,right.z);
+		Eigen::Vector3d uv (up.x,up.y,up.z);
+		Eigen::Vector3d lv (left.x,left.y,left.z);
+		Eigen::Vector3d dv (down.x,down.y,down.z);
+
+		Eigen::Vector3d n1 = (npv-rv).cross(npv-uv);
+		Eigen::Vector3d n2 = (npv-rv).cross(npv-dv);
+		Eigen::Vector3d n3 = (npv-lv).cross(npv-uv);
+		Eigen::Vector3d n4 = (npv-lv).cross(npv-dv);
+
+		n1.normalize();
+		n2.normalize();
+		n3.normalize();
+		n4.normalize();
+
+		double s1 = scoren(n1,n1)+scoren(n1,n2)+scoren(n1,n3)+scoren(n1,n4);
+		double s2 = scoren(n2,n1)+scoren(n2,n2)+scoren(n2,n3)+scoren(n2,n4);
+		double s3 = scoren(n3,n1)+scoren(n3,n2)+scoren(n3,n3)+scoren(n3,n4);
+		double s4 = scoren(n4,n1)+scoren(n4,n2)+scoren(n4,n3)+scoren(n4,n4);
+
+		Eigen::Vector3d n = n1;
+		double score = s1;
+		if(s2 > score){score = s2; n = n2;}
+		if(s3 > score){score = s3; n = n3;}
+		if(s4 > score){score = s4; n = n4;}
+
+		if((npv+n).norm() > npv.norm()){
+			n = -n;
+		}
+
+		np.normal_x = n(0);
+		np.normal_y = n(1);
+		np.normal_z = n(2);
+	}
+}
+
+pcl::PointCloud<pcl::PointXYZRGBNormal> getCloudWithNormals(pcl::PointCloud<pcl::PointXYZRGB> & cloud, int normaltype = 0){
+	pcl::PointCloud<pcl::PointXYZRGBNormal> normalcloud;
+	normalcloud.points.resize(cloud.points.size());
+	for(int i = 0; i < cloud.points.size(); i++){
+		pcl::PointXYZRGB & op = cloud.points[i];
+		pcl::PointXYZRGBNormal & np = normalcloud.points[i];
+		np.r = 0;//op.r;
+		np.g = 255;//op.g;
+		np.b = 0;//op.b;
+		np.x = op.x;
+		np.y = op.y;
+		np.z = op.z;
+	}
+
+	//	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	//	*cloud_ptr = normalcloud;
+	//	viewer->removeAllPointClouds();
+	//	viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_ptr, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_ptr), "cloud");
+	//	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
+	//	//viewer->addPointCloudNormals<pcl::PointXYZRGBNormal> (cloud_ptr, 17, 1.5, "normals");
+	//	viewer->spin();
+	fillNormals(normalcloud,normaltype);
+	return normalcloud;
+}
+
+
+pcl::PointCloud<pcl::PointXYZRGBNormal> getSparsifyCloud(pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, int stepw){
+    pcl::PointCloud<pcl::PointXYZRGBNormal> sparsecloud;
+    sparsecloud.reserve(cloud.points.size()/stepw);
+    for(int i = 0; i < cloud.points.size(); i+= 16*5){//16*(stepw-1)){
+        int starti = i;
+        for(;i < starti+16; i++){
+            sparsecloud.push_back(cloud.points[i]);
+        }
+    }
+    return sparsecloud;
+}
+
+pcl::PointCloud<pcl::PointXYZRGBNormal> getCloudFromParts(pcl::PointCloud<pcl::PointXYZRGB> prev, pcl::PointCloud<pcl::PointXYZRGB> curr, Eigen::Matrix4d motion = Eigen::Matrix4d::Identity()){
+    pcl::PointCloud<pcl::PointXYZRGB> prev_cloud;
+    prev_cloud.resize(prev.points.size()/2);
+    for(int i = 0; i < prev.points.size(); i+=2){
+        prev_cloud.points[i/2] = prev.points[i];
+    }
+
+    pcl::PointCloud<pcl::PointXYZRGB> curr_cloud;
+    curr_cloud.resize(curr.points.size()/2);
+    for(int i = 0; i < prev.points.size(); i+=2){
+        curr_cloud.points[i/2] = curr.points[i];
+    }
+
+
+    pcl::PointCloud<pcl::PointXYZRGB> fullcloud = prev_cloud+curr_cloud;
+
+    Eigen::Matrix4d invmotion = motion.inverse();
+    Eigen::Matrix3d rot = invmotion.block(0,0,3,3);
+    Vector3d ea = rot.eulerAngles(2, 0, 2);
+
+    for(int i = 0; i < fullcloud.points.size(); i+=16){
+        double part = double(i)/double(fullcloud.points.size());
+
+
+        Eigen::Affine3d mat;
+        mat = Eigen::AngleAxisd(part*ea(0), Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(part*ea(1), Eigen::Vector3d::UnitX())*Eigen::AngleAxisd(part*ea(2), Eigen::Vector3d::UnitZ());
+
+        const double & m00 = mat(0,0); const double & m01 = mat(0,1); const double & m02 = mat(0,2); const double & m03 = part*invmotion(0,3);
+        const double & m10 = mat(1,0); const double & m11 = mat(1,1); const double & m12 = mat(1,2); const double & m13 = part*invmotion(1,3);
+        const double & m20 = mat(2,0); const double & m21 = mat(2,1); const double & m22 = mat(2,2); const double & m23 = part*invmotion(2,3);
+
+        for(int j = i; j < i+16; j++){//TODO set zeros to zero still
+            pcl::PointXYZRGB & p = fullcloud.points[j];
+            const double & src_x = p.x;
+            const double & src_y = p.y;
+            const double & src_z = p.z;
+            p.x = m00*src_x + m01*src_y + m02*src_z + m03;
+            p.y = m10*src_x + m11*src_y + m12*src_z + m13;
+            p.z = m20*src_x + m21*src_y + m22*src_z + m23;
+        }
+    }
+
+    pcl::PointCloud<pcl::PointXYZRGBNormal> fullcloudnormals = getCloudWithNormals(fullcloud,0);
+    return fullcloudnormals;
+}
+
+pcl::PointCloud<pcl::PointXYZRGB> prevl;
+std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal> > all_cloudsl;
+std::vector< Eigen::Matrix4d > all_posesl;
 void  cloud_cb_l(const sensor_msgs::PointCloud2ConstPtr& input){
-	ROS_INFO("l pointcloud in %i",counterl);
+    ROS_INFO("l pointcloud in %i",counterl);
+
+    pcl::PointCloud<pcl::PointXYZRGB> tmpcloud;
+    pcl::fromROSMsg (*input, tmpcloud);
+
+    pcl::PointCloud<pcl::PointXYZRGB> cloud;
+    cloud.resize(tmpcloud.points.size()/2);
+    for(int i = 0; i < tmpcloud.points.size(); i+=2){cloud.points[i/2] = tmpcloud.points[i];}
+
+
+    char buf[1024];
+    if(counterl % 2 == 1){
+        pcl::PointCloud<pcl::PointXYZRGBNormal> fullcloudnormals = getCloudFromParts(prevl,cloud, Eigen::Matrix4d::Identity());
+        pcl::PointCloud<pcl::PointXYZRGBNormal> sparsecloudnormals = getSparsifyCloud(fullcloudnormals,6);
+//        all_clouds.push_back(fullcloudnormals);
+//        if(all_poses.size()==0){all_poses.push_back(Eigen::Matrix4d::Identity());
+//        }else if(all_poses.size()==1){all_poses.push_back(all_poses.back());
+//        }else{
+//            all_poses.push_back(all_poses.back()*all_poses[all_poses.size()].inverse()*all_poses.back());
+//        }
+
+//        reglib::MassRegistrationPPR * massreg = new reglib::MassRegistrationPPR(0.1);
+//        massreg->timeout = 60;
+//        massreg->viewer = viewer;
+//        massreg->visualizationLvl = 1;
+
+////        massreg->setData(clouds);
+
+//        massreg->stopval = 0.001;
+//        massreg->steps = 10;
+
+////      reglib::MassFusionResults mfr = massreg->getTransforms(relativeposes);
+//        delete massreg;
+
+//		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+//		*cloud_ptr = fullcloudnormals;
+//		viewer->removeAllPointClouds();
+//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_ptr, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_ptr), "cloud");
+//		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
+//		viewer->addPointCloudNormals<pcl::PointXYZRGBNormal> (cloud_ptr, 17, 0.5, "normals");
+//		viewer->spinOnce();
+
+        sprintf(buf,"%s/normalsfullleft_%.10i.pcd",path.c_str(),counterl/2);
+        pcl::io::savePCDFileBinary (string(buf), fullcloudnormals);
+        sprintf(buf,"%s/normalssparseleft_%.10i.pcd",path.c_str(),counterl/2);
+        pcl::io::savePCDFileBinary (string(buf), sparsecloudnormals);
+    }
+
+    prevl = cloud;
+    counterl++;
 }
 
+pcl::PointCloud<pcl::PointXYZRGB> prevr;
+std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > all_clouds;
+std::vector< Eigen::Matrix4d > all_poses;
+
 void  cloud_cb_r(const sensor_msgs::PointCloud2ConstPtr& input){
-	ROS_INFO("r pointcloud in %i",counterr);
-//	if(counter % 5 == 4){
-//		pcl::PointCloud<pcl::PointXYZRGB> cloud;
-//		pcl::fromROSMsg (*input, cloud);
-//		//addAndUpdate(cloud);
-//		char buf[1024];
-//		sprintf(buf,"%s%.10i.pcd",path.c_str(),counter);
-//		printf("saving: %s\n",buf);
-//		pcl::io::savePCDFileBinary (string(buf), cloud);
-
-//		cv::Mat rgb;
-//		rgb.create(cloud.height,cloud.width,CV_8UC3);
-//		unsigned char * rgbdata = (unsigned char *)rgb.data;
-//		unsigned int nr_data = cloud.height * cloud.width;
-//		for(unsigned int i = 0; i < nr_data; i++){
-//			pcl::PointXYZRGB p = cloud.points[i];
-//			rgbdata[3*i+0]	= p.b;
-//			rgbdata[3*i+1]	= p.g;
-//			rgbdata[3*i+2]	= p.r;
-//		}
-//		cv::namedWindow(	"rgb", cv::WINDOW_AUTOSIZE );
-//		cv::imshow(			"rgb", rgb );
-//		cv::waitKey(30);
-//	}
-//	counter++;
+    ROS_INFO("r pointcloud in %i",counterr);
+
+    pcl::PointCloud<pcl::PointXYZRGB> tmpcloud;
+    pcl::fromROSMsg (*input, tmpcloud);
+
+    pcl::PointCloud<pcl::PointXYZRGB> cloud;
+    cloud.resize(tmpcloud.points.size()/2);
+    for(int i = 0; i < tmpcloud.points.size(); i+=2){cloud.points[i/2] = tmpcloud.points[i];}
+
+
+    char buf[1024];
+    if(counterr % 2 == 1){
+        pcl::PointCloud<pcl::PointXYZRGBNormal> fullcloudnormals = getCloudFromParts(prevr,cloud, Eigen::Matrix4d::Identity());
+        pcl::PointCloud<pcl::PointXYZRGBNormal> sparsecloudnormals = getSparsifyCloud(fullcloudnormals,6);
+
+        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+        *cloud_ptr = fullcloudnormals;
+        all_clouds.push_back(cloud_ptr);
+        if(all_poses.size()<=1){all_poses.push_back(Eigen::Matrix4d::Identity());
+        }else{
+            Eigen::Matrix4d v = all_poses[all_poses.size()-2].inverse()*all_poses.back();
+            all_poses.push_back(v*all_poses.back());
+        }
+        if(all_poses.size()>2){
+
+            std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > ac;
+            ac.push_back(all_clouds.front());
+            ac.push_back(all_clouds.back());
+
+            std::vector< Eigen::Matrix4d > ap;
+            ap.push_back(all_poses.front());
+            ap.push_back(all_poses.back());
+            reglib::MassRegistrationPPR * massreg = new reglib::MassRegistrationPPR(0.1);
+            massreg->timeout = 60;
+            massreg->viewer = viewer;
+            massreg->visualizationLvl = 0;
+            if(all_poses.size()%10 == 0){massreg->visualizationLvl = 1;}
+
+
+            massreg->setData(ac);
+
+            massreg->stopval = 0.001;
+            massreg->steps = 10;
+
+            reglib::MassFusionResults mfr = massreg->getTransforms(ap);
+            all_poses.back() = mfr.poses.back();
+            delete massreg;
+        }
+
+
+
+//		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+//		*cloud_ptr = fullcloudnormals;
+//		viewer->removeAllPointClouds();
+//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_ptr, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_ptr), "cloud");
+//		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
+//		viewer->addPointCloudNormals<pcl::PointXYZRGBNormal> (cloud_ptr, 17, 0.5, "normals");
+//		viewer->spinOnce();
+
+        sprintf(buf,"%s/normalssparseright_%.10i.pcd",path.c_str(),counterr/2);
+        pcl::io::savePCDFileBinary (string(buf), sparsecloudnormals);
+        sprintf(buf,"%s/normalsfullright_%.10i.pcd",path.c_str(),counterr/2);
+        pcl::io::savePCDFileBinary (string(buf), fullcloudnormals);
+    }
+
+    prevr = cloud;
+    counterr++;
 }
 
 int main(int argc, char **argv){
-	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("viewer"));
+	viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("viewer"));
 	viewer->addCoordinateSystem();
 	viewer->setBackgroundColor(0.8,0.8,0.8);
+	viewer->initCameraParameters ();
 
-	string pathr;
-	if(argc < 2){	pathr = "/camera/depth_registered/points";}
-	else{			pathr = string(argv[1]);}
-
-	string pathl;
-	if(argc < 3){	pathl = "/camera/depth_registered/points";}
-	else{			pathl = string(argv[2]);}
+	string pathr = "/velodyne_points_right";
+	string pathl = "/velodyne_points_left";
+	if(argc > 1){	pathr = string(argv[1]);}
+	if(argc > 2){	pathl = string(argv[2]);}
 
-	ros::init(argc, argv, "massreg_velodyne_node");
+	ros::init(argc, argv, "massregVelodyneNode");
 	ros::NodeHandle n;
-	ros::Subscriber subr = n.subscribe (path, 0, cloud_cb_r);
-	ros::Subscriber subl = n.subscribe (path, 0, cloud_cb_l);
+	ros::Subscriber subr = n.subscribe (pathr, 0, cloud_cb_r);
+	ros::Subscriber subl = n.subscribe (pathl, 0, cloud_cb_l);
 	ros::spin();
 
 	return 0;
diff --git a/quasimodo/quasimodo_brain/src/voPCD.cpp b/quasimodo/quasimodo_brain/src/voPCD.cpp
new file mode 100644
index 00000000..08f96e13
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/voPCD.cpp
@@ -0,0 +1,88 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include "std_msgs/String.h"
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+#include <tf_conversions/tf_eigen.h>
+
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+#include "quasimodo_msgs/retrieval_query_result.h"
+#include "quasimodo_msgs/retrieval_query.h"
+#include "quasimodo_msgs/retrieval_result.h"
+//#include "soma2_msgs/SOMA2Object.h"
+
+//#include "soma_manager/SOMA2InsertObjs.h"
+
+//#include "modelupdater/ModelUpdater.h"
+//#include "/home/johane/catkin_ws_dyn/src/quasimodo_models/include/modelupdater/ModelUpdater.h"
+#include "modelupdater/ModelUpdater.h"
+#include "core/RGBDFrame.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include "metaroom_xml_parser/simple_xml_parser.h"
+#include "metaroom_xml_parser/simple_summary_parser.h"
+
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <map>
+
+#include "ModelDatabase/ModelDatabase.h"
+
+#include <thread>
+
+#include <sys/time.h>
+
+#include <sys/types.h>
+#include <dirent.h>
+#include <errno.h>
+#include <vector>
+#include <string>
+#include <iostream>
+
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+
+int main(int argc, char **argv){
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("viewer"));
+	viewer->addCoordinateSystem(0.1);
+	viewer->setBackgroundColor(0.9,0.9,0.9);
+
+	std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > clouds;
+	std::vector<Eigen::Matrix4d> relativeposes;
+	for(int arg = 1; arg < argc; arg++){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+		if (pcl::io::loadPCDFile<pcl::PointXYZRGBNormal> (argv[arg], *cloud) == -1){
+			printf ("Error: Couldn't read file %s\n",argv[arg]);
+			return (-1);
+		}
+		clouds.push_back(cloud);
+		relativeposes.push_back(Eigen::Matrix4d::Identity());
+	}
+
+
+	reglib::MassRegistrationPPR * massreg = new reglib::MassRegistrationPPR(0.1);
+	massreg->timeout = 60;
+	massreg->viewer = viewer;
+	massreg->visualizationLvl = 1;
+
+	massreg->setData(clouds);
+
+    massreg->stopval = 0.00001;
+	massreg->steps = 10;
+
+	reglib::MassFusionResults mfr = massreg->getTransforms(relativeposes);
+
+	return 0;
+}
diff --git a/quasimodo/quasimodo_brain/src/zoomer.cpp b/quasimodo/quasimodo_brain/src/zoomer.cpp
new file mode 100644
index 00000000..bd24051a
--- /dev/null
+++ b/quasimodo/quasimodo_brain/src/zoomer.cpp
@@ -0,0 +1,191 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sensor_msgs/PointCloud2.h>
+#include <string.h>
+
+#include <pcl_ros/point_cloud.h>
+
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
+
+#include <sys/time.h>
+#include <sys/resource.h>
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <signal.h>
+
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include "opencv2/opencv.hpp"
+#include <iostream>
+
+int thickness = 3;
+
+std::vector< cv::Mat > imgs;
+int state = 0;
+
+double corner1_x = 0;
+double corner1_y = 0;
+
+double corner2_x = 0;
+double corner2_y = 0;
+
+double corner3_x = 0;
+double corner3_y = 0;
+
+double corner4_x = 0;
+double corner4_y = 0;
+void CallBackFunc(int event, int x, int y, int flags, void* userdata){
+	if			( event == cv::EVENT_LBUTTONDOWN ){
+		std::cout << "Left button of the mouse is clicked - position (" << x << ", " << y << ")" << endl;
+		if(state == 0){
+			state = 1;
+			corner1_x = x;
+			corner1_y = y;
+		}else if(state == 1){
+			state = 2;
+
+			if(x > corner1_x){
+				corner2_x = x;
+			}else{
+				corner2_x = corner1_x;
+				corner1_x = x;
+			}
+
+			if(y > corner1_y){
+				corner2_y = y;
+			}else{
+				corner2_y = corner1_y;
+				corner1_y = y;
+			}
+		}else if(state == 2){
+			state = 3;
+			corner3_x = x;
+			corner3_y = y;
+		}else if(state == 3){
+			state = 4;
+			corner4_x = x;
+			corner4_y = y;
+		}
+	}else if	( event == cv::EVENT_RBUTTONDOWN ){
+		//std::cout << "Right button of the mouse is clicked - position (" << x << ", " << y << ")" << endl;
+	}else if	( event == cv::EVENT_MBUTTONDOWN ){
+		//std::cout << "Middle button of the mouse is clicked - position (" << x << ", " << y << ")" << endl;
+	}else if	( event == cv::EVENT_MOUSEMOVE ){
+		//std::cout << "Mouse move over the window - position (" << x << ", " << y << ")" << endl;
+	}
+
+	if(state == 5){return;}
+	//printf("state: %i\n",state);
+	if(state >= 1){
+		cv::Mat m = imgs.front().clone();
+
+		if(state >= 2){
+			cv::Rect ROI1(cv::Point(corner1_x,corner1_y),cv::Point(corner2_x,corner2_y));
+			cv::rectangle(m, ROI1 , cv::Scalar(0,0,255),thickness);
+
+			//			cv::line(m, cv::Point(x,		y),				cv::Point(corner1_x,	corner1_y),	cv::Scalar(0,255,0),thickness);
+			//			cv::line(m, cv::Point(x,		y),				cv::Point(corner2_x,	corner2_y),	cv::Scalar(0,255,0),thickness);
+
+			if(state >= 3){
+
+				double ratio = fabs(corner1_x-corner2_x)/fabs(corner1_y-corner2_y);
+				//printf("ratio: %f\n",ratio);
+				double offsets = fabs(std::max(fabs(corner3_x-x)/ratio,fabs(corner3_y-y)));
+				offsets = std::max(offsets,1.0);
+
+
+				double c1x = corner3_x+offsets*ratio;
+				double c1y = corner3_y+offsets;
+				double c2x = corner3_x-offsets*ratio;
+				double c2y = corner3_y-offsets;
+
+				double midx = 0.5*(corner1_x+corner2_x);
+				double midy = 0.5*(corner1_y+corner2_y);
+
+
+
+				cv::Point a = cv::Point(c2x,	c2y);
+				cv::Point b = cv::Point(corner1_x,	corner1_y);
+
+//				if( c2x < std::min(corner1_x,corner2_x )){// && c1y < std::min(corner1_y,corner2_y )){
+//					a.x = c2x;
+//					a.y = c1y;
+//				}
+
+//				if		(c2x < corner1_x){
+//					a.y = cv::Point(c2x,	c1y);
+////					cv::line(m, cv::Point(c2x,	c1y),	cv::Point(corner1_x,	corner1_y),	cv::Scalar(0,255,0),thickness);
+////				}else if(c2x > corner1_x && c2y < corner1_y){
+////					cv::line(m, cv::Point(c2x,	c2y),	cv::Point(corner1_x,	corner1_y),	cv::Scalar(255,0,0),thickness);
+//				}else{
+//					//cv::line(m, cv::Point(c2x,	c2y),	cv::Point(corner1_x,	corner1_y),	cv::Scalar(0,0,255),thickness);
+//					a = cv::Point(c2x,	c2y);
+//				}
+
+
+				//cv::line(m, cv::Point(c2x,	c1y),	cv::Point(corner1_x,	corner1_y),	cv::Scalar(0,255,0),thickness);
+				cv::line(m, a,	b,	cv::Scalar(0,255,0),thickness);
+				cv::line(m, cv::Point(c1x,	c1y),	cv::Point(corner2_x,	corner2_y),	cv::Scalar(0,0,255),thickness);
+
+				cv::Rect ROI2(cv::Point(c1x,c1y),cv::Point(c2x,	c2y));
+				cv::rectangle(m, ROI2 , cv::Scalar(255,0,255),thickness);
+
+				if(state >= 4){
+					state = 5;
+					cv::Mat localimg = imgs.front()(ROI1);
+
+					cv::Mat outimg;
+					outimg.create(ROI2.height, ROI2.width,CV_8UC3);
+					cv::resize(localimg, outimg, outimg.size(),0,0,cv::INTER_AREA);
+
+					outimg.copyTo(m(ROI2));
+					cv::rectangle(m, ROI2 , cv::Scalar(0,0,255),thickness);
+
+					cv::imshow("Image", m);
+					cv::waitKey(0);
+				}
+			}else{
+				cv::line(m, cv::Point(x,		y),				cv::Point(corner1_x,	corner1_y),	cv::Scalar(0,255,0),thickness);
+				cv::line(m, cv::Point(x,		y),				cv::Point(corner2_x,	corner2_y),	cv::Scalar(0,255,0),thickness);
+			}
+		}else{
+			cv::Rect ROI1(cv::Point(corner1_x,corner1_y),cv::Point(x,y));
+			cv::rectangle(m, ROI1 , cv::Scalar(255,0,255),thickness);
+		}
+		cv::imshow("Image", m);
+		cv::waitKey(0);
+	}
+}
+
+int main(int argc, char** argv){
+
+	for(int i = 1; i < argc;i++){
+		printf("input: %s\n",argv[i]);
+		cv::Mat m = cv::imread(argv[i], CV_LOAD_IMAGE_UNCHANGED);
+		if(m.data ){
+			cv::namedWindow( "Image", cv::WINDOW_AUTOSIZE );
+			cv::imshow( "Image", m );
+			cv::waitKey(30);
+			imgs.push_back(m);
+		}
+	}
+
+	if(imgs.size() > 0){
+		cv::namedWindow("Image", 1);
+		cv::setMouseCallback("Image", CallBackFunc, NULL);
+		cv::imshow("Image", imgs.front());
+		cv::waitKey(0);
+	}
+
+	return 0;
+}
diff --git a/quasimodo/quasimodo_conversions/CMakeLists.txt b/quasimodo/quasimodo_conversions/CMakeLists.txt
new file mode 100644
index 00000000..cefc5a75
--- /dev/null
+++ b/quasimodo/quasimodo_conversions/CMakeLists.txt
@@ -0,0 +1,192 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(quasimodo_conversions)
+
+add_definitions(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS roscpp quasimodo_msgs soma_llsd_msgs mongodb_store pcl_ros cv_bridge eigen_conversions tf_conversions)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# Find PCL
+find_package(PCL 1.6 REQUIRED COMPONENTS common io search visualization surface kdtree features surface segmentation octree filter keypoints)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+find_package(OpenCV REQUIRED)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES conversions
+  CATKIN_DEPENDS quasimodo_msgs soma_llsd_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+## Declare a C++ library
+add_library(conversions
+  src/${PROJECT_NAME}/conversions.cpp include/${PROJECT_NAME}/conversions.h
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(conversions ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(quasimodo_conversions_node src/quasimodo_conversions_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(quasimodo_conversions_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(conversions
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS quasimodo_conversions quasimodo_conversions_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_quasimodo_conversions.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h
new file mode 100644
index 00000000..91b5efb0
--- /dev/null
+++ b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h
@@ -0,0 +1,36 @@
+#ifndef QUASIMODO_CONVERSIONS_H
+#define QUASIMODO_CONVERSIONS_H
+
+#include <soma_llsd_msgs/Segment.h>
+#include <soma_llsd_msgs/Observation.h>
+#include <soma_llsd_msgs/Scene.h>
+#include <quasimodo_msgs/model.h>
+#include <quasimodo_msgs/rgbd_frame.h>
+#include <ros/ros.h>
+
+#include <opencv2/opencv.hpp>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+namespace quasimodo_conversions {
+
+
+void convert_to_img_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image);
+void convert_to_depth_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image);
+void convert_to_mask_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image);
+
+void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment);
+void soma_segment_to_model(ros::NodeHandle& n, const soma_llsd_msgs::Segment& segment, quasimodo_msgs::model& model);
+void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observation& obs, quasimodo_msgs::rgbd_frame& frame);
+void frame_to_soma_observation(ros::NodeHandle& n, const quasimodo_msgs::rgbd_frame& frame, soma_llsd_msgs::Observation& obs);
+bool raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv::Mat& depth,
+                              const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud,
+                              const Eigen::Matrix4d& pose, const Eigen::Matrix3d& K,
+                              const std::string& waypoint, const std::string episode_id,
+                              soma_llsd_msgs::Scene& scene);
+bool add_masks_to_soma_segment(ros::NodeHandle& n, std::vector<std::string>& scene_ids, std::vector<cv::Mat>& masks,
+                               std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& poses, soma_llsd_msgs::Segment& segment);
+
+} // namespace quasimodo_conversions
+
+#endif // QUASIMODO_CONVERSIONS_H
diff --git a/quasimodo/quasimodo_conversions/package.xml b/quasimodo/quasimodo_conversions/package.xml
new file mode 100644
index 00000000..afd9c82b
--- /dev/null
+++ b/quasimodo/quasimodo_conversions/package.xml
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<package>
+  <name>quasimodo_conversions</name>
+  <version>0.0.0</version>
+  <description>The quasimodo_conversions package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="nbore@todo.todo">nbore</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/quasimodo_conversions</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>quasimodo_msgs</build_depend>
+  <build_depend>soma_llsd_msgs</build_depend>
+  <run_depend>quasimodo_msgs</run_depend>
+  <run_depend>soma_llsd_msgs</run_depend>
+  
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp
new file mode 100644
index 00000000..7b54b69a
--- /dev/null
+++ b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp
@@ -0,0 +1,266 @@
+#include <quasimodo_conversions/conversions.h>
+#include <soma_llsd/GetScene.h>
+#include <soma_llsd/InsertScene.h>
+#include <soma_llsd/AddObservationsToSegment.h>
+#include <soma_llsd/InsertSegment.h>
+
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <tf_conversions/tf_eigen.h>
+#include <eigen_conversions/eigen_msg.h>
+
+namespace quasimodo_conversions {
+
+void convert_to_img_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image)
+{
+    cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage);
+    cv_pub_ptr->image = cv_image;
+    cv_pub_ptr->encoding = "bgr8";
+    ros_image = *cv_pub_ptr->toImageMsg();
+}
+
+void convert_to_depth_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image)
+{
+    cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage);
+    cv_pub_ptr->image = cv_image;
+    cv_pub_ptr->encoding = "mono16";
+    ros_image = *cv_pub_ptr->toImageMsg();
+}
+
+void convert_to_mask_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image)
+{
+    cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage);
+    cv_pub_ptr->image = cv_image;
+    cv_pub_ptr->encoding = "mono8";
+    ros_image = *cv_pub_ptr->toImageMsg();
+}
+
+void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment)
+{
+    ros::ServiceClient client = n.serviceClient<soma_llsd::InsertScene>("/soma_llsd/insert_scene");
+    ROS_INFO("Waiting for /soma_llsd/insert_scene service...");
+    if (!client.waitForExistence(ros::Duration(1.0))) {
+        ROS_INFO("Failed to get /soma_llsd/insert_scene service!");
+        return;
+    }
+    ROS_INFO("Got /soma_llsd/insert_scene service");
+
+    segment.id = std::to_string(model.model_id);
+
+    size_t counter = 0;
+    for (const quasimodo_msgs::rgbd_frame& frame : model.frames) {
+        soma_llsd::InsertScene scene;
+        scene.request.rgb_img = frame.rgb;
+        scene.request.depth_img = frame.depth;
+        scene.request.camera_info = frame.camera;
+        scene.request.robot_pose = frame.pose; // not good actually but whatevs
+        scene.request.cloud = model.clouds[counter];
+
+        if (!client.call(scene)) {
+            ROS_ERROR("Failed to call service /soma_llsd/insert_scene");
+            return;
+        }
+
+        soma_llsd_msgs::Observation obs;
+        obs.scene_id = scene.response.response.id;
+        obs.camera_cloud = model.clouds[counter];
+        obs.image_mask = model.masks[counter];
+        //frame.camera = // see above
+        //frame.frame_id = // see
+        obs.pose = model.local_poses[counter]; //frame.pose;
+        //obs.pose = model.local_poses[counter];
+        obs.id = std::to_string(frame.frame_id);
+        obs.timestamp = ros::Time::now().nsec;
+        segment.observations.push_back(obs);
+        ++counter;
+    }
+
+}
+
+void soma_segment_to_model(ros::NodeHandle& n, const soma_llsd_msgs::Segment& segment, quasimodo_msgs::model& model)
+{
+    ros::ServiceClient client = n.serviceClient<soma_llsd::GetScene>("/soma_llsd/get_scene");
+    ROS_INFO("Waiting for /soma_llsd/get_scene service...");
+    if (!client.waitForExistence(ros::Duration(1.0))) {
+        ROS_INFO("Failed to get /soma_llsd/get_scene service!");
+        return;
+    }
+    ROS_INFO("Got /soma_llsd/get_scene service");
+
+    // segment.scene_id; // this can be used to fetch the scene from mongodb, from which we can get the camera parameters
+
+    model.model_id = std::stoi(segment.id);
+
+    for (const soma_llsd_msgs::Observation& obs : segment.observations) {
+        soma_llsd::GetScene srv;
+        srv.request.scene_id = obs.scene_id;
+        if (!client.call(srv)) {
+            ROS_ERROR("Failed to call service /soma_llsd/get_scene");
+            return;
+        }
+        model.clouds.push_back(obs.camera_cloud);
+        quasimodo_msgs::rgbd_frame frame;
+        frame.depth = srv.response.response.depth_img;
+        frame.rgb = srv.response.response.rgb_img;
+        model.masks.push_back(obs.image_mask);
+        //frame.camera = // see above
+        frame.frame_id = std::stoi(obs.id);
+        //frame.pose = obs.pose;
+        model.local_poses.push_back(obs.pose);
+        model.frames.push_back(frame);
+        model.local_poses.push_back(obs.pose);
+        //model.global_pose = // could probably get this from scene also?
+        //model.masks
+    }
+}
+
+void frame_to_soma_observation(ros::NodeHandle& n, const quasimodo_msgs::rgbd_frame& frame, soma_llsd_msgs::Observation& obs)
+{
+    ros::ServiceClient client = n.serviceClient<soma_llsd::InsertScene>("/soma_llsd/insert_scene");
+    ROS_INFO("Waiting for /soma_llsd/insert_scene service...");
+    if (!client.waitForExistence(ros::Duration(1.0))) {
+        ROS_INFO("Failed to get /soma_llsd/insert_scene service!");
+        return;
+    }
+    ROS_INFO("Got /soma_llsd/insert_scene service");
+
+    soma_llsd::InsertScene scene;
+    scene.request.rgb_img = frame.rgb;
+    scene.request.depth_img = frame.depth;
+    scene.request.camera_info = frame.camera;
+    scene.request.robot_pose = frame.pose; // not good actually but whatevs
+
+    if (!client.call(scene)) {
+        ROS_ERROR("Failed to call service /soma_llsd/insert_scene");
+        return;
+    }
+
+    obs.scene_id = scene.response.response.id;
+    obs.id = std::to_string(frame.frame_id);
+    obs.pose = frame.pose;
+}
+
+void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observation& obs, quasimodo_msgs::rgbd_frame& frame)
+{
+    ros::ServiceClient client = n.serviceClient<soma_llsd::GetScene>("/soma_llsd/get_scene");
+    ROS_INFO("Waiting for /soma_llsd/get_scene service...");
+    if (!client.waitForExistence(ros::Duration(1.0))) {
+        ROS_INFO("Failed to get /soma_llsd/get_scene service!");
+        return;
+    }
+    ROS_INFO("Got /soma_llsd/get_scene service");
+
+    soma_llsd::GetScene srv;
+    srv.request.scene_id = obs.scene_id;
+    if (!client.call(srv)) {
+        ROS_ERROR("Failed to call service /soma_llsd/get_scene");
+        return;
+    }
+
+    frame.depth = srv.response.response.depth_img;
+    frame.rgb = srv.response.response.rgb_img;
+    //frame.camera = // see above
+    frame.frame_id = std::stoi(obs.id);
+    frame.pose = obs.pose; // not good actually but whatevs
+}
+
+//string episode_id
+//string waypoint
+//string meta_data
+//uint32 timestamp
+
+//tf/tfMessage transform
+//sensor_msgs/PointCloud2 cloud
+//sensor_msgs/Image rgb_img
+//sensor_msgs/Image depth_img
+//sensor_msgs/CameraInfo camera_info
+//geometry_msgs/Pose robot_pose
+//---
+//bool result
+//soma_llsd_msgs/Scene response
+
+// ### response has id field! ###
+
+bool raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv::Mat& depth,
+                              const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud,
+                              const Eigen::Matrix4d& pose, const Eigen::Matrix3d& K,
+                              const std::string& waypoint, const std::string episode_id,
+                              soma_llsd_msgs::Scene& scene)
+{
+    ros::ServiceClient client = n.serviceClient<soma_llsd::InsertScene>("/soma_llsd/insert_scene");
+    ROS_INFO("Waiting for /soma_llsd/insert_scene service...");
+    if (!client.waitForExistence(ros::Duration(1.0))) {
+        ROS_INFO("Failed to get /soma_llsd/insert_scene service!");
+		return false;
+    }
+    ROS_INFO("Got /soma_llsd/insert_scene service");
+
+    soma_llsd::InsertScene scene_srv;
+
+    convert_to_img_msg(rgb, scene_srv.request.rgb_img);
+    convert_to_depth_msg(depth, scene_srv.request.depth_img);
+	//pcl::toROSMsg(*cloud, scene_srv.request.cloud);
+    tf::poseEigenToMsg(Eigen::Affine3d(pose), scene_srv.request.robot_pose);
+    sensor_msgs::CameraInfo info;
+    info.K.at(0) = K(0, 0);
+    info.K.at(1) = K(0, 1);
+    info.K.at(2) = K(0, 2);
+    info.K.at(3) = K(1, 0);
+    info.K.at(4) = K(1, 1);
+    info.K.at(5) = K(1, 2);
+    info.K.at(8) = K(2, 2);
+
+    info.P.at(0) = K(0, 0);
+    info.P.at(1) = K(0, 1);
+    info.P.at(2) = K(0, 2);
+    info.P.at(4) = K(1, 0);
+    info.P.at(5) = K(1, 1);
+    info.P.at(6) = K(1, 2);
+    info.P.at(10) = K(2, 2);
+    info.height = rgb.rows;
+    info.width = rgb.cols;
+    scene_srv.request.camera_info = info;
+    scene_srv.request.waypoint = waypoint;
+    scene_srv.request.episode_id = episode_id;
+
+	if (!client.call(scene_srv) || !scene_srv.response.result) {
+        ROS_ERROR("Failed to call service /soma_llsd/insert_scene");
+		return false;
+    }
+
+    scene = scene_srv.response.response;
+	return true;
+}
+
+bool add_masks_to_soma_segment(ros::NodeHandle& n, std::vector<std::string>& scene_ids, std::vector<cv::Mat>& masks,
+                               std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& poses, soma_llsd_msgs::Segment& segment)
+{
+    ros::ServiceClient client = n.serviceClient<soma_llsd::InsertSegment>("/soma_llsd/insert_segment");
+    ROS_INFO("Waiting for /soma_llsd/insert_segment service...");
+    if (!client.waitForExistence(ros::Duration(1.0))) {
+        ROS_INFO("Failed to get /soma_llsd/insert_segment service!");
+		return false;
+    }
+    ROS_INFO("Got /soma_llsd/insert_segment service");
+
+    soma_llsd::InsertSegment segment_srv;
+
+    size_t N = masks.size();
+    segment_srv.request.observations.resize(N);
+    for (size_t i = 0; i < N; ++i) {
+        convert_to_mask_msg(masks[i], segment_srv.request.observations[i].image_mask);
+        tf::poseEigenToMsg(Eigen::Affine3d(poses[i]), segment_srv.request.observations[i].pose);
+        segment_srv.request.observations[i].scene_id = scene_ids[i];
+    }
+
+	if (!client.call(segment_srv) || !segment_srv.response.result) {
+        ROS_ERROR("Failed to call service /soma_llsd/insert_segment");
+		return false;
+    }
+
+    segment = segment_srv.response.response;
+	return true;
+}
+
+} // namespace quasimodo_conversions
diff --git a/quasimodo/quasimodo_launch/launch/quasimodo.launch b/quasimodo/quasimodo_launch/launch/quasimodo.launch
index 7abcc149..ee54d00f 100644
--- a/quasimodo/quasimodo_launch/launch/quasimodo.launch
+++ b/quasimodo/quasimodo_launch/launch/quasimodo.launch
@@ -1,6 +1,7 @@
 <launch>
 
-    <arg name="data_path" default="/home/strands/.semanticMap"/>
+<!--    <arg name="data_path" default="/home/strands/.semanticMap"/> -->
+	<arg name="data_path" default="/media/johane/SSDstorage/final_tsc_semantic_maps//semantic_maps/"/>
     <arg name="vocabulary_path" default="$(arg data_path)/vocabulary"/>
     <arg name="occlusion_penalty" default="5"/>
     <arg name="massreg_timeout" default="60"/>
@@ -10,6 +11,7 @@
 
     <arg name="topicname" default="/some/topic"/>
 
+    <!--
     <include file="$(find retrieval_processing)/launch/processing.launch">
         <arg name="data_path" value="$(arg data_path)"/>
         <arg name="add_previous_maps" value="$(arg add_previous_maps)"/>
@@ -18,7 +20,14 @@
     <include file="$(find quasimodo_retrieval)/launch/retrieval.launch">
         <arg name="vocabulary_path" value="$(arg vocabulary_path)"/>
     </include>
-
+    -->
+    
+    <include file="$(find quasimodo_object_search)/launch/object_search.launch">
+        <arg name="data_path" value="$(arg data_path)"/>
+        <arg name="enable_incremental" value="true"/>
+    </include>
+    
+<!--
     <include file="$(find quasimodo_brain)/launch/modelserver.launch">
         <arg name="occlusion_penalty" value="$(arg occlusion_penalty)"/>
         <arg name="massreg_timeout" value="$(arg massreg_timeout)"/>
@@ -29,5 +38,5 @@
     <include file="$(find quasimodo_brain)/launch/robot_listener.launch">
         <arg name="topicname" value="$(arg topicname)"/>
     </include>
-
+-->
 </launch>
diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt
index 3d16a0b5..a231d30f 100644
--- a/quasimodo/quasimodo_models/CMakeLists.txt
+++ b/quasimodo/quasimodo_models/CMakeLists.txt
@@ -2,13 +2,14 @@ cmake_minimum_required(VERSION 2.8.3)
 project(quasimodo_models)
 
 #find_package(catkin REQUIRED)# quasimodo_msgs)# COMPONENTS quasimodo_msgs)
-set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fPIC -std=c++0x -o popcnt -mssse3")
-
+#set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3")
+set(CMAKE_CXX_FLAGS "-O2 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3")
 
 find_package(catkin REQUIRED COMPONENTS
 	#metaroom_xml_parser
 	#semantic_map
 	pcl_ros
+	quasimodo_conversions
 )
 
 
@@ -52,25 +53,40 @@ include_directories(/usr/local/include/)
 FIND_PACKAGE(Ceres REQUIRED)
 INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 
+add_library(quasimodo_SignalProcessing src/weightfunctions/SignalProcessing.cpp)
+target_link_libraries(quasimodo_SignalProcessing ${catkin_LIBRARIES})
+
+add_library(quasimodo_distribution src/weightfunctions/Distribution.cpp src/weightfunctions/GaussianDistribution.cpp src/weightfunctions/GeneralizedGaussianDistribution.cpp)
+target_link_libraries(quasimodo_distribution ceres glog ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp src/weightfunctions/DistanceWeightFunction2PPR.cpp src/weightfunctions/DistanceWeightFunction2PPR2.cpp src/weightfunctions/DistanceWeightFunction2PPR3.cpp)
+target_link_libraries(quasimodo_weightlib quasimodo_SignalProcessing quasimodo_distribution ceres glog ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+add_library(quasimodo_superpoint src/core/superpoint.cpp)
+target_link_libraries(quasimodo_superpoint ${catkin_LIBRARIES})
+
+add_library(quasimodo_ReprojectionResult src/core/ReprojectionResult.cpp)
+target_link_libraries(quasimodo_ReprojectionResult quasimodo_superpoint ${catkin_LIBRARIES})
+
+add_library(quasimodo_OcclusionScore src/core/OcclusionScore.cpp)
+target_link_libraries(quasimodo_OcclusionScore ${catkin_LIBRARIES})
 
-add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp src/weightfunctions/DistanceWeightFunction2PPR.cpp src/weightfunctions/DistanceWeightFunction2PPR2.cpp)
-target_link_libraries(quasimodo_weightlib ceres glog ${catkin_LIBRARIES})
 
 add_library(quasimodo_core src/core/Util.cpp src/core/Camera.cpp src/core/RGBDFrame.cpp)
-target_link_libraries(quasimodo_core ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})
+target_link_libraries(quasimodo_core quasimodo_OcclusionScore quasimodo_ReprojectionResult quasimodo_superpoint quasimodo_weightlib ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})
 
 add_library(quasimodo_reglib src/registration/Registration.cpp src/registration/RegistrationRandom.cpp src/registration/RegistrationRefinement.cpp)
 target_link_libraries(quasimodo_reglib quasimodo_weightlib ceres glog ${catkin_LIBRARIES})
 
-add_library(quasimodo_massreglib src/registration/MassRegistration.cpp src/registration/MassRegistrationPPR.cpp)
-target_link_libraries(quasimodo_massreglib quasimodo_reglib quasimodo_core)
-
 add_library(quasimodo_modelmask src/model/ModelMask.cpp)
 target_link_libraries(quasimodo_modelmask quasimodo_core quasimodo_reglib ${catkin_LIBRARIES})
 
 add_library(quasimodo_model src/model/Model.cpp)
 target_link_libraries(quasimodo_model quasimodo_modelmask quasimodo_core quasimodo_reglib ${catkin_LIBRARIES})
 
+add_library(quasimodo_massreglib src/registration/MassRegistration.cpp src/registration/MassRegistrationPPR.cpp src/registration/MassRegistrationPPR2.cpp)
+target_link_libraries(quasimodo_massreglib quasimodo_model quasimodo_reglib quasimodo_core)
+
 add_library(quasimodo_Mesher src/mesher/Mesh.cpp)
 target_link_libraries(quasimodo_Mesher quasimodo_model ${catkin_LIBRARIES})
 
@@ -89,12 +105,9 @@ target_link_libraries(quasimodo_ModelUpdater quasimodo_Mesher quasimodo_model qu
 
 
 # Mark executables and/or libraries for installation
-install(TARGETS quasimodo_weightlib quasimodo_core quasimodo_reglib quasimodo_massreglib quasimodo_modelmask quasimodo_model quasimodo_Mesher quasimodo_ModelUpdater
+install(TARGETS quasimodo_SignalProcessing quasimodo_distribution quasimodo_weightlib quasimodo_superpoint quasimodo_ReprojectionResult quasimodo_OcclusionScore quasimodo_core quasimodo_reglib quasimodo_massreglib quasimodo_modelmask quasimodo_model quasimodo_Mesher quasimodo_ModelUpdater
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
diff --git a/quasimodo/quasimodo_models/include/core/Camera.h b/quasimodo/quasimodo_models/include/core/Camera.h
index a35ea6dd..011e78d0 100644
--- a/quasimodo/quasimodo_models/include/core/Camera.h
+++ b/quasimodo/quasimodo_models/include/core/Camera.h
@@ -37,6 +37,7 @@ namespace reglib
 		void save(std::string path = "");
 		void print();
 		static Camera * load(std::string path);
+		Camera * clone();
 	};
 
 }
diff --git a/quasimodo/quasimodo_models/include/core/OcclusionScore.h b/quasimodo/quasimodo_models/include/core/OcclusionScore.h
new file mode 100644
index 00000000..7109406d
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/core/OcclusionScore.h
@@ -0,0 +1,31 @@
+#ifndef reglibOcclusionScore_H
+#define reglibOcclusionScore_H
+
+#include <iostream>
+#include <vector>
+#include <stdio.h>
+#include <stdlib.h>
+#include <iostream>
+#include <unordered_map>
+
+#include "OcclusionScore.h"
+
+namespace reglib
+{
+
+class OcclusionScore{
+	public:
+	double score;
+	double occlusions;
+
+	OcclusionScore();
+	OcclusionScore(	double score_ ,double occlusions_);
+	~OcclusionScore();
+
+	void add(OcclusionScore oc);
+	void print();
+};
+
+}
+
+#endif
diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h
index 405ea371..4564d923 100644
--- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h
+++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h
@@ -16,23 +16,38 @@
 
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <opencv2/features2d/features2d.hpp>
+
+#include "superpoint.h"
 #include "Util.h"
 #include "Camera.h"
+#include "../weightfunctions/DistanceWeightFunction2.h"
 
 namespace reglib
 {
 	class RGBDFrame{
 		public:
+		static int saveId;
+
+		std::string keyval;
 		Camera * camera;
 		unsigned long id;
 		double capturetime;
 		Eigen::Matrix4d pose;
 		int sweepid;
+		std::string soma_id;
 
+		//float * rgbdata;
+		cv::Mat det_dilate;
 		cv::Mat rgb;
 		cv::Mat depth;
 		cv::Mat normals;
 		cv::Mat depthedges;
+
+		cv::Mat de;
+		cv::Mat ce;
 		int * labels;
 		int nr_labels;
 
@@ -40,17 +55,26 @@ namespace reglib
 		std::vector< std::vector<double> > intersections;
 
 		RGBDFrame();
-		RGBDFrame(Camera * camera_,cv::Mat rgb_, cv::Mat depth_, double capturetime_ = 0, Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(), bool compute_normals = true);
+		RGBDFrame(Camera * camera_,cv::Mat rgb_, cv::Mat depth_, double capturetime_ = 0, Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(), bool compute_normals = true, std::string savePath = "", bool compute_imgedges = true);
 		~RGBDFrame();
 
 		void show(bool stop = false);
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr getPCLcloud();
-        void savePCD(std::string path = "cloud.pcd");
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr getSmallPCLcloud();
+        void savePCD(std::string path = "cloud.pcd", Eigen::Matrix4d pose = Eigen::Matrix4d::Identity());
 
         void save(std::string path = "");
+		RGBDFrame * clone();
 		static RGBDFrame * load(Camera * cam, std::string path);
-	};
+		std::vector<ReprojectionResult> getReprojections(std::vector<superpoint> & spvec, Eigen::Matrix4d cp, bool * maskvec,  bool useDet = true);
+
+		std::vector<superpoint> getSuperPoints(Eigen::Matrix4d cp = Eigen::Matrix4d::Identity(), unsigned int step = 1, bool zeroinclude = true);
 
+		std::vector< std::vector<float> > getImageProbs(bool depthonly = false);
+
+        void saveCombinedImages(std::string path);
+        void saveCombinedProcessedImages(std::string path);
+	};
 }
 
 #endif // reglibRGBDFrame_H
diff --git a/quasimodo/quasimodo_models/include/core/ReprojectionResult.h b/quasimodo/quasimodo_models/include/core/ReprojectionResult.h
new file mode 100644
index 00000000..95dfd91a
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/core/ReprojectionResult.h
@@ -0,0 +1,30 @@
+#ifndef reglibReprojectionResult_H
+#define reglibReprojectionResult_H
+
+#include <Eigen/Dense>
+
+
+namespace reglib
+{
+class ReprojectionResult{
+	public:
+	unsigned long	src_ind;
+	unsigned long	dst_ind;
+	double			angle;
+	double			residualZ;
+	double			residualE2;
+	double			residualR;
+	double			residualG;
+	double			residualB;
+
+	double noiseZ;
+	double noiseRGB;
+
+	ReprojectionResult();
+	ReprojectionResult(unsigned long si, unsigned long di, double rz, double rE2, double a, double rr, double rg, double rb, double nz, double nrgb);
+	~ReprojectionResult();
+	void print();
+};
+}
+
+#endif
diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h
index 066d7803..f63fb10d 100644
--- a/quasimodo/quasimodo_models/include/core/Util.h
+++ b/quasimodo/quasimodo_models/include/core/Util.h
@@ -20,6 +20,28 @@
 #include "ceres/rotation.h"
 #include "ceres/iteration_callback.h"
 
+#include <boost/graph/incremental_components.hpp>
+#include <boost/graph/graphviz.hpp>
+#include <boost/graph/graph_utility.hpp>
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/iteration_macros.hpp>
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/one_bit_color_map.hpp>
+#include <boost/graph/stoer_wagner_min_cut.hpp>
+#include <boost/range/adaptor/reversed.hpp>
+#include <boost/graph/copy.hpp>
+
+#include <iostream>
+#include <unordered_map>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "superpoint.h"
+#include "ReprojectionResult.h"
+#include "OcclusionScore.h"
+#include "quasimodo_point.h"
+
 using ceres::NumericDiffCostFunction;
 using ceres::SizedCostFunction;
 using ceres::CENTRAL;
@@ -30,62 +52,102 @@ using ceres::Solver;
 using ceres::Solve;
 using ceres::Solve;
 
+typedef boost::property<boost::edge_weight_t, float> edge_weight_property;
+typedef boost::property<boost::vertex_name_t, size_t> vertex_name_property;
+using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, vertex_name_property, edge_weight_property>;
+using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
+using VertexIndex = boost::graph_traits<Graph>::vertices_size_type;
+using Edge = boost::graph_traits<Graph>::edge_descriptor;
+using Components = boost::component_index<VertexIndex>;
+
 namespace reglib
 {
 
-//namespace nanoflann {
-//	/// KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
-//	/// This code is adapted from the KDTreeEigenMatrixAdaptor class of nanoflann.hpp
-//	template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = int>
-//	struct KDTreeAdaptor {
-//		typedef KDTreeAdaptor<MatrixType,DIM,Distance> self_t;
-//		typedef typename MatrixType::Scalar              num_t;
-//		typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
-//		typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType>  index_t;
-//		index_t* index;
-//		KDTreeAdaptor(const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat) {
-//			const size_t dims = mat.rows();
-//			index = new index_t( dims, *this, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
-//			index->buildIndex();
-//		}
-//		~KDTreeAdaptor() {delete index;}
-//		const MatrixType &m_data_matrix;
-//		/// Query for the num_closest closest points to a given point (entered as query_point[0:dim-1]).
-//		inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq) const {
-//			nanoflann::KNNResultSet<typename MatrixType::Scalar,IndexType> resultSet(num_closest);
-//			resultSet.init(out_indices, out_distances_sq);
-//			index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
-//		}
-//		/// Query for the closest points to a given point (entered as query_point[0:dim-1]).
-//		inline IndexType closest(const num_t *query_point) const {
-//			IndexType out_indices;
-//			num_t out_distances_sq;
-//			query(query_point, 1, &out_indices, &out_distances_sq);
-//			return out_indices;
-//		}
-//		const self_t & derived() const {return *this;}
-//		self_t & derived() {return *this;}
-//		inline size_t kdtree_get_point_count() const {return m_data_matrix.cols();}
-//		/// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
-//		inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const {
-//			num_t s=0;
-//			for (size_t i=0; i<size; i++) {
-//				const num_t d= p1[i]-m_data_matrix.coeff(i,idx_p2);
-//				s+=d*d;
-//			}
-//			return s;
-//		}
-//		/// Returns the dim'th component of the idx'th point in the class:
-//		inline num_t kdtree_get_pt(const size_t idx, int dim) const {
-//			return m_data_matrix.coeff(dim,idx);
-//		}
-//		/// Optional bounding-box computation: return false to default to a standard bbox computation loop.
-//		template <class BBOX> bool kdtree_get_bbox(BBOX&) const {return false;}
-//	};
-//}
-
-// This is an exampleof a custom data set class
+void pn(double p, unsigned int len = 3);
+
+
+
+float graph_cut(std::vector<Graph*>& graphs_out,std::vector<std::vector<int>>& second_graphinds, Graph& graph_in, std::vector<int> graph_inds);
+
+float recursive_split(std::vector<Graph*> * graphs_out,std::vector<std::vector<int>> * graphinds_out, Graph * graph, std::vector<int> graph_inds);
+
+std::vector<int> partition_graph(std::vector< std::vector< float > > & scores);
+
+inline double getNoise(double depth){return depth*depth;}
+
+inline double getInformation(double depth){
+	//if(depth == 0 || depth > 4){return 0.00000000001;}
+	double n = getNoise(depth);
+	return 1.0/(n*n);
+}
+
+inline double getNoise(double n1, double n2){
+	double info1 = 1.0/(n1*n1);
+	double info2 = 1.0/(n2*n2);
+	double info3 = info1+info2;
+	return sqrt(1.0/info3);
+}
+
+inline double getNoiseFromInformation(double info1, double info2){
+	double info3 = info1+info2;
+	return sqrt(1.0/info3);
+}
+
+inline double getNoiseFromInformation(double info){
+	double cov = 1/info;
+	return sqrt(cov);
+}
+
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr getPointCloudFromVector(std::vector<superpoint> & spvec, int colortype = 0, int r = -1, int g = -1, int b = -1);
+
+
+/**
+ * @brief Returns homogenous 4x4 transformation matrix for given rotation (quaternion) and translation components
+ * @param q rotation represented as quaternion
+ * @param trans homogenous translation
+ * @return tf 4x4 homogeneous transformation matrix
+ *
+ */
+inline Eigen::Matrix4f
+RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector4f &trans)
+{
+    Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();;
+    tf.block<3,3>(0,0) = q.toRotationMatrix();
+    tf.block<4,1>(0,3) = trans;
+    tf(3,3) = 1.f;
+    return tf;
+}
+
 
+/**
+ * @brief Returns homogenous 4x4 transformation matrix for given rotation (quaternion) and translation components
+ * @param q rotation represented as quaternion
+ * @param trans translation
+ * @return tf 4x4 homogeneous transformation matrix
+ *
+ */
+inline Eigen::Matrix4f
+RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector3f &trans)
+{
+    Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
+    tf.block<3,3>(0,0) = q.toRotationMatrix();
+    tf.block<3,1>(0,3) = trans;
+    return tf;
+}
+
+/**
+ * @brief Returns rotation (quaternion) and translation components from a homogenous 4x4 transformation matrix
+ * @param tf 4x4 homogeneous transformation matrix
+ * @param q rotation represented as quaternion
+ * @param trans homogenous translation
+ */
+inline void
+Mat4f2RotTrans(const Eigen::Matrix4f &tf, Eigen::Quaternionf &q, Eigen::Vector4f &trans)
+{
+    Eigen::Matrix3f rotation = tf.block<3,3>(0,0);
+    q = rotation;
+    trans = tf.block<4,1>(0,3);
+}
 
 template <typename T> struct ArrayData3D {
 	int rows;
@@ -138,6 +200,8 @@ template <typename T> struct ArrayData3D {
 	//typedef nanoflann2::KDTreeEigenMatrixAdaptor< Eigen::Matrix<double,-1,-1>, SAMPLES_DIM,nanoflann2::metric_L2_Simple> KDTreed;
 	//typedef nanoflann2::KDTreeEigenMatrixAdaptor< Eigen::Matrix<float,-1,-1>, SAMPLES_DIM,nanoflann2::metric_L2_Simple> KDTreef;
 	double getTime();
+	double mysign(double v);
+
 
 
 	struct PointCostFunctor {
@@ -230,7 +294,7 @@ template <typename T> struct ArrayData3D {
 								Eigen::Matrix<double, 3, Eigen::Dynamic> & Y,
 								Eigen::Matrix<double, 3, Eigen::Dynamic> & Yn,
 								Eigen::VectorXd & W);
-	bool isconverged(std::vector<Eigen::Matrix4d> before, std::vector<Eigen::Matrix4d> after, double stopvalr = 0.001, double stopvalt = 0.001);
+    bool isconverged(std::vector<Eigen::Matrix4d> before, std::vector<Eigen::Matrix4d> after, double stopvalr = 0.001, double stopvalt = 0.001, bool verbose = false);
 	void setFirstIdentity(std::vector<Eigen::Matrix4d> & v);
 }
 
@@ -273,6 +337,8 @@ struct PointCloud
 	template <class BBOX>
 	bool kdtree_get_bbox(BBOX& /* bb */) const { return false; }
 };
+
+
 }
 
 typedef nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Simple_Adaptor<double, reglib::ArrayData3D<double> > , reglib::ArrayData3D<double>,3> Tree3d;
diff --git a/quasimodo/quasimodo_models/include/core/quasimodo_point.h b/quasimodo/quasimodo_models/include/core/quasimodo_point.h
new file mode 100644
index 00000000..0fb49fef
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/core/quasimodo_point.h
@@ -0,0 +1,42 @@
+#ifndef __QUASIMODO_POINT_TYPE__H
+#define __QUASIMODO_POINT_TYPE__H
+
+#define PCL_NO_PRECOMPILE
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+
+struct QuasimodoPointType
+{
+	//PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
+	//PCL_ADD_RGB;//Add color to the point
+	PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4])
+	union
+	{
+		struct
+		{
+			float ce_x;
+			float ce_y;
+			float de_x;
+			float de_y;
+		};
+		float data_t[4];
+	};
+
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
+} EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (QuasimodoPointType,
+//								   (float, x, x)
+//								   (float, y, y)
+//								   (float, z, z)
+								   (float, normal_x, normal_x)
+								   (float, normal_y, normal_y)
+								   (float, normal_z, normal_z)
+								   //(uint32_t, rgba, rgba)
+								   (float, ce_x, ce_x)
+								   (float, ce_y, ce_y)
+								   (float, de_x, de_x)
+								   (float, de_y, de_y)
+								   )
+#endif
diff --git a/quasimodo/quasimodo_models/include/core/superpoint.h b/quasimodo/quasimodo_models/include/core/superpoint.h
new file mode 100644
index 00000000..ec0fec85
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/core/superpoint.h
@@ -0,0 +1,25 @@
+#ifndef reglibsuperpoint_H
+#define reglibsuperpoint_H
+
+#include <Eigen/Dense>
+
+
+namespace reglib
+{
+class superpoint{
+public:
+	Eigen::Vector3f point;
+	Eigen::Vector3f normal;
+	Eigen::VectorXf feature;
+	double point_information;
+	double normal_information;
+	double feature_information;
+	int last_update_frame_id;
+
+	superpoint(Eigen::Vector3f p = Eigen::Vector3f(0,0,0), Eigen::Vector3f n = Eigen::Vector3f(0,0,0), Eigen::VectorXf f = Eigen::VectorXf(3), double pi = 1, double fi = 1, int id = 0);
+	~superpoint();
+	void merge(superpoint p, double weight = 1);
+};
+}
+
+#endif
diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h
index 21a90dae..b795085b 100644
--- a/quasimodo/quasimodo_models/include/model/Model.h
+++ b/quasimodo/quasimodo_models/include/model/Model.h
@@ -20,41 +20,9 @@
 
 namespace reglib
 {
+using namespace std;
+using namespace Eigen;
 
-class superpoint{
-	public:
-	Eigen::Vector3f point;
-	Eigen::Vector3f normal;
-	Eigen::VectorXf feature;
-	double point_information;
-	double feature_information;
-	int last_update_frame_id;
-
-	superpoint(Eigen::Vector3f p, Eigen::Vector3f n, Eigen::VectorXf f, double pi = 1, double fi = 1, int id = 0){
-		point = p;
-		normal = n;
-		feature = f;
-		point_information = pi;
-		feature_information = fi;
-		last_update_frame_id = id;
-	}
-
-	~superpoint(){}
-
-	void merge(superpoint p, double weight = 1){
-		point = weight*p.point_information*p.point + point_information*point;
-		point /= weight*p.point_information + point_information;
-		point_information = weight*p.point_information + point_information;
-
-		normal = weight*p.point_information*p.normal + point_information*normal;
-		normal.normalize();
-
-		feature = weight*p.feature_information*p.feature + feature_information*feature;
-		feature /= weight*p.feature_information + feature_information;
-		feature_information = weight*p.feature_information + feature_information;
-		last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id);
-	}
-};
 
 	class Model{
 		public:
@@ -64,23 +32,45 @@ class superpoint{
 
 		int last_changed;
 
+		std::string savePath;
+		std::string soma_id;
+
+		std::string pointspath;
 		std::vector<superpoint> points;
 
-		std::vector<Eigen::Matrix4d> relativeposes;
-		std::vector<RGBDFrame*> frames;
-		//std::vector<cv::Mat> masks;
-		std::vector<ModelMask*> modelmasks;
+		std::vector< std::vector<cv::KeyPoint> >	all_keypoints;
+		std::vector< cv::Mat >						all_descriptors;
+		std::vector<Eigen::Matrix4d>				relativeposes;
+		std::vector<RGBDFrame*>						frames;
+		std::vector<ModelMask*>						modelmasks;
+
+		std::vector<Eigen::Matrix4d>	rep_relativeposes;
+		std::vector<RGBDFrame*>			rep_frames;
+		std::vector<ModelMask*>			rep_modelmasks;
 
 		double total_scores;
 		std::vector<std::vector < float > > scores;
 
+		std::vector<Model *>				submodels;
+		std::vector<Eigen::Matrix4d>		submodels_relativeposes;
+		std::vector<std::vector < float > > submodels_scores;
+		Model * parrent;
+
 		Model();
 		Model(RGBDFrame * frame_, cv::Mat mask, Eigen::Matrix4d pose = Eigen::Matrix4d::Identity());
 		~Model();
+
+		void fullDelete();
 		
 		void merge(Model * model, Eigen::Matrix4d p);
 
-		void recomputeModelPoints();
+		void showHistory(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer);
+
+		std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> getHistory();
+
+		void addSuperPoints(	vector<superpoint> & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = 0);
+		void addAllSuperPoints(	vector<superpoint> & spvec, Eigen::Matrix4d pose,								boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = 0);
+		void recomputeModelPoints(Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(),							boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = 0);
 		void addPointsToModel(RGBDFrame * frame, ModelMask * modelmask, Eigen::Matrix4d p);
 
 		//void addFrameToModel(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d p);
@@ -92,6 +82,10 @@ class superpoint{
 		void save(std::string path = "");
 		static Model * load(Camera * cam, std::string path);
 		bool testFrame(int ind = 0);
+
+		void getData(std::vector<Eigen::Matrix4d> & po, std::vector<RGBDFrame*> & fr, std::vector<ModelMask*> & mm, Eigen::Matrix4d p = Eigen::Matrix4d::Identity());
+
+		//void getData(std::vector<Eigen::Matrix4d> & po, std::vector<RGBDFrame*> & fr, std::vector<ModelMask*> & mm, Eigen::Matrix4d p = Eigen::Matrix4d::Identity());
 	};
 
 }
diff --git a/quasimodo/quasimodo_models/include/model/ModelMask.h b/quasimodo/quasimodo_models/include/model/ModelMask.h
index a75ecf88..cb5cb4fb 100644
--- a/quasimodo/quasimodo_models/include/model/ModelMask.h
+++ b/quasimodo/quasimodo_models/include/model/ModelMask.h
@@ -19,6 +19,7 @@ namespace reglib
 	class ModelMask{
 		public:
 		int id;
+		std::string label;
 		cv::Mat mask;
 		int width;
 		int height;
@@ -27,7 +28,7 @@ namespace reglib
 		std::vector<int> testh;
 		int sweepid;
 
-		ModelMask(cv::Mat mask_);
+		ModelMask(cv::Mat mask_, std::string label_ = "");
 		~ModelMask();
 		cv::Mat getMask();
 	};
diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h
index 3b098a42..3d0ec141 100644
--- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h
+++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h
@@ -13,65 +13,139 @@
 #include "../registration/MassRegistration.h"
 #include "../mesher/Mesh.h"
 
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <opencv2/features2d/features2d.hpp>
+
 
 #include <pcl/visualization/pcl_visualizer.h>
 
+#include <pcl/ModelCoefficients.h>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+//#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <map>
+
+//#include "../../densecrf/include/densecrf.h"
+
 namespace reglib
 {
-	class UpdatedModels{
-		public:
-		std::vector< Model * > new_models;
-		std::vector< Model * > updated_models;
-		std::vector< Model * > unchanged_models;
-		std::vector< Model * > deleted_models;
-		
-		UpdatedModels(){}
-	};
-	
-	class OcclusionScore{
-		public:
-		double score;
-		double occlusions;
-
-		OcclusionScore(){score = 0;occlusions = 0;}
-		OcclusionScore(	double score_ ,double occlusions_){score = score_;occlusions = occlusions_;}
-		~OcclusionScore(){}
-	};
-
-	class ModelUpdater{
-        public:
-        double occlusion_penalty;
-        double massreg_timeout;
-		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
-		Model * model;
-		Mesh * mesher;
-
-		ModelUpdater();
-		ModelUpdater(Model * model_);
-		~ModelUpdater();
-
-
-		virtual FusionResults registerModel(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1);
-		virtual void fuse(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1);
-		virtual UpdatedModels fuseData(FusionResults * f, Model * model1,Model * model2);
-		virtual void refine(double reg = 0.05,bool useFullMask = false);
-		virtual void show(bool stop = true);
-		virtual void pruneOcclusions();
-//		virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false);
-		virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false);
-		virtual void computeResiduals(std::vector<float> & residuals, std::vector<float> & weights, RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, bool debugg = false);
-
-		virtual std::vector<std::vector< OcclusionScore > >	computeAllOcclusionScores(	RGBDFrame * src, cv::Mat src_mask, RGBDFrame * dst, cv::Mat dst_mask,Eigen::Matrix4d p, bool debugg = false);
-		virtual	void computeMassRegistration(std::vector<Eigen::Matrix4d> current_poses, std::vector<RGBDFrame*> current_frames,std::vector<cv::Mat> current_masks);
-
-		std::vector<std::vector < OcclusionScore > > getOcclusionScores(std::vector<Eigen::Matrix4d> current_poses, std::vector<RGBDFrame*> current_frames, std::vector<ModelMask*> current_modelmasks,  bool debugg_scores = false, double speedup = 1.0);
-        std::vector<std::vector < float > > getScores(std::vector<std::vector < OcclusionScore > > occlusionScores);
-		std::vector<int> getPartition(std::vector< std::vector< float > > & scores, int dims = 2, int nr_todo = 5, double timelimit = 2);
-
-		virtual void recomputeScores();
-
-		CloudData * getCD(std::vector<Eigen::Matrix4d> current_poses, std::vector<RGBDFrame*> current_frames,std::vector<cv::Mat> current_masks, int step);
-	};
+using namespace std;
+using namespace Eigen;
+using namespace cv;
+
+class UpdatedModels{
+public:
+	std::vector< Model * > new_models;
+	std::vector< Model * > updated_models;
+	std::vector< Model * > unchanged_models;
+	std::vector< Model * > deleted_models;
+
+	UpdatedModels(){}
+	~UpdatedModels(){}
+};
+
+class SegmentationResults{
+public:
+
+	std::vector< std::vector< unsigned long > > component_dynamic;
+	std::vector< double > scores_dynamic;
+	std::vector< double > total_dynamic;
+	std::vector< std::vector< unsigned long > > component_moving;
+	std::vector< double > scores_moving;
+	std::vector< double > total_moving;
+
+	SegmentationResults(){}
+	~SegmentationResults(){}
+};
+
+class ModelUpdater{
+public:
+	double occlusion_penalty;
+	double massreg_timeout;
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+	Model * model;
+	Mesh * mesher;
+
+	std::map<std::string,double> benchtime;
+
+	int show_init_lvl;//init show
+	int show_refine_lvl;//refine show
+	bool show_scoring;//fuse scoring show
+
+	ModelUpdater();
+	ModelUpdater(Model * model_);
+	~ModelUpdater();
+
+
+	virtual FusionResults registerModel(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1);
+	virtual void fuse(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1);
+	virtual UpdatedModels fuseData(FusionResults * f, Model * model1,Model * model2);
+
+	virtual bool isRefinementNeeded();
+	virtual bool refineIfNeeded();
+
+	virtual void makeInitialSetup();
+
+	virtual double getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector<Matrix4d> & cp, vector<RGBDFrame*> & cf, vector<ModelMask*> & cm);
+	virtual void getGoodCompareFrames(vector<Matrix4d> & cp, vector<RGBDFrame*> & cf, vector<ModelMask*> & cm);
+
+	virtual pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr getPCLnormalcloud(vector<superpoint> & points);
+	virtual void addSuperPoints(vector<superpoint> & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int type = 1, bool debugg = false);
+	virtual vector<superpoint> getSuperPoints(vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<ModelMask*> cm, int type = 1, bool debugg = false);
+
+	virtual void getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2);
+
+	virtual void getDynamicWeights(bool store_distance, std::vector<double> & dvec, std::vector<double> & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, double * notocclusions, RGBDFrame* frame2,   int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<float> > & interframe_connectionStrength, double debugg);
+	virtual void getDynamicWeights(bool store_distance, std::vector<double> & dvec, std::vector<double> & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector<superpoint> & framesp1_test, std::vector<superpoint> & framesp1, double * overlaps, double * occlusions, double * notocclusions, RGBDFrame* frame2, std::vector<superpoint> & framesp2,  int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<float> > & interframe_connectionStrength, double debugg);
+
+
+	virtual void computeOcclusionAreas(vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<ModelMask*> cm);
+//	virtual void getDynamicWeights(bool store_distance, std::vector<double> & dvec, std::vector<double> & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2,   int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<float> > & interframe_connectionStrength, double debugg);
+//	virtual void getDynamicWeights(bool store_distance, std::vector<double> & dvec, std::vector<double> & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector<superpoint> & framesp1_test, std::vector<superpoint> & framesp1, double * overlaps, double * occlusions, RGBDFrame* frame2, std::vector<superpoint> & framesp2,  int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<float> > & interframe_connectionStrength, double debugg);
+	virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<double> > & interframe_connectionStrength,double debugg = false);
+	virtual std::vector<cv::Mat> computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<cv::Mat> masks);
+	virtual vector<Mat> computeDynamicObject(vector<Matrix4d> bgcp, vector<RGBDFrame*> bgcf, vector<Mat> bgmm, vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<Mat> mm, vector<Matrix4d> poses, vector<RGBDFrame*> frames, vector<Mat> masks, bool debugg = false);
+
+	virtual void computeMovingDynamicStatic(std::vector<cv::Mat> & movemask, std::vector<cv::Mat> & dynmask, vector<Matrix4d> bgcp, vector<RGBDFrame*> bgcf, vector<Matrix4d> poses, vector<RGBDFrame*> frames, bool debugg, std::string savePath = "");
+	//virtual SegmentationResults computeMovingDynamicStatic(vector<Matrix4d> bgcp, vector<RGBDFrame*> bgcf, vector<Matrix4d> poses, vector<RGBDFrame*> frames, bool debugg);
+
+	virtual OcclusionScore computeOcclusionScore(vector<superpoint> & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1,  bool debugg = false);
+	virtual OcclusionScore computeOcclusionScore(Model * mod, vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<ModelMask*> cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false);
+	virtual OcclusionScore computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp = Matrix4d::Identity(),int step = 1, bool debugg = false);
+	virtual vector<vector < OcclusionScore > > computeOcclusionScore(vector<Model *> models, vector<Matrix4d> rps, int step = 1, bool debugg = false);
+
+	virtual double computeOcclusionScoreCosts(vector<Model *> models);
+
+	virtual void addModelsToVector(vector<Model *> & models, vector<Matrix4d> & rps, Model * model, Matrix4d rp);
+
+	virtual void refine(double reg = 0.05,bool useFullMask = false, int visualization = 0);
+	virtual void show(bool stop = true);
+	virtual void pruneOcclusions();
+	//		virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false);
+	virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false);
+	virtual void computeResiduals(std::vector<float> & residuals, std::vector<float> & weights, RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, bool debugg = false);
+
+	virtual std::vector<std::vector< OcclusionScore > >	computeAllOcclusionScores(	RGBDFrame * src, cv::Mat src_mask, RGBDFrame * dst, cv::Mat dst_mask,Eigen::Matrix4d p, bool debugg = false);
+	virtual	void computeMassRegistration(std::vector<Eigen::Matrix4d> current_poses, std::vector<RGBDFrame*> current_frames,std::vector<cv::Mat> current_masks);
+
+	std::vector<std::vector < OcclusionScore > > getOcclusionScores(std::vector<Eigen::Matrix4d> current_poses, std::vector<RGBDFrame*> current_frames, std::vector<ModelMask*> current_modelmasks,  bool debugg_scores = false, double speedup = 1.0);
+	std::vector<std::vector < float > > getScores(std::vector<std::vector < OcclusionScore > > occlusionScores);
+	std::vector<int> getPartition(std::vector< std::vector< float > > & scores, int dims = 2, int nr_todo = 5, double timelimit = 2);
+
+	virtual void recomputeScores();
+
+	CloudData * getCD(std::vector<Eigen::Matrix4d> current_poses, std::vector<RGBDFrame*> current_frames,std::vector<cv::Mat> current_masks, int step);
+};
 
 }
 
diff --git a/quasimodo/quasimodo_models/include/registration/.MassRegistrationPPR2.h.swp b/quasimodo/quasimodo_models/include/registration/.MassRegistrationPPR2.h.swp
new file mode 100644
index 00000000..fee1de36
Binary files /dev/null and b/quasimodo/quasimodo_models/include/registration/.MassRegistrationPPR2.h.swp differ
diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistration.h b/quasimodo/quasimodo_models/include/registration/MassRegistration.h
index d5fe2634..00b01539 100644
--- a/quasimodo/quasimodo_models/include/registration/MassRegistration.h
+++ b/quasimodo/quasimodo_models/include/registration/MassRegistration.h
@@ -13,10 +13,10 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/common/transformation_from_correspondences.h>
-#include <pcl_ros/transforms.h>
-#include <pcl_ros/point_cloud.h>
-#include <pcl/ros/conversions.h>
-#include <pcl_conversions/pcl_conversions.h>
+//#include <pcl_ros/transforms.h>
+//#include <pcl_ros/point_cloud.h>
+//#include <pcl/ros/conversions.h>
+//#include <pcl_conversions/pcl_conversions.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
 //#include "ICP.h"
@@ -67,12 +67,24 @@ namespace reglib
 
 		unsigned int visualizationLvl;
 
+
+		std::string savePath;
+
 		std::vector<RGBDFrame *> frames;
 		std::vector<ModelMask *> mmasks;
 
 		MassRegistration();
 		~MassRegistration();
 
+		bool okVal(double v);
+		bool isValidPoint(pcl::PointXYZRGBNormal p);
+
+
+		virtual void clearData();
+		virtual void addData(RGBDFrame* frame, ModelMask * mmask);
+		virtual void addModelData(Model * model, bool submodels = true);
+		virtual void addData(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud);
+
 		virtual void setData(std::vector<RGBDFrame*> frames_, std::vector<ModelMask *> mmasks);
 		virtual void setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > all_clouds);
 		void setVisualizationLvl(unsigned int lvl);
@@ -81,10 +93,14 @@ namespace reglib
 		virtual void show(std::vector<Eigen::MatrixXd> Xv, bool save = false, std::string filename = "", bool stop = true);
 		Eigen::MatrixXd getMat(int rows, int cols, double * datas);
 
+
+		virtual void savePCD(std::vector<Eigen::MatrixXd> Xv, std::string path);
+
 	};
 
 }
 
 #include "MassRegistrationPPR.h"
+#include "MassRegistrationPPR2.h"
 
 #endif // MassRegistration_H
diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR.h
index fb436d05..bd491369 100644
--- a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR.h
+++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR.h
@@ -21,6 +21,23 @@ namespace reglib
 		double stopval;
 		unsigned steps;
 
+		double rematch_time;
+		double residuals_time;
+		double opt_time;
+		double computeModel_time;
+		double setup_matches_time;
+		double setup_equation_time;
+		double setup_equation_time2;
+		double solve_equation_time;
+		double total_time_start;
+
+		const unsigned int maxcount = 1000000;
+		double * Qp_arr;
+		double * Qn_arr;
+		double * Xp_arr;
+		double * Xn_arr;
+		double * rangeW_arr;
+
 		std::vector<int> nr_datas;
 
 		std::vector< bool > is_ok;
@@ -31,8 +48,13 @@ namespace reglib
 		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > transformed_normals;
 		std::vector< Eigen::VectorXd > informations;
 
-        std::vector< int > nr_arraypoints;
+		std::vector< int > nr_arraypoints;
 		std::vector< double * > arraypoints;
+		std::vector< double * > arraynormals;
+		std::vector< double * > arraycolors;
+		std::vector< double * > arrayinformations;
+
+
 		std::vector< Tree3d * > trees3d;
 		std::vector< ArrayData3D<double> * > a3dv;
 
@@ -61,10 +83,19 @@ namespace reglib
 		MassRegistrationPPR(double startreg = 0.05, bool visualize = false);
 		~MassRegistrationPPR();
 		
+		void addModelData(Model * model, bool submodels = true);
+
+		void clearData();
+		void addData(RGBDFrame* frame, ModelMask * mmask);
+		void addData(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud);
+
 		MassFusionResults getTransforms(std::vector<Eigen::Matrix4d> guess);
 		void setData(std::vector<RGBDFrame*> frames_, std::vector<ModelMask *> mmasks);
 		void setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > all_clouds);
 
+		void rematch(std::vector<Eigen::Matrix4d> poses, std::vector<Eigen::Matrix4d> prev_poses, bool first);
+		Eigen::MatrixXd getAllResiduals(std::vector<Eigen::Matrix4d> poses);
+		std::vector<Eigen::Matrix4d> optimize(std::vector<Eigen::Matrix4d> poses);
 	};
 
 }
diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h
new file mode 100644
index 00000000..b843a7e6
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h
@@ -0,0 +1,168 @@
+#ifndef MassRegistrationPPR2_H
+#define MassRegistrationPPR2_H
+
+#include "MassRegistration.h"
+#include <time.h>
+
+namespace reglib
+{
+
+	class TestMatch{
+		public:
+
+		long src;
+		long dst;
+		double d;
+
+		TestMatch(long src_, long dst_,	double d_){
+			src = src_;
+			dst = dst_;
+			d = d_;
+		}
+
+		~TestMatch(){}
+	};
+
+	class MassRegistrationPPR2 : public MassRegistration
+	{
+		public:
+
+		MatchType type;
+
+		bool fast_opt;
+		bool use_PPR_weight;
+		bool use_features;
+		bool normalize_matchweights;
+
+		double stopval;
+		unsigned steps;
+
+		double rematch_time;
+		double residuals_time;
+		double opt_time;
+		double computeModel_time;
+		double setup_matches_time;
+		double setup_equation_time;
+		double setup_equation_time2;
+		double solve_equation_time;
+		double total_time_start;
+
+		const unsigned long maxcount = 1000000;
+
+		Model * model;
+
+		std::vector<long > nr_datas;
+
+		std::vector< bool > is_ok;
+
+		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > points;
+		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > colors;
+		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > normals;
+		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > transformed_points;
+		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > transformed_normals;
+		std::vector< Eigen::VectorXd > informations;
+
+
+
+		std::vector< long >				kp_nr_arraypoints;
+		std::vector< double * >			kp_arraypoints;
+		std::vector< double * >			kp_arraynormals;
+		std::vector< double * >			kp_arrayinformations;
+		std::vector< uint64_t * >		kp_arraydescriptors;
+		std::vector< std::vector< std::vector< TestMatch > > > kp_matches;
+		double * kp_Qp_arr;
+		double * kp_Qn_arr;
+		double * kp_Xp_arr;
+		double * kp_Xn_arr;
+		double * kp_rangeW_arr;
+		DistanceWeightFunction2PPR2 * kpfunc;
+		//std::vector< std::vector< std::vector<long > > > matchids;
+
+		std::vector< long >      frameid;
+
+		bool use_surface;
+		std::vector< long >		nr_arraypoints;
+		std::vector< double * > arraypoints;
+		std::vector< double * > arraynormals;
+		std::vector< double * > arraycolors;
+		std::vector< double * > arrayinformations;
+		std::vector< Tree3d * > trees3d;
+		std::vector< ArrayData3D<double> * > a3dv;
+		std::vector<long > nr_matches;
+		std::vector< std::vector< std::vector<long > > > matchids;
+        std::vector< std::vector< std::vector<double> > > matchdists;
+		double * Qp_arr;
+		double * Qn_arr;
+		double * Xp_arr;
+		double * Xn_arr;
+		double * rangeW_arr;
+		DistanceWeightFunction2PPR2 * func;
+		std::vector< std::vector< double > > matchscores;
+
+
+		bool use_depthedge;
+		//long depthedge_nr_neighbours;
+		std::vector< long >		depthedge_nr_arraypoints;
+        std::vector< double * > depthedge_arraypoints;
+		//std::vector< long * >    depthedge_neighbours;
+		std::vector< double * > depthedge_arrayinformations;
+		std::vector< Tree3d * > depthedge_trees3d;
+		std::vector< ArrayData3D<double> * > depthedge_a3dv;
+		std::vector<long > depthedge_nr_matches;
+		std::vector< std::vector< std::vector<long > > > depthedge_matchids;
+		std::vector< std::vector< std::vector<double> > > depthedge_matchdists;
+		double * depthedge_Qp_arr;
+		double * depthedge_Xp_arr;
+		double * depthedge_rangeW_arr;
+		DistanceWeightFunction2PPR2 * depthedge_func;
+
+		std::vector<long > sweepids;
+		std::vector<long > background_nr_datas;
+//		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > background_points;
+//		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > background_colors;
+//		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > background_normals;
+//		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > background_transformed_points;
+//		std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > background_transformed_normals;
+//		std::vector< Eigen::VectorXd > background_informations;
+//		std::vector< nanoflann::KDTreeAdaptor<Eigen::Matrix3Xd, 3, nanoflann::metric_L2_Simple> * > background_trees;
+//		std::vector<long > background_nr_matches;
+//		std::vector< std::vector< std::vector<long > > > background_matchids;
+
+
+//		std::vector<long > feature_start;//Dimension of data a specific feature starts, if the feature is RGB this should be 3
+//		std::vector<long > feature_end;//Dimension of data a specific feature ends, if the feature is RGB this should be 5
+//		std::vector< DistanceWeightFunction2 * > feature_func;
+
+
+
+
+		MassRegistrationPPR2(double startreg = 0.05, bool visualize = false);
+		~MassRegistrationPPR2();
+
+		void clearData();
+        void addModel(Model * model);
+		void addModelData(Model * model, bool submodels = true);
+		void addData(RGBDFrame* frame, ModelMask * mmask);
+		void addData(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud);
+
+		MassFusionResults getTransforms(std::vector<Eigen::Matrix4d> guess);
+		void setData(std::vector<RGBDFrame*> frames_, std::vector<ModelMask *> mmasks);
+		void setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > all_clouds);
+
+		void rematchKeyPoints(std::vector<Eigen::Matrix4d> poses, std::vector<Eigen::Matrix4d> prev_poses, bool first);
+		void rematch(std::vector<Eigen::Matrix4d> poses, std::vector<Eigen::Matrix4d> prev_poses, bool rematch_surface, bool rematch_edges,bool first);
+		Eigen::MatrixXd getAllResiduals(std::vector<Eigen::Matrix4d> poses);
+		Eigen::MatrixXd depthedge_getAllResiduals(std::vector<Eigen::Matrix4d> poses);
+		Eigen::MatrixXd getAllKpResiduals(std::vector<Eigen::Matrix4d> poses);
+
+
+		void showEdges(std::vector<Eigen::Matrix4d> poses);
+
+		//Eigen::MatrixXd getAllKPResiduals(std::vector<Eigen::Matrix4d> poses);
+
+		std::vector<Eigen::Matrix4d> optimize(std::vector<Eigen::Matrix4d> poses);
+	};
+
+}
+
+#endif
diff --git a/quasimodo/quasimodo_models/include/registration/Registration.h b/quasimodo/quasimodo_models/include/registration/Registration.h
index ead1437f..29aa199c 100644
--- a/quasimodo/quasimodo_models/include/registration/Registration.h
+++ b/quasimodo/quasimodo_models/include/registration/Registration.h
@@ -44,6 +44,7 @@ namespace reglib
 
 		Eigen::MatrixXd guess;
 		double score;
+		double stop;
 		bool timeout;
 
 		std::vector< Eigen::MatrixXd > candidates;
@@ -81,7 +82,7 @@ namespace reglib
 		void setVisualizationLvl(unsigned int lvl);
 
 		virtual FusionResults getTransform(Eigen::MatrixXd guess);
-		virtual void show(Eigen::MatrixXd X, Eigen::MatrixXd Y);
+		virtual void show(Eigen::MatrixXd X, Eigen::MatrixXd Y, bool stop = true);
 		virtual void show(Eigen::MatrixXd X, Eigen::MatrixXd Y, Eigen::VectorXd W);
 
 		virtual void show(Eigen::MatrixXd X, Eigen::MatrixXd Xn, Eigen::MatrixXd Y, Eigen::MatrixXd Yn);
diff --git a/quasimodo/quasimodo_models/include/registration/RegistrationRandom.h b/quasimodo/quasimodo_models/include/registration/RegistrationRandom.h
index c2d16163..851969d1 100644
--- a/quasimodo/quasimodo_models/include/registration/RegistrationRandom.h
+++ b/quasimodo/quasimodo_models/include/registration/RegistrationRandom.h
@@ -20,6 +20,8 @@ namespace reglib
 
 		RegistrationRandom();
 		~RegistrationRandom();
+
+		bool issame(FusionResults fr1, FusionResults fr2, int stepxsmall);
 		
 		FusionResults getTransform(Eigen::MatrixXd guess);
 	};
diff --git a/quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h b/quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h
index f34b7979..b7c9f37b 100644
--- a/quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h
+++ b/quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h
@@ -18,12 +18,11 @@ namespace reglib
 
 		Eigen::Matrix<double, 3, Eigen::Dynamic> Y;
 		Eigen::Matrix<double, 3, Eigen::Dynamic> N;
-		std::vector<double> total_dweight;
 		unsigned int ycols;
 
 		Eigen::VectorXd DST_INORMATION;
 
-		DistanceWeightFunction2PPR2 * func;
+		//DistanceWeightFunction2PPR2 * func;
 
 		int nr_arraypoints;
 		double * arraypoints;
diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h
index d14540ac..38c70231 100644
--- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h
+++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h
@@ -41,12 +41,21 @@ class DistanceWeightFunction2
 	double convergence_threshold;
 	bool debugg_print;
 
+	std::string savePath;
+	std::stringstream saveData;
+
 	DistanceWeightFunction2();
 	~DistanceWeightFunction2();
 
+	MatrixXd getMat(std::vector<double> & vec);
+
+	virtual void computeModel(std::vector<double> & vec);
 	virtual void computeModel(MatrixXd mat);
+	virtual VectorXd getProbs(std::vector<double> & vec);
 	virtual VectorXd getProbs(MatrixXd mat);
-	virtual double getProb(double d);
+
+	virtual double getProbInfront(double d, bool debugg = false);
+	virtual double getProb(double d, bool debugg = false);
 	virtual double getNoise();
 	virtual double getConvergenceThreshold();
 	virtual bool update();
@@ -58,4 +67,5 @@ class DistanceWeightFunction2
 
 #include "DistanceWeightFunction2PPR.h"
 #include "DistanceWeightFunction2PPR2.h"
+#include "DistanceWeightFunction2PPR3.h"
 #endif // DistanceWeightFunction2_H
diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h
index 61ea57bf..2ec40118 100644
--- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h
+++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h
@@ -11,6 +11,15 @@
 #include "gflags/gflags.h"
 #include "glog/logging.h"
 
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <opencv2/features2d/features2d.hpp>
+
+#include "weightfunctions/Distribution.h"
+
+
 using namespace Eigen;
 namespace reglib{
 
@@ -20,6 +29,15 @@ using ceres::Problem;
 using ceres::Solver;
 using ceres::Solve;
 
+class distributionFunction {
+	public:
+
+	virtual void train(std::vector<float> & hist, int nr_bins = -1);
+	virtual void update();
+	virtual double getval(double x);
+	virtual double getcdf(double x);
+};
+
 class DistanceWeightFunction2PPR2 : public DistanceWeightFunction2
 {
 public:
@@ -49,6 +67,7 @@ class DistanceWeightFunction2PPR2 : public DistanceWeightFunction2
 
 	double startmaxd;
 	double maxd;
+	double mind;
 	int histogram_size;
 	int starthistogram_size;
 	double blurval;
@@ -56,11 +75,16 @@ class DistanceWeightFunction2PPR2 : public DistanceWeightFunction2
 
 	double noiseval;
 	double startreg;
-	
+
+	bool ggd;
+	bool compute_infront;
+
 	std::vector<float> prob;
+	std::vector<float> infront;
 	std::vector<float> histogram;
 	std::vector<float> blur_histogram;
 	std::vector<float> noise;
+	std::vector<float> noisecdf;
 
 	int nr_refineiters;
 	bool refine_mean;
@@ -81,16 +105,21 @@ class DistanceWeightFunction2PPR2 : public DistanceWeightFunction2
 	bool bidir;
 	int iter;
 
+	double histogram_mul;
+	double histogram_mul2;
+
 	DistanceWeightFunction2PPR2(	double maxd_	= 0.25, int histogram_size_ = 25000);
 	~DistanceWeightFunction2PPR2();
 	virtual void computeModel(MatrixXd mat);
 	virtual VectorXd getProbs(MatrixXd mat);
-	virtual double getProb(double d);
+	virtual double getProb(double d, bool debugg = false);
+	virtual double getProbInfront(double d, bool debugg = false);
 	virtual double getNoise();
 	virtual double getConvergenceThreshold();
 	virtual bool update();
 	virtual void reset();
 	virtual std::string getString();
+	virtual double getInd(double d, bool debugg = false);
 };
 
 }
diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h
new file mode 100644
index 00000000..609862f5
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h
@@ -0,0 +1,126 @@
+#ifndef DistanceWeightFunction2PPR3_H
+#define DistanceWeightFunction2PPR3_H
+
+#include <cmath>
+#include <sys/time.h>
+#include "DistanceWeightFunction2.h"
+
+#include "ceres/ceres.h"
+#include "ceres/rotation.h"
+#include "ceres/iteration_callback.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <opencv2/features2d/features2d.hpp>
+
+#include "weightfunctions/Distribution.h"
+#include "core/Util.h"
+#include "weightfunctions/SignalProcessing.h"
+
+
+using namespace Eigen;
+namespace reglib{
+
+class DistanceWeightFunction2PPR3 : public DistanceWeightFunction2
+{
+public:
+
+    SignalProcessing * sp;
+    Distribution * dist;
+
+    bool fixed_histogram_size;
+
+	double stdval;
+	double stdval2;
+	double mulval;
+	double meanval;
+	double meanval2;
+
+    double maxnoise;
+
+    bool zeromean;
+
+	double costpen;
+
+	double maxp;
+
+	bool first;
+
+	bool update_size;
+	double target_length;
+	double data_per_bin;
+	double meanoffset;
+	double blur;
+
+	double startmaxd;
+	double maxd;
+	double mind;
+	int histogram_size;
+	int starthistogram_size;
+	double blurval;
+	double stdgrow;
+
+	double noiseval;
+	double startreg;
+
+	bool ggd;
+	bool compute_infront;
+
+	std::vector<float> prob;
+	std::vector<float> infront;
+	std::vector<float> histogram;
+	std::vector<float> blur_histogram;
+	std::vector<float> noise;
+	std::vector<float> noisecdf;
+
+	int nr_refineiters;
+	bool refine_mean;
+	bool refine_mul;
+	bool refine_std;
+
+	bool threshold;
+	bool uniform_bias;
+	bool scale_convergence;
+	double nr_inliers;
+
+	bool rescaling;
+
+	bool interp;
+
+	bool max_under_mean;
+
+	bool bidir;
+	int iter;
+
+	double histogram_mul;
+	double histogram_mul2;
+
+
+    DistanceWeightFunction2PPR3(Distribution * dist,	double maxd_	= 0.25, int histogram_size_ = 25000);
+
+    DistanceWeightFunction2PPR3(	double maxd_	= 0.25, int histogram_size_ = 25000);
+    ~DistanceWeightFunction2PPR3();
+
+    virtual void recomputeHistogram(std::vector<float> & hist, MatrixXd & mat);
+    virtual void recomputeProbs();
+
+	virtual void computeModel(MatrixXd mat);
+	virtual VectorXd getProbs(MatrixXd mat);
+	virtual double getProb(double d, bool debugg = false);
+	virtual double getProbInfront(double d, bool debugg = false);
+	virtual double getNoise();
+	virtual double getConvergenceThreshold();
+	virtual bool update();
+	virtual void reset();
+	virtual std::string getString();
+	virtual double getInd(double d, bool debugg = false);
+    virtual double getDfromInd(double ind, bool debugg = false);
+};
+
+}
+
+#endif // DistanceWeightFunction2PPR3test_H
diff --git a/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h b/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h
new file mode 100644
index 00000000..f21a10f7
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h
@@ -0,0 +1,41 @@
+#ifndef Distribution_H
+#define Distribution_H
+
+#include <vector>
+#include <stdio.h>
+#include <stdlib.h>
+#include <Eigen/Dense>
+
+using namespace Eigen;
+namespace reglib
+{
+
+class Distribution
+{
+public:
+    double regularization;
+    double mean;
+    bool debugg_print;
+
+
+    Distribution();
+    ~Distribution();
+	virtual void reset();
+    virtual void train(std::vector<float> & hist, unsigned int nr_bins = 0);
+    virtual void update();
+
+    virtual double getNoise();
+    virtual void setNoise(double x);
+
+    virtual double getval(double x);
+    virtual double getcdf(double x);
+    virtual void setRegularization(double x);
+    virtual void print();
+    virtual void getMaxdMind(double & maxd, double & mind, double prob = 0.0001);
+};
+
+}
+
+#include "GaussianDistribution.h"
+#include "GeneralizedGaussianDistribution.h"
+#endif // Distribution
diff --git a/quasimodo/quasimodo_models/include/weightfunctions/GaussianDistribution.h b/quasimodo/quasimodo_models/include/weightfunctions/GaussianDistribution.h
new file mode 100644
index 00000000..8f3614ba
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/weightfunctions/GaussianDistribution.h
@@ -0,0 +1,56 @@
+#ifndef GaussianDistribution_H
+#define GaussianDistribution_H
+
+#include <vector>
+#include <stdio.h>
+#include <stdlib.h> 
+#include <Eigen/Dense>
+#include "Distribution.h"
+
+using namespace Eigen;
+namespace reglib
+{
+
+class GaussianDistribution  : public Distribution
+{
+public:
+
+    const double step_h = 0.00001;
+    const unsigned int step_iter = 25;
+    const double cutoff_exp = -14;
+
+    double mul;
+    double stdval;
+    double scaledinformation;
+
+    bool refine_mean;
+    bool refine_mul;
+    bool refine_std;
+
+    double costpen;
+    bool zeromean;
+    int nr_refineiters;
+
+    GaussianDistribution(bool refine_std_ = true, bool zeromean = true, bool refine_mean = false, bool refine_mul = false, double costpen_ = 3,int nr_refineiters_ = 1,double mul_ = 1, double mean_ = 0,	double stdval_ = 1);
+    //GaussianDistribution(double mul_ = 1, double mean_ = 0,	double stdval_ = 1);
+    ~GaussianDistribution();
+    virtual void train(std::vector<float> & hist, unsigned int nr_bins = 0);
+    virtual void update();
+    virtual double getNoise();
+    virtual void setNoise(double x);
+    virtual double getval(double x);
+    virtual double getcdf(double x);
+    virtual void print();
+
+    //virtual void getMaxdMind(double & maxd, double & mind, double prob = 0.1);
+
+    double scoreCurrent2(double mul, double mean, double stddiv,  std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+    double fitStdval2   (double mul, double mean, double std_mid, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+    double fitMean2     (double mul, double mean, double std_mid, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+    double fitMul2      (double mul, double mean, double std_mid, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+
+};
+
+}
+
+#endif // GaussianDistribution
diff --git a/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h b/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h
new file mode 100644
index 00000000..245dcf97
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h
@@ -0,0 +1,66 @@
+#ifndef GeneralizedGaussianDistribution_H
+#define GeneralizedGaussianDistribution_H
+
+#include <vector>
+#include <stdio.h>
+#include <stdlib.h> 
+#include <Eigen/Dense>
+#include "Distribution.h"
+
+using namespace Eigen;
+namespace reglib
+{
+
+class GeneralizedGaussianDistribution  : public Distribution
+{
+public:
+
+    const double step_h = 0.00001;
+    const unsigned int step_iter = 25;
+    const double cutoff_exp = -14;
+
+    double mul;
+    double stdval;
+    double scaledinformation;
+    double power;
+
+    bool refine_mean;
+    bool refine_mul;
+    bool refine_std;
+    bool refine_power;
+
+    double costpen;
+    bool zeromean;
+    int nr_refineiters;
+
+    double start;
+    double stop;
+    std::vector<float> numcdf_vec;
+
+    GeneralizedGaussianDistribution(bool refine_std_ = true, bool refine_power_ = false, bool zeromean = true, bool refine_mean = false, bool refine_mul = false, double costpen_ = 3,int nr_refineiters_ = 1,double mul_ = 1, double mean_ = 0,	double stdval_ = 1, double power_ = 2);
+    //GeneralizedGaussianDistribution(double mul_ = 1, double mean_ = 0,	double stdval_ = 1);
+    ~GeneralizedGaussianDistribution();
+    virtual void train(std::vector<float> & hist, unsigned int nr_bins = 0);
+    virtual void update();
+	virtual double getInp(double x = 0);
+    virtual double getNoise();
+	virtual void setNoise(double x = 0);
+	virtual double getval(double x = 0);
+	virtual double getcdf(double x = 0);
+    virtual void print();
+
+    //virtual void getMaxdMind(double & maxd, double & mind, double prob = 0.1);
+
+    double scoreCurrent3(double mul, double mean, double stddiv,  double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+    double fitStdval3   (double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+    double fitMean3     (double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+    double fitMul3      (double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+    double fitPower3    (double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen);
+
+    virtual void update_numcdf_vec(unsigned int bins = 1000, double prob = 0.000001);
+    //double interp(double x);
+};
+
+}
+
+#endif // GeneralizedGaussianDistribution
diff --git a/quasimodo/quasimodo_models/include/weightfunctions/SignalProcessing.h b/quasimodo/quasimodo_models/include/weightfunctions/SignalProcessing.h
new file mode 100644
index 00000000..e0dacec5
--- /dev/null
+++ b/quasimodo/quasimodo_models/include/weightfunctions/SignalProcessing.h
@@ -0,0 +1,23 @@
+#ifndef SignalProcessing_H
+#define SignalProcessing_H
+
+#include <vector>
+#include <stdio.h>
+#include <stdlib.h>
+#include <Eigen/Dense>
+
+using namespace Eigen;
+namespace reglib
+{
+
+class SignalProcessing
+{
+public:
+    SignalProcessing();
+    ~SignalProcessing();
+    virtual void process(std::vector<float> & in, std::vector<float> & out, double stdval, unsigned int nr_bins = 0);
+};
+
+}
+
+#endif // SignalProcessing
diff --git a/quasimodo/quasimodo_models/src/core/2d/convolution.h b/quasimodo/quasimodo_models/src/core/2d/convolution.h
new file mode 100644
index 00000000..43e8b991
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/convolution.h
@@ -0,0 +1,159 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_CONVOLUTION_H
+#define PCL_2D_CONVOLUTION_H
+
+#include <pcl/pcl_base.h>
+#include <pcl/filters/filter.h>
+#include <pcl/point_types.h>
+
+namespace pcl
+{
+  /**
+   * This typedef is used to represent a point cloud containing edge information
+   */
+  struct PointXYZIEdge
+  {
+    PCL_ADD_POINT4D;                    // preferred way of adding a XYZ+padding
+    float magnitude;
+    float direction;
+    float magnitude_x;
+    float magnitude_y;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW     // make sure our new allocators are aligned
+  } EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment
+
+  /** \brief A 2D convolution class. */ 
+  template <typename PointT>
+  class Convolution : public Filter<PointT>
+  {
+    public:
+      using Filter<PointT>::input_;
+      
+      /**
+       * Extra pixels are added to the input image so that convolution can be performed over the entire image.
+       *
+       * (kernel_height/2) rows are added before the first row and after the last row
+       * (kernel_width/2) columns are added before the first column and after the last column
+       * border options define what values are set for these extra rows and columns
+       *
+       * Assume that the three rows of right edge of the image looks like this:
+       *    .. 3 2 1
+       *    .. 6 5 4
+       *    .. 9 8 7
+       *
+       * BOUNDARY_OPTION_CLAMP : the extra pixels are set to the pixel value of the boundary pixel
+       *    This option makes it seem as if it were:
+       *    .. 3 2 1| 1 1 1 ..
+       *    .. 6 5 4| 4 4 4 ..
+       *    .. 9 8 7| 7 7 7 ..
+       *
+       * BOUNDARY_OPTION_MIRROR : the input image is mirrored at the boundary.
+       *    This option makes it seem as if it were:
+       *    .. 3 2 1| 1 2 3 ..
+       *    .. 6 5 4| 4 5 6 ..
+       *    .. 9 8 7| 7 8 9 ..
+       *
+       * BOUNDARY_OPTION_ZERO_PADDING : the extra pixels are simply set to 0
+       *    This option makes it seem as if it were:
+       *    .. 3 2 1| 0 0 0 ..
+       *    .. 6 5 4| 0 0 0 ..
+       *    .. 9 8 7| 0 0 0 ..
+       *
+       * Note that the input image is not actually extended in size. Instead, based on these options,
+       * the convolution is performed differently at the border pixels.
+       */
+      enum BOUNDARY_OPTIONS_ENUM
+      {
+        BOUNDARY_OPTION_CLAMP,
+        BOUNDARY_OPTION_MIRROR,
+        BOUNDARY_OPTION_ZERO_PADDING
+      };
+
+      Convolution ()
+      {
+        boundary_options_ = BOUNDARY_OPTION_CLAMP;
+      }
+
+      /** \brief Sets the kernel to be used for convolution
+        * \param[in] kernel convolution kernel passed by reference
+        */
+      inline void 
+      setKernel (const pcl::PointCloud<PointT> &kernel)
+      {
+        kernel_ = kernel;
+      }
+
+      /**
+        * \param[in] boundary_options enum indicating the boundary options to be used for convolution
+        */
+      inline void 
+      setBoundaryOptions (BOUNDARY_OPTIONS_ENUM boundary_options)
+      {
+        boundary_options_ = boundary_options;
+      }
+
+      /** \brief Performs 2D convolution of the input point cloud with the kernel.
+        * Uses clamp as the default boundary option.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      filter (pcl::PointCloud<PointT> &output);
+
+    protected:
+      /** \brief This is an over-ride function for the pcl::Filter interface. */
+      void 
+      applyFilter (pcl::PointCloud<PointT> &) {}
+
+    private:
+      BOUNDARY_OPTIONS_ENUM boundary_options_;
+      pcl::PointCloud<PointT> kernel_;
+  };
+}
+
+#include "impl/convolution.hpp"
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZIEdge,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (float, magnitude, magnitude)
+    (float, direction, direction)
+    (float, magnitude_x, magnitude_x)
+    (float, magnitude_y, magnitude_y)
+)
+#endif // PCL_2D_CONVOLUTION_2D_H
diff --git a/quasimodo/quasimodo_models/src/core/2d/edge.h b/quasimodo/quasimodo_models/src/core/2d/edge.h
new file mode 100644
index 00000000..821f285e
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/edge.h
@@ -0,0 +1,306 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_EDGE_H
+#define PCL_2D_EDGE_H
+
+#include <pcl/pcl_base.h>
+#include "convolution.h"
+#include "kernel.h"
+
+namespace pcl
+{
+  template <typename PointInT, typename PointOutT>
+  class Edge
+  {
+    private:
+      typedef typename pcl::PointCloud<PointInT> PointCloudIn;
+      typedef typename PointCloudIn::Ptr PointCloudInPtr;
+
+      PointCloudInPtr input_;
+      pcl::Convolution<PointInT> convolution_;
+      kernel<PointInT>  kernel_;
+
+      /** \brief This function performs edge tracing for Canny Edge detector.
+        *
+        * \param[in] rowOffset row offset for direction in which the edge is to be traced
+        * \param[in] colOffset column offset for direction in which the edge is to be traced
+        * \param[in] row row location of the edge point
+        * \param[in] col column location of the edge point
+        * \param[out] maxima point cloud containing the edge information in the magnitude channel
+        */
+      inline void
+      cannyTraceEdge (int rowOffset, int colOffset, int row, int col, 
+                      pcl::PointCloud<pcl::PointXYZI> &maxima);
+
+      /** \brief This function discretizes the edge directions in steps of 22.5 degrees.
+        * \param thet point cloud containing the edge information in the direction channel
+        */
+      void
+      discretizeAngles (pcl::PointCloud<PointOutT> &thet);
+
+      /** \brief This function suppresses the edges which don't form a local maximum 
+        * in the edge direction.
+        * \param[in] edges point cloud containing all the edges
+        * \param[out] maxima point cloud containing the non-max supressed edges
+        * \param[in] tLow
+        */
+      void
+      suppressNonMaxima (const pcl::PointCloud<PointXYZIEdge> &edges, 
+                         pcl::PointCloud<pcl::PointXYZI> &maxima, float tLow);
+
+    public:
+      typedef boost::shared_ptr<Edge> Ptr;
+      typedef boost::shared_ptr<const Edge> ConstPtr;
+
+      enum OUTPUT_TYPE
+      {
+        OUTPUT_Y,
+        OUTPUT_X,
+        OUTPUT_X_Y,
+        OUTPUT_MAGNITUDE,
+        OUTPUT_DIRECTION,
+        OUTPUT_MAGNITUDE_DIRECTION,
+        OUTPUT_ALL
+      };
+
+      enum DETECTOR_KERNEL_TYPE
+      {
+        CANNY,
+        SOBEL,
+        PREWITT,
+        ROBERTS,
+        LOG,
+        DERIVATIVE_CENTRAL,
+        DERIVATIVE_FORWARD,
+        DERIVATIVE_BACKWARD
+      };
+
+    private:
+      OUTPUT_TYPE output_type_;
+      DETECTOR_KERNEL_TYPE detector_kernel_type_;
+      bool non_maximal_suppression_;
+      bool hysteresis_thresholding_;
+
+      float hysteresis_threshold_low_;
+      float hysteresis_threshold_high_;
+      float non_max_suppression_radius_x_;
+      float non_max_suppression_radius_y_;
+
+    public:
+      Edge () :
+        output_type_ (OUTPUT_X),
+        detector_kernel_type_ (SOBEL),
+        non_maximal_suppression_ (false),
+        hysteresis_thresholding_ (false),
+        hysteresis_threshold_low_ (20),
+        hysteresis_threshold_high_ (80),
+        non_max_suppression_radius_x_ (3),
+        non_max_suppression_radius_y_ (3)
+      {
+      }
+
+      /** \brief Set the output type.
+        * \param[in] output_type the output type
+        */
+      void
+      setOutputType (OUTPUT_TYPE output_type)
+      {
+        output_type_ = output_type;
+      }
+
+      void
+      setHysteresisThresholdLow (float threshold)
+      {
+        hysteresis_threshold_low_ = threshold;
+      }
+
+      void
+      setHysteresisThresholdHigh (float threshold)
+      {
+        hysteresis_threshold_high_ = threshold;
+      }
+
+      /**
+        * \param[in] input_x
+        * \param[in] input_y
+        * \param[out] output
+        */
+      void 
+      sobelMagnitudeDirection (const pcl::PointCloud<PointInT> &input_x, 
+                               const pcl::PointCloud<PointInT> &input_y,
+                               pcl::PointCloud<PointOutT> &output);
+
+
+      /** \brief Perform Canny edge detection with two separated input images for 
+        * horizontal and vertical derivatives.
+        * All edges of magnitude above t_high are always classified as edges. All edges 
+        * below t_low are discarded. Edge values between t_low and t_high are classified 
+        * as edges only if they are connected to edges having magnitude > t_high and are 
+        * located in a direction perpendicular to that strong edge.
+        *
+        * \param[in] input_x Input point cloud passed by reference for the first derivative in the horizontal direction
+        * \param[in] input_y Input point cloud passed by reference for the first derivative in the vertical direction
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      canny (const pcl::PointCloud<PointInT> &input_x, 
+             const pcl::PointCloud<PointInT> &input_y,
+             pcl::PointCloud<PointOutT> &output);
+
+      /** \brief This is a convenience function which performs edge detection based on 
+        * the variable detector_kernel_type_
+        * \param[out] output
+        */
+      void 
+      detectEdge (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief All edges of magnitude above t_high are always classified as edges. 
+        * All edges below t_low are discarded.
+        * Edge values between t_low and t_high are classified as edges only if they are 
+        * connected to edges having magnitude > t_high and are located in a direction 
+        * perpendicular to that strong edge.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      detectEdgeCanny (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Uses the Sobel kernel for edge detection.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      detectEdgeSobel (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Uses the Prewitt kernel for edge detection.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      detectEdgePrewitt (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Uses the Roberts kernel for edge detection.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      detectEdgeRoberts (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Uses the LoG kernel for edge detection.
+        * Zero crossings of the Laplacian operator applied on an image indicate edges.
+        * Gaussian kernel is used to smoothen the image prior to the Laplacian.
+        * This is because Laplacian uses the second order derivative of the image and hence, is very sensitive to noise.
+        * The implementation is not two-step but rather applies the LoG kernel directly.
+        *
+        * \param[in] kernel_sigma variance of the LoG kernel used.
+        * \param[in] kernel_size a LoG kernel of dimensions kernel_size x kernel_size is used.
+        * \param[out] output Output point cloud passed by reference.
+        */
+      void 
+      detectEdgeLoG (const float kernel_sigma, const float kernel_size,
+                     pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeYCentralKernel.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      computeDerivativeXCentral (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYCentralKernel.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      computeDerivativeYCentral (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeYForwardKernel.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      computeDerivativeXForward (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYForwardKernel.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      computeDerivativeYForward (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeXBackwardKernel.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param output Output point cloud passed by reference
+        */
+      void 
+      computeDerivativeXBackward (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYBackwardKernel.
+        * This function does NOT include a smoothing step.
+        * The image should be smoothed before using this function to reduce noise.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      computeDerivativeYBackward (pcl::PointCloud<PointOutT> &output);
+
+      /** \brief Override function to implement the pcl::Filter interface
+        */
+      void 
+      applyFilter (pcl::PointCloud<PointOutT>& /*output*/) {}
+
+      /** \brief Set the input point cloud pointer
+        * \param[in] input pointer to input point cloud
+        */
+      void 
+      setInputCloud (PointCloudInPtr input)
+      {
+        input_ = input;
+      }
+  };
+}
+#include "impl/edge.hpp"
+
+#endif    // PCL_2D_EDGE_H 
+
diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/convolution.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/convolution.hpp
new file mode 100644
index 00000000..0ded38e8
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/impl/convolution.hpp
@@ -0,0 +1,153 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_CONVOLUTION_IMPL_HPP
+#define PCL_2D_CONVOLUTION_IMPL_HPP
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Convolution<PointT>::filter (pcl::PointCloud<PointT> &output)
+{
+  int input_row = 0;
+  int input_col = 0;
+  // default boundary option : zero padding
+  output = *input_;
+
+  int iw = static_cast<int> (input_->width),
+      ih = static_cast<int> (input_->height),
+      kw = static_cast<int> (kernel_.width),
+      kh = static_cast<int> (kernel_.height);
+  switch (boundary_options_)
+  {
+    default:
+    case BOUNDARY_OPTION_CLAMP:
+    {
+      for (int i = 0; i < ih; i++)
+      {
+        for (int j = 0; j < iw; j++)
+        {
+          float intensity = 0;
+          for (int k = 0; k < kh; k++)
+          {
+            for (int l = 0; l < kw; l++)
+            {
+              int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2;
+              if (ikkh < 0)
+                input_row = 0;
+              else if (ikkh >= ih)
+                input_row = ih - 1;
+              else
+                input_row = ikkh;
+
+              if (jlkw < 0)
+                input_col = 0;
+              else if (jlkw >= iw)
+                input_col = iw - 1;
+              else
+                input_col = jlkw;
+
+              intensity += kernel_ (l, k).intensity * (*input_)(input_col, input_row).intensity;
+            }
+          }
+          output (j, i).intensity = intensity;
+        }
+      }
+      break;
+    }
+
+    case BOUNDARY_OPTION_MIRROR:
+    {
+      for (int i = 0; i < ih; i++)
+      {
+        for (int j = 0; j < iw; j++)
+        {
+          float intensity = 0;
+          for (int k = 0; k < kh; k++)
+          {
+            for (int l = 0; l < kw; l++)
+            {
+              int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2;
+              if (ikkh < 0)
+                input_row = -ikkh - 1;
+              else if (ikkh >= ih)
+                input_row = 2 * ih - 1 - ikkh;
+              else
+                input_row = ikkh;
+
+              if (jlkw < 0)
+                input_col = -jlkw - 1;
+              else if (jlkw >= iw)
+                input_col = 2 * iw - 1 - jlkw;
+              else
+                input_col = jlkw;
+
+              intensity += kernel_ (l, k).intensity * ((*input_)(input_col, input_row).intensity);
+            }
+          }
+          output (j, i).intensity = intensity;
+        }
+      }
+      break;
+    }
+    
+    case BOUNDARY_OPTION_ZERO_PADDING:
+    {
+      for (int i = 0; i < ih; i++)
+      {
+        for (int j = 0; j < iw; j++)
+        {
+          float intensity = 0;
+          for (int k = 0; k < kh; k++)
+          {
+            for (int l = 0; l < kw; l++)
+            {
+              int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2;
+              if (ikkh < 0 || ikkh >= ih || jlkw < 0 || jlkw >= iw)
+                continue;
+              else
+                intensity += kernel_ (l, k).intensity * ((*input_)(jlkw, ikkh).intensity);
+            }
+          }
+          output (j, i).intensity = intensity;
+        }
+      }
+      break;
+    }
+  }  // switch
+}
+
+#endif
diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/edge.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/edge.hpp
new file mode 100644
index 00000000..07e0afa3
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/impl/edge.hpp
@@ -0,0 +1,494 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_EDGE_IMPL_HPP
+#define PCL_2D_EDGE_IMPL_HPP
+
+#include "../convolution.h"
+#include <pcl/common/common_headers.h> // rad2deg()
+#include <pcl/console/time.h>
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgeSobel (
+    pcl::PointCloud<PointOutT> &output)
+{
+  //pcl::console::TicToc tt;
+  //tt.tic ();
+  convolution_.setInputCloud (input_);
+  pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
+  pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::SOBEL_X);
+  kernel_.fetchKernel (*kernel_x);
+  convolution_.setKernel (*kernel_x);
+  convolution_.filter (*magnitude_x);
+  //PCL_ERROR ("Convolve X: %g\n", tt.toc ()); tt.tic ();
+
+  pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
+  pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::SOBEL_Y);
+  kernel_.fetchKernel (*kernel_y);
+  convolution_.setKernel (*kernel_y);
+  convolution_.filter (*magnitude_y);
+  //PCL_ERROR ("Convolve Y: %g\n", tt.toc ()); tt.tic ();
+
+  const int height = input_->height;
+  const int width = input_->width;
+
+  output.resize (height * width);
+  output.height = height;
+  output.width = width;
+
+  for (size_t i = 0; i < output.size (); ++i)
+  {
+    output[i].magnitude_x = (*magnitude_x)[i].intensity;
+    output[i].magnitude_y = (*magnitude_y)[i].intensity;
+    output[i].magnitude = 
+      sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + 
+             (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
+    output[i].direction = 
+      atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
+  }
+  //PCL_ERROR ("Rest: %g\n", tt.toc ());
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::sobelMagnitudeDirection (
+    const pcl::PointCloud<PointInT> &input_x, 
+    const pcl::PointCloud<PointInT> &input_y,
+    pcl::PointCloud<PointOutT> &output)
+{
+  convolution_.setInputCloud (input_x.makeShared());
+  pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
+  pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::SOBEL_X);
+  kernel_.fetchKernel (*kernel_x);
+  convolution_.setKernel (*kernel_x);
+  convolution_.filter (*magnitude_x);
+
+  convolution_.setInputCloud (input_y.makeShared());
+  pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
+  pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::SOBEL_Y);
+  kernel_.fetchKernel (*kernel_y);
+  convolution_.setKernel (*kernel_y);
+  convolution_.filter (*magnitude_y);
+
+  const int height = input_x.height;
+  const int width = input_x.width;
+
+  output.resize (height * width);
+  output.height = height;
+  output.width = width;
+
+  for (size_t i = 0; i < output.size (); ++i)
+  {
+    output[i].magnitude_x = (*magnitude_x)[i].intensity;
+    output[i].magnitude_y = (*magnitude_y)[i].intensity;
+    output[i].magnitude = 
+      sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + 
+             (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
+    output[i].direction = 
+      atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgePrewitt (pcl::PointCloud<PointOutT> &output)
+{
+  convolution_.setInputCloud (input_);
+
+  pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
+  pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::PREWITT_X);
+  kernel_.fetchKernel (*kernel_x);
+  convolution_.setKernel (*kernel_x);
+  convolution_.filter (*magnitude_x);
+
+  pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
+  pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::PREWITT_Y);
+  kernel_.fetchKernel (*kernel_y);
+  convolution_.setKernel (*kernel_y);
+  convolution_.filter (*magnitude_y);
+
+  const int height = input_->height;
+  const int width = input_->width;
+
+  output.resize (height * width);
+  output.height = height;
+  output.width = width;
+
+  for (size_t i = 0; i < output.size (); ++i)
+  {
+    output[i].magnitude_x = (*magnitude_x)[i].intensity;
+    output[i].magnitude_y = (*magnitude_y)[i].intensity;
+    output[i].magnitude = 
+      sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + 
+             (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
+    output[i].direction = 
+      atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgeRoberts (pcl::PointCloud<PointOutT> &output)
+{
+  convolution_.setInputCloud (input_);
+
+  pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
+  pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::ROBERTS_X);
+  kernel_.fetchKernel (*kernel_x);
+  convolution_.setKernel (*kernel_x);
+  convolution_.filter (*magnitude_x);
+
+  pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
+  pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::ROBERTS_Y);
+  kernel_.fetchKernel (*kernel_y);
+  convolution_.setKernel (*kernel_y);
+  convolution_.filter (*magnitude_y);
+
+  const int height = input_->height;
+  const int width = input_->width;
+
+  output.resize (height * width);
+  output.height = height;
+  output.width = width;
+
+  for (size_t i = 0; i < output.size (); ++i)
+  {
+    output[i].magnitude_x = (*magnitude_x)[i].intensity;
+    output[i].magnitude_y = (*magnitude_y)[i].intensity;
+    output[i].magnitude = 
+      sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + 
+             (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
+    output[i].direction = 
+      atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::cannyTraceEdge (
+    int rowOffset, int colOffset, int row, int col, 
+    pcl::PointCloud<PointXYZI> &maxima)
+{
+  int newRow = row + rowOffset;
+  int newCol = col + colOffset;
+  PointXYZI &pt = maxima (newCol, newRow);
+
+  if (newRow > 0 && newRow < static_cast<int> (maxima.height) && newCol > 0 && newCol < static_cast<int> (maxima.width))
+  {
+    if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits<float>::max ())
+      return;
+
+    pt.intensity = std::numeric_limits<float>::max ();
+    cannyTraceEdge ( 1, 0, newRow, newCol, maxima);
+    cannyTraceEdge (-1, 0, newRow, newCol, maxima);
+    cannyTraceEdge ( 1, 1, newRow, newCol, maxima);
+    cannyTraceEdge (-1, -1, newRow, newCol, maxima);
+    cannyTraceEdge ( 0, -1, newRow, newCol, maxima);
+    cannyTraceEdge ( 0, 1, newRow, newCol, maxima);
+    cannyTraceEdge (-1, 1, newRow, newCol, maxima);
+    cannyTraceEdge ( 1, -1, newRow, newCol, maxima);
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::discretizeAngles (pcl::PointCloud<PointOutT> &thet)
+{
+  const int height = thet.height;
+  const int width = thet.width;
+  float angle;
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      angle = pcl::rad2deg (thet (j, i).direction);
+      if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) || (angle <= -157.5))
+        thet (j, i).direction = 0;
+      else
+        if (((angle > 22.5) && (angle < 67.5)) || ((angle < -112.5) && (angle > -157.5)))
+          thet (j, i).direction = 45;
+        else
+          if (((angle >= 67.5) && (angle <= 112.5)) || ((angle <= -67.5) && (angle >= -112.5)))
+            thet (j, i).direction = 90;
+          else
+            if (((angle > 112.5) && (angle < 157.5)) || ((angle < -22.5) && (angle > -67.5)))
+              thet (j, i).direction = 135;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::suppressNonMaxima (
+    const pcl::PointCloud<PointXYZIEdge> &edges, 
+    pcl::PointCloud<PointXYZI> &maxima, float tLow)
+{
+  const int height = edges.height;
+  const int width = edges.width;
+
+  maxima.height = height;
+  maxima.width = width;
+  maxima.resize (height * width);
+
+  for (size_t i = 0; i < maxima.size (); ++i)
+    maxima[i].intensity = 0.0f;
+
+  // tHigh and non-maximal supression
+  for (int i = 1; i < height - 1; i++)
+  {
+    for (int j = 1; j < width - 1; j++)
+    {
+      const PointXYZIEdge &ptedge = edges (j, i);
+      PointXYZI &ptmax = maxima (j, i);
+
+      if (ptedge.magnitude < tLow)
+        continue;
+
+      //maxima (j, i).intensity = 0;
+      
+      switch (int (ptedge.direction))
+      {
+        case 0:
+        {
+          if (ptedge.magnitude >= edges (j - 1, i).magnitude && 
+              ptedge.magnitude >= edges (j + 1, i).magnitude)
+            ptmax.intensity = ptedge.magnitude;
+          break;
+        }
+        case 45:
+        {
+          if (ptedge.magnitude >= edges (j - 1, i - 1).magnitude && 
+              ptedge.magnitude >= edges (j + 1, i + 1).magnitude)
+            ptmax.intensity = ptedge.magnitude;
+          break;
+        }
+        case 90:
+        {
+          if (ptedge.magnitude >= edges (j, i - 1).magnitude && 
+              ptedge.magnitude >= edges (j, i + 1).magnitude)
+            ptmax.intensity = ptedge.magnitude;
+          break;
+        }
+        case 135:
+        {
+          if (ptedge.magnitude >= edges (j + 1, i - 1).magnitude && 
+              ptedge.magnitude >= edges (j - 1, i + 1).magnitude)
+            ptmax.intensity = ptedge.magnitude;
+          break;
+        }
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgeCanny (pcl::PointCloud<PointOutT> &output)
+{
+  float tHigh = hysteresis_threshold_high_;
+  float tLow = hysteresis_threshold_low_;
+  const int height = input_->height;
+  const int width = input_->width;
+
+  output.resize (height * width);
+  output.height = height;
+  output.width = width;
+
+  //pcl::console::TicToc tt;
+  //tt.tic ();
+  
+  // Noise reduction using gaussian blurring
+  pcl::PointCloud<PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<PointXYZI>);
+  PointCloudInPtr smoothed_cloud (new PointCloudIn);
+  kernel_.setKernelSize (3);
+  kernel_.setKernelSigma (1.0);
+  kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
+  kernel_.fetchKernel (*gaussian_kernel);
+  convolution_.setKernel (*gaussian_kernel);
+  convolution_.setInputCloud (input_);
+  convolution_.filter (*smoothed_cloud);
+  //PCL_ERROR ("Gaussian blur: %g\n", tt.toc ()); tt.tic ();
+  
+  // Edge detection usign Sobel
+  pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
+  setInputCloud (smoothed_cloud);
+  detectEdgeSobel (*edges);
+  //PCL_ERROR ("Sobel: %g\n", tt.toc ()); tt.tic ();
+  
+  // Edge discretization
+  discretizeAngles (*edges);
+  //PCL_ERROR ("Discretize: %g\n", tt.toc ()); tt.tic ();
+
+  // tHigh and non-maximal supression
+  pcl::PointCloud<PointXYZI>::Ptr maxima (new pcl::PointCloud<PointXYZI>);
+  suppressNonMaxima (*edges, *maxima, tLow);
+  //PCL_ERROR ("NM suppress: %g\n", tt.toc ()); tt.tic ();
+
+  // Edge tracing
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
+        continue;
+
+      (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
+      cannyTraceEdge ( 1, 0, i, j, *maxima);
+      cannyTraceEdge (-1, 0, i, j, *maxima);
+      cannyTraceEdge ( 1, 1, i, j, *maxima);
+      cannyTraceEdge (-1, -1, i, j, *maxima);
+      cannyTraceEdge ( 0, -1, i, j, *maxima);
+      cannyTraceEdge ( 0, 1, i, j, *maxima);
+      cannyTraceEdge (-1, 1, i, j, *maxima);
+      cannyTraceEdge ( 1, -1, i, j, *maxima);
+    }
+  }
+  //PCL_ERROR ("Edge tracing: %g\n", tt.toc ());
+
+  // Final thresholding
+  for (size_t i = 0; i < input_->size (); ++i)
+  {
+    if ((*maxima)[i].intensity == std::numeric_limits<float>::max ())
+      output[i].magnitude = 255;
+    else
+      output[i].magnitude = 0;
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::canny (
+    const pcl::PointCloud<PointInT> &input_x, 
+    const pcl::PointCloud<PointInT> &input_y,
+    pcl::PointCloud<PointOutT> &output)
+{
+  float tHigh = hysteresis_threshold_high_;
+  float tLow = hysteresis_threshold_low_;
+  const int height = input_x.height;
+  const int width = input_x.width;
+
+  output.resize (height * width);
+  output.height = height;
+  output.width = width;
+
+  // Noise reduction using gaussian blurring
+  pcl::PointCloud<PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelSize (3);
+  kernel_.setKernelSigma (1.0);
+  kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
+  kernel_.fetchKernel (*gaussian_kernel);
+  convolution_.setKernel (*gaussian_kernel);
+
+  PointCloudIn smoothed_cloud_x;
+  convolution_.setInputCloud (input_x.makeShared());
+  convolution_.filter (smoothed_cloud_x);
+
+  PointCloudIn smoothed_cloud_y;
+  convolution_.setInputCloud (input_y.makeShared());
+  convolution_.filter (smoothed_cloud_y);
+
+
+  // Edge detection usign Sobel
+  pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
+  sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ());
+
+  // Edge discretization
+  discretizeAngles (*edges);
+
+  pcl::PointCloud<PointXYZI>::Ptr maxima (new pcl::PointCloud<PointXYZI>);
+  suppressNonMaxima (*edges, *maxima, tLow);
+
+  // Edge tracing
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
+        continue;
+
+      (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
+      cannyTraceEdge ( 1, 0, i, j, *maxima);
+      cannyTraceEdge (-1, 0, i, j, *maxima);
+      cannyTraceEdge ( 1, 1, i, j, *maxima);
+      cannyTraceEdge (-1, -1, i, j, *maxima);
+      cannyTraceEdge ( 0, -1, i, j, *maxima);
+      cannyTraceEdge ( 0, 1, i, j, *maxima);
+      cannyTraceEdge (-1, 1, i, j, *maxima);
+      cannyTraceEdge ( 1, -1, i, j, *maxima);
+    }
+  }
+
+  // Final thresholding
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
+        output (j, i).magnitude = 255;
+      else
+        output (j, i).magnitude = 0;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgeLoG (
+    const float kernel_sigma, const float kernel_size,
+    pcl::PointCloud<PointOutT> &output)
+{
+  convolution_.setInputCloud (input_);
+
+  pcl::PointCloud<PointXYZI>::Ptr log_kernel (new pcl::PointCloud<PointXYZI>);
+  kernel_.setKernelType (kernel<PointXYZI>::LOG);
+  kernel_.setKernelSigma (kernel_sigma);
+  kernel_.setKernelSize (kernel_size);
+  kernel_.fetchKernel (*log_kernel);
+  convolution_.setKernel (*log_kernel);
+  convolution_.filter (output);
+}
+
+#endif
diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/kernel.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/kernel.hpp
new file mode 100644
index 00000000..223fba8c
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/impl/kernel.hpp
@@ -0,0 +1,328 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_KERNEL_IMPL_HPP
+#define PCL_2D_KERNEL_IMPL_HPP
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::fetchKernel (pcl::PointCloud<PointT> &kernel)
+{
+  switch (kernel_type_)
+  {
+    case SOBEL_X:
+    {
+      sobelKernelX (kernel);
+      break;
+    }
+    case SOBEL_Y:
+    {
+      sobelKernelY (kernel);
+      break;
+    }
+    case PREWITT_X:
+    {
+      prewittKernelX (kernel);
+      break;
+    }
+    case PREWITT_Y:
+    {
+      prewittKernelY (kernel);
+      break;
+    }
+    case ROBERTS_X:
+    {
+      robertsKernelX (kernel);
+      break;
+    }
+    case ROBERTS_Y:
+    {
+      robertsKernelY (kernel);
+      break;
+    }
+    case LOG:
+    {
+      loGKernel (kernel);
+      break;
+    }
+    case DERIVATIVE_CENTRAL_X:
+    {
+      derivativeXCentralKernel (kernel);
+      break;
+    }
+    case DERIVATIVE_FORWARD_X:
+    {
+      derivativeXForwardKernel (kernel);
+      break;
+    }
+    case DERIVATIVE_BACKWARD_X:
+    {
+      derivativeXBackwardKernel (kernel);
+      break;
+    }
+    case DERIVATIVE_CENTRAL_Y:
+    {
+      derivativeYCentralKernel (kernel);
+      break;
+    }
+    case DERIVATIVE_FORWARD_Y:
+    {
+      derivativeYForwardKernel (kernel);
+      break;
+    }
+    case DERIVATIVE_BACKWARD_Y:
+    {
+      derivativeYBackwardKernel (kernel);
+      break;
+    }
+    case GAUSSIAN:
+    {
+      gaussianKernel (kernel);
+      break;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::gaussianKernel (pcl::PointCloud<PointT> &kernel)
+{
+  float sum = 0;
+  kernel.resize (kernel_size_ * kernel_size_);
+  kernel.height = kernel_size_;
+  kernel.width = kernel_size_;
+
+  double sigma_sqr = 2 * sigma_ * sigma_;
+
+  for (int i = 0; i < kernel_size_; i++)
+  {
+    for (int j = 0; j < kernel_size_; j++)
+    {
+      int iks = (i - kernel_size_ / 2);
+      int jks = (j - kernel_size_ / 2);
+      kernel (j, i).intensity = expf (float (- double (iks * iks + jks * jks) / sigma_sqr));
+      sum += float (kernel (j, i).intensity);
+    }
+  }
+
+  // Normalizing the kernel
+  for (size_t i = 0; i < kernel.size (); ++i)
+    kernel[i].intensity /= sum;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT> void
+pcl::kernel<PointT>::loGKernel (pcl::PointCloud<PointT> &kernel)
+{
+  float sum = 0;
+  float temp = 0;
+  kernel.resize (kernel_size_ * kernel_size_);
+  kernel.height = kernel_size_;
+  kernel.width = kernel_size_;
+
+  double sigma_sqr = 2 * sigma_ * sigma_;
+  
+  for (int i = 0; i < kernel_size_; i++)
+  {
+    for (int j = 0; j < kernel_size_; j++)
+    {
+      int iks = (i - kernel_size_ / 2); 
+      int jks = (j - kernel_size_ / 2); 
+      temp = float (double (iks * iks  + jks * jks) / sigma_sqr);
+      kernel (j, i).intensity = (1.0f - temp) * expf (-temp);
+      sum += kernel (j, i).intensity;
+    }
+  }
+
+  // Normalizing the kernel
+  for (size_t i = 0; i < kernel.size (); ++i)
+    kernel[i].intensity /= sum;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::sobelKernelX (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (9);
+  kernel.height = 3;
+  kernel.width = 3;
+  kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1;
+  kernel (0, 1).intensity = -2; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 2;
+  kernel (0, 2).intensity = -1; kernel (1, 2).intensity = 0; kernel (2, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::prewittKernelX (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (9);
+  kernel.height = 3;
+  kernel.width = 3;
+  kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1;
+  kernel (0, 1).intensity = -1; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 1;
+  kernel (0, 2).intensity = -1; kernel (1, 2).intensity = 0; kernel (2, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::robertsKernelX (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (4);
+  kernel.height = 2;
+  kernel.width = 2;
+  kernel (0, 0).intensity = 1; kernel (1, 0).intensity = 0;
+  kernel (0, 1).intensity = 0; kernel (1, 1).intensity = -1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::sobelKernelY (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (9);
+  kernel.height = 3;
+  kernel.width = 3;
+  kernel (0, 0).intensity = -1; kernel (1, 0).intensity = -2; kernel (2, 0).intensity = -1;
+  kernel (0, 1).intensity = 0; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 0;
+  kernel (0, 2).intensity = 1; kernel (1, 2).intensity = 2; kernel (2, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::prewittKernelY (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (9);
+  kernel.height = 3;
+  kernel.width = 3;
+  kernel (0, 0).intensity = 1; kernel (1, 0).intensity = 1; kernel (2, 0).intensity = 1;
+  kernel (0, 1).intensity = 0; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 0;
+  kernel (0, 2).intensity = -1; kernel (1, 2).intensity = -1; kernel (2, 2).intensity = -1;
+}
+
+template <typename PointT> void
+pcl::kernel<PointT>::robertsKernelY (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (4);
+  kernel.height = 2;
+  kernel.width = 2;
+  kernel (0, 0).intensity = 0; kernel (1, 0).intensity = 1;
+  kernel (0, 1).intensity = -1; kernel (1, 1).intensity = 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeXCentralKernel (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (3);
+  kernel.height = 1;
+  kernel.width = 3;
+  kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeXForwardKernel (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (3);
+  kernel.height = 1;
+  kernel.width = 3;
+  kernel (0, 0).intensity = 0; kernel (1, 0).intensity = -1; kernel (2, 0).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeXBackwardKernel (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (3);
+  kernel.height = 1;
+  kernel.width = 3;
+  kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 1; kernel (2, 0).intensity = 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeYCentralKernel (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (3);
+  kernel.height = 3;
+  kernel.width = 1;
+  kernel (0, 0).intensity = -1; kernel (0, 1).intensity = 0; kernel (0, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeYForwardKernel (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (3);
+  kernel.height = 3;
+  kernel.width = 1;
+  kernel (0, 0).intensity = 0; kernel (0, 1).intensity = -1; kernel (0, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeYBackwardKernel (pcl::PointCloud<PointT> &kernel)
+{
+  kernel.resize (3);
+  kernel.height = 3;
+  kernel.width = 1;
+  kernel (0, 0).intensity = -1; kernel (0, 1).intensity = 1; kernel (0, 2).intensity = 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::setKernelType (KERNEL_ENUM kernel_type)
+{
+  kernel_type_ = kernel_type;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::setKernelSize (int kernel_size)
+{
+  kernel_size_ = kernel_size;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::setKernelSigma (float kernel_sigma)
+{
+  sigma_ = kernel_sigma;
+}
+
+
+#endif
diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/keypoint.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/keypoint.hpp
new file mode 100644
index 00000000..f0417dc3
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/impl/keypoint.hpp
@@ -0,0 +1,247 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  keypoint.hpp
+ *
+ *  Created on: May 28, 2012
+ *      Author: somani
+ */
+
+#ifndef PCL_2D_KEYPOINT_HPP_
+#define PCL_2D_KEYPOINT_HPP_
+
+#include "../edge.h"
+#include "../convolution.h"
+#include <limits>
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoint::harrisCorner (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh){
+
+  /*creating the gaussian kernels*/
+  ImageType kernel_d;
+  ImageType kernel_i;
+  conv_2d.gaussianKernel  (5, sigma_d, kernel_d);
+  conv_2d.gaussianKernel  (5, sigma_i, kernel_i);
+
+  /*scaling the image with differentiation scale*/
+  ImageType smoothed_image;
+  conv_2d.convolve  (smoothed_image, kernel_d, input);
+
+  /*image derivatives*/
+  ImageType I_x, I_y;
+  edge_detection.ComputeDerivativeXCentral  (I_x, smoothed_image);
+  edge_detection.ComputeDerivativeYCentral  (I_y, smoothed_image);
+
+  /*second moment matrix*/
+  ImageType I_x2, I_y2, I_xI_y;
+  imageElementMultiply  (I_x2, I_x, I_x);
+  imageElementMultiply  (I_y2, I_y, I_y);
+  imageElementMultiply  (I_xI_y, I_x, I_y);
+
+  /*scaling second moment matrix with integration scale*/
+  ImageType M00, M10, M11;
+  conv_2d.convolve  (M00, kernel_i, I_x2);
+  conv_2d.convolve  (M10, kernel_i, I_xI_y);
+  conv_2d.convolve  (M11, kernel_i, I_y2);
+
+  /*harris function*/
+  const size_t height = input.size ();
+  const size_t width = input[0].size ();
+  output.resize (height);
+  for (size_t i = 0; i < height; i++)
+  {
+    output[i].resize (width);
+    for (size_t j = 0; j < width; j++)
+    {
+      output[i][j] = M00[i][j] * M11[i][j] - (M10[i][j] * M10[i][j]) - alpha * ((M00[i][j] + M11[i][j]) * (M00[i][j] + M11[i][j]));
+      if (thresh != 0)
+      {
+        if (output[i][j] < thresh)
+          output[i][j] = 0;
+        else
+          output[i][j] = 255;
+      }
+    }
+  }
+
+  /*local maxima*/
+  for (size_t i = 1; i < height - 1; i++)
+  {
+    for (size_t j = 1; j < width - 1; j++)
+    {
+      if (output[i][j] > output[i - 1][j - 1] && output[i][j] > output[i - 1][j] && output[i][j] > output[i - 1][j + 1] &&
+          output[i][j] > output[i][j - 1] && output[i][j] > output[i][j + 1] &&
+          output[i][j] > output[i + 1][j - 1] && output[i][j] > output[i + 1][j] && output[i][j] > output[i + 1][j + 1])
+        ;
+      else
+        output[i][j] = 0;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float sigma, bool SCALED){
+  /*creating the gaussian kernels*/
+  ImageType kernel, cornerness;
+  conv_2d.gaussianKernel  (5, sigma, kernel);
+
+  /*scaling the image with differentiation scale*/
+  ImageType smoothed_image;
+  conv_2d.convolve  (smoothed_image, kernel, input);
+
+  /*image derivatives*/
+  ImageType I_x, I_y;
+  edge_detection.ComputeDerivativeXCentral  (I_x, smoothed_image);
+  edge_detection.ComputeDerivativeYCentral  (I_y, smoothed_image);
+
+  /*second moment matrix*/
+  ImageType I_xx, I_yy, I_xy;
+  edge_detection.ComputeDerivativeXCentral  (I_xx, I_x);
+  edge_detection.ComputeDerivativeYCentral  (I_xy, I_x);
+  edge_detection.ComputeDerivativeYCentral  (I_yy, I_y);
+  /*Determinant of Hessian*/
+  const size_t height = input.size ();
+  const size_t width = input[0].size ();
+  float min = std::numeric_limits<float>::max();
+  float max = std::numeric_limits<float>::min();
+  cornerness.resize (height);
+  for (size_t i = 0; i < height; i++)
+  {
+    cornerness[i].resize (width);
+    for (size_t j = 0; j < width; j++)
+    {
+      cornerness[i][j] = sigma*sigma*(I_xx[i][j]+I_yy[i][j]-I_xy[i][j]*I_xy[i][j]);
+      if(SCALED){
+        if(cornerness[i][j]  < min)
+          min = cornerness[i][j];
+        if(cornerness[i][j] > max)
+          max = cornerness[i][j];
+      }
+    }
+
+    /*local maxima*/
+    output.resize (height);
+    output[0].resize (width);
+    output[height-1].resize (width);
+    for (size_t i = 1; i < height - 1; i++)
+    {
+      output[i].resize (width);
+      for (size_t j = 1; j < width - 1; j++)
+      {
+        if(SCALED)
+          output[i][j] = ((cornerness[i][j]-min)/(max-min));
+        else
+          output[i][j] = cornerness[i][j];
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales){
+  const size_t height = input.size();
+  const size_t width = input[0].size();
+  const int local_search_radius = 1;
+  float scale = start_scale;
+  std::vector<ImageType> cornerness;
+  cornerness.resize(num_scales);
+  for(int i = 0;i < num_scales;i++){
+    hessianBlob(cornerness[i], input, scale, false);
+    scale *= scaling_factor;
+  }
+  bool non_max_flag = false;
+  float scale_max, local_max;
+  for(size_t i = 0;i < height;i++){
+    for(size_t j = 0;j < width;j++){
+      scale_max = std::numeric_limits<float>::min();
+      /*default output in case of no blob at the current point is 0*/
+      output[i][j] = 0;
+      for(int k = 0;k < num_scales;k++){
+        /*check if the current point (k,i,j) is a maximum in the defined search radius*/
+        non_max_flag = false;
+        local_max = cornerness[k][i][j];
+        for(int n = -local_search_radius; n <= local_search_radius;n++){
+          if(n+k < 0 || n+k >= num_scales)
+            continue;
+          for(int l = -local_search_radius;l <= local_search_radius;l++){
+            if(l+i < 0 || l+i >= height)
+              continue;
+            for(int m = -local_search_radius; m <= local_search_radius;m++){
+              if(m+j < 0 || m+j >= width)
+                continue;
+              if(cornerness[n+k][l+i][m+j] > local_max){
+                non_max_flag = true;
+                break;
+              }
+            }
+            if(non_max_flag)
+              break;
+          }
+          if(non_max_flag)
+            break;
+        }
+        /*if the current point is a point of local maximum, check if it is a maximum point across scales*/
+        if(!non_max_flag){
+          if(cornerness[k][i][j] > scale_max){
+            scale_max = cornerness[k][i][j];
+            /*output indicates the scale at which the blob is found at the current location in the image*/
+            output[i][i] = start_scale*pow(scaling_factor, k);
+          }
+        }
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoint::imageElementMultiply (ImageType &output, ImageType &input1, ImageType &input2){
+  const size_t height = input1.size ();
+  const size_t width = input1[0].size ();
+  output.resize (height);
+  for (size_t i = 0; i < height; i++)
+  {
+    output[i].resize (width);
+    for (size_t j = 0; j < width; j++)
+    {
+      output[i][j] = input1[i][j] * input2[i][j];
+    }
+  }
+}
+
+#endif  // PCL_2D_KEYPOINT_HPP_
diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/morphology.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/morphology.hpp
new file mode 100644
index 00000000..6528eed9
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/impl/morphology.hpp
@@ -0,0 +1,380 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_MORPHOLOGY_HPP_
+#define PCL_2D_MORPHOLOGY_HPP_
+
+//////////////////////////////////////////////////////////////////////////////
+// Assumes input, kernel and output images have 0's and 1's only
+template<typename PointT> void
+pcl::Morphology<PointT>::erosionBinary (pcl::PointCloud<PointT> &output)
+{
+  const int height = input_->height;
+  const int width = input_->width;
+  const int kernel_height = structuring_element_->height;
+  const int kernel_width = structuring_element_->width;
+  bool mismatch_flag;
+
+  output.width = width;
+  output.height = height;
+  output.resize (width * height);
+
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      // Operation done only at 1's
+      if ((*input_)(j, i).intensity == 0)
+      {
+        output (j, i).intensity = 0;
+        continue;
+      }
+      mismatch_flag = false;
+      for (int k = 0; k < kernel_height; k++)
+      {
+        if (mismatch_flag)
+          break;
+        for (int l = 0; l < kernel_width; l++)
+        {
+          // We only check for 1's in the kernel
+          if ((*structuring_element_)(l, k).intensity == 0)
+            continue;
+          if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width)
+          {
+            continue;
+          }
+          // If one of the elements of the kernel and image dont match, 
+          // the output image is 0. So, move to the next point.
+          if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity != 1)
+          {
+            output (j, i).intensity = 0;
+            mismatch_flag = true;
+            break;
+          }
+        }
+      }
+      // Assign value according to mismatch flag
+      output (j, i).intensity = (mismatch_flag) ? 0 : 1;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Assumes input, kernel and output images have 0's and 1's only
+template <typename PointT> void
+pcl::Morphology<PointT>::dilationBinary (pcl::PointCloud<PointT> &output)
+{
+  const int height = input_->height;
+  const int width = input_->width;
+  const int kernel_height = structuring_element_->height;
+  const int kernel_width = structuring_element_->width;
+  bool match_flag;
+
+  output.width = width;
+  output.height = height;
+  output.resize (width * height);
+
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      match_flag = false;
+      for (int k = 0; k < kernel_height; k++)
+      {
+        if (match_flag)
+          break;
+        for (int l = 0; l < kernel_width; l++)
+        {
+          // We only check for 1's in the kernel
+          if ((*structuring_element_)(l, k).intensity == 0)
+            continue;
+          if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= height)
+          {
+            continue;
+          }
+          // If any position where kernel is 1 and image is also one is detected, 
+          // matching occurs
+          if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity == 1)
+          {
+            match_flag = true;
+            break;
+          }
+        }
+      }
+      // Assign value according to match flag
+      output (j, i).intensity = (match_flag) ? 1 : 0;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Assumes input, kernel and output images have 0's and 1's only
+template <typename PointT> void
+pcl::Morphology<PointT>::openingBinary (pcl::PointCloud<PointT> &output)
+{
+  PointCloudInPtr intermediate_output (new PointCloudIn);
+  erosionBinary (*intermediate_output);
+  this->setInputCloud (intermediate_output);
+  dilationBinary (output);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Assumes input, kernel and output images have 0's and 1's only
+template <typename PointT> void
+pcl::Morphology<PointT>::closingBinary (pcl::PointCloud<PointT> &output)
+{
+  PointCloudInPtr intermediate_output (new PointCloudIn);
+  dilationBinary (*intermediate_output);
+  this->setInputCloud (intermediate_output);
+  erosionBinary (output);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::erosionGray (pcl::PointCloud<PointT> &output)
+{
+  const int height = input_->height;
+  const int width = input_->width;
+  const int kernel_height = structuring_element_->height;
+  const int kernel_width = structuring_element_->width;
+  float min;
+  output.resize (width * height);
+  output.width = width;
+  output.height = height;
+
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      min = -1;
+      for (int k = 0; k < kernel_height; k++)
+      {
+        for (int l = 0; l < kernel_width; l++)
+        {
+          // We only check for 1's in the kernel
+          if ((*structuring_element_)(l, k).intensity == 0)
+            continue;
+          if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width)
+          {
+            continue;
+          }
+          // If one of the elements of the kernel and image dont match, 
+          // the output image is 0. So, move to the next point.
+          if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity < min || min == -1)
+          {
+            min = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity;
+          }
+        }
+      }
+      // Assign value according to mismatch flag
+      output (j, i).intensity = min;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::dilationGray (pcl::PointCloud<PointT> &output)
+{
+  const int height = input_->height;
+  const int width = input_->width;
+  const int kernel_height = structuring_element_->height;
+  const int kernel_width = structuring_element_->width;
+  float max;
+
+  output.resize (width * height);
+  output.width = width;
+  output.height = height;
+
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      max = -1;
+      for (int k = 0; k < kernel_height; k++)
+      {
+        for (int l = 0; l < kernel_width; l++)
+        {
+          // We only check for 1's in the kernel
+          if ((*structuring_element_)(l, k).intensity == 0)
+            continue;
+          if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width)
+          {
+            continue;
+          }
+          // If one of the elements of the kernel and image dont match, 
+          // the output image is 0. So, move to the next point.
+          if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity > max || max == -1)
+          {
+            max = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity;
+          }
+        }
+      }
+      // Assign value according to mismatch flag
+      output (j, i).intensity = max;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::openingGray (pcl::PointCloud<PointT> &output)
+{
+  PointCloudInPtr intermediate_output (new PointCloudIn);
+  erosionGray (*intermediate_output);
+  this->setInputCloud (intermediate_output);
+  dilationGray (output);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::closingGray (pcl::PointCloud<PointT> &output)
+{
+  PointCloudInPtr intermediate_output (new PointCloudIn);
+  dilationGray (*intermediate_output);
+  this->setInputCloud (intermediate_output);
+  erosionGray (output);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::subtractionBinary (
+    pcl::PointCloud<PointT> &output, 
+    const pcl::PointCloud<PointT> &input1, 
+    const pcl::PointCloud<PointT> &input2)
+{
+  const int height = (input1.height < input2.hieght) ? input1.height : input2.height;
+  const int width = (input1.width < input2.width) ? input1.width : input2.width;
+  output.width = width;
+  output.height = height;
+  output.resize (height * width);
+
+  for (size_t i = 0; i < output.size (); ++i)
+  {
+    if (input1[i].intensity == 1 && input2[i].intensity == 0)
+      output[i].intensity = 1;
+    else
+      output[i].intensity = 0;
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::unionBinary (
+    pcl::PointCloud<PointT> &output, 
+    const pcl::PointCloud<PointT> &input1, 
+    const pcl::PointCloud<PointT> &input2)
+{
+  const int height = (input1.height < input2.hieght) ? input1.height : input2.height;
+  const int width = (input1.width < input2.width) ? input1.width : input2.width;
+  output.width = width;
+  output.height = height;
+  output.resize (height * width);
+
+  for (size_t i = 0; i < output.size (); ++i)
+  {
+    if (input1[i].intensity == 1 || input2[i].intensity == 1)
+      output[i].intensity = 1;
+    else
+      output[i].intensity = 0;
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::intersectionBinary (
+    pcl::PointCloud<PointT> &output, 
+    const pcl::PointCloud<PointT> &input1, 
+    const pcl::PointCloud<PointT> &input2)
+{
+  const int height = (input1.height < input2.height) ? input1.height : input2.height;
+  const int width = (input1.width < input2.width) ? input1.width : input2.width;
+  output.width = width;
+  output.height = height;
+  output.resize (height * width);
+
+  for (size_t i = 0; i < output.size (); ++i)
+  {
+    if (input1[i].intensity == 1 && input2[i].intensity == 1)
+      output[i].intensity = 1;
+    else
+      output[i].intensity = 0;
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::structuringElementCircular (
+    pcl::PointCloud<PointT> &kernel, const int radius)
+{
+  const int dim = 2 * radius;
+  kernel.height = dim;
+  kernel.width = dim;
+  kernel.resize (dim * dim);
+
+  for (int i = 0; i < dim; i++)
+  {
+    for (int j = 0; j < dim; j++)
+    {
+      if (((i - radius) * (i - radius) + (j - radius) * (j - radius)) < radius * radius)
+        kernel (j, i).intensity = 1;
+      else
+        kernel (j, i).intensity = 0;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::structuringElementRectangle (
+    pcl::PointCloud<PointT> &kernel, const int height, const int width)
+{
+  kernel.height = height;
+  kernel.width = width;
+  kernel.resize (height * width);
+  for (size_t i = 0; i < kernel.size (); ++i)
+    kernel[i].intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::setStructuringElement (const PointCloudInPtr &structuring_element)
+{
+  structuring_element_ = structuring_element;
+}
+
+#endif    // PCL_2D_MORPHOLOGY_HPP_
diff --git a/quasimodo/quasimodo_models/src/core/2d/kernel.h b/quasimodo/quasimodo_models/src/core/2d/kernel.h
new file mode 100644
index 00000000..1bf85e22
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/kernel.h
@@ -0,0 +1,244 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_KERNEL_H_
+#define PCL_2D_KERNEL_H_
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+namespace pcl
+{
+  template<typename PointT>
+  class kernel
+  {
+    public:
+
+      /**
+       * enumerates the different types of kernels available.
+       */
+      enum KERNEL_ENUM
+      {
+        SOBEL_X,              //!< SOBEL_X
+        SOBEL_Y,              //!< SOBEL_Y
+        PREWITT_X,            //!< PREWITT_X
+        PREWITT_Y,            //!< PREWITT_Y
+        ROBERTS_X,            //!< ROBERTS_X
+        ROBERTS_Y,            //!< ROBERTS_Y
+        LOG,                  //!< LOG
+        DERIVATIVE_CENTRAL_X, //!< DERIVATIVE_CENTRAL_X
+        DERIVATIVE_FORWARD_X, //!< DERIVATIVE_FORWARD_X
+        DERIVATIVE_BACKWARD_X,//!< DERIVATIVE_BACKWARD_X
+        DERIVATIVE_CENTRAL_Y, //!< DERIVATIVE_CENTRAL_Y
+        DERIVATIVE_FORWARD_Y, //!< DERIVATIVE_FORWARD_Y
+        DERIVATIVE_BACKWARD_Y,//!< DERIVATIVE_BACKWARD_Y
+        GAUSSIAN              //!< GAUSSIAN
+      };
+
+      int kernel_size_;
+      float sigma_;
+      KERNEL_ENUM kernel_type_;
+
+      kernel () :
+        kernel_size_ (3),
+        sigma_ (1.0),
+        kernel_type_ (GAUSSIAN)
+      {
+
+      }
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * Helper function which returns the kernel selected by the kernel_type_ enum
+       */
+      void fetchKernel (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_
+       */
+
+      void gaussianKernel (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * Laplacian of Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_
+       */
+
+      void loGKernel (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * 3x3 Sobel kernel in the X direction
+       */
+
+      void sobelKernelX (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * 3x3 Prewitt kernel in the X direction
+       */
+
+      void prewittKernelX (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * 2x2 Roberts kernel in the X direction
+       */
+
+      void robertsKernelX (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * 3x3 Sobel kernel in the Y direction
+       */
+
+      void sobelKernelY (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * 3x3 Prewitt kernel in the Y direction
+       */
+
+      void prewittKernelY (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * 2x2 Roberts kernel in the Y direction
+       */
+
+      void robertsKernelY (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * kernel [-1 0 1]
+       */
+
+      void derivativeXCentralKernel (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * kernel [-1 0 1]'
+       */
+
+      void derivativeYCentralKernel (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * kernel [0 -1 1]
+       */
+
+      void derivativeXForwardKernel (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * kernel [0 -1 1]'
+       */
+
+      void derivativeYForwardKernel (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * kernel [-1 1 0]
+       */
+
+      void derivativeXBackwardKernel (pcl::PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel Kernel point cloud passed by reference
+       *
+       * kernel [-1 1 0]'
+       */
+
+      void derivativeYBackwardKernel (PointCloud<PointT> &kernel);
+
+      /**
+       *
+       * @param kernel_type enum indicating the kernel type wanted
+       *
+       * select the kernel type.
+       */
+      void setKernelType (KERNEL_ENUM kernel_type);
+
+      /**
+       *
+       * @param kernel_size kernel of size kernel_size x kernel_size is created(LoG and Gaussian only)
+       *
+       * Setter function for kernel_size_
+       */
+      void setKernelSize (int kernel_size);
+
+      /**
+       *
+       * @param kernel_sigma variance of the Gaussian or LoG kernels.
+       *
+       * Setter function for kernel_sigma_
+       */
+      void setKernelSigma (float kernel_sigma);
+  };
+}
+
+#include "impl/kernel.hpp"
+
+#endif    // PCL_2D_KERNEL_H_
diff --git a/quasimodo/quasimodo_models/src/core/2d/keypoint.h b/quasimodo/quasimodo_models/src/core/2d/keypoint.h
new file mode 100644
index 00000000..7e6f6381
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/keypoint.h
@@ -0,0 +1,75 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  keypoint.h
+ *
+ *  Created on: May 28, 2012
+ *      Author: somani
+ */
+
+#ifndef PCL_2D_KEYPOINT_H_
+#define PCL_2D_KEYPOINT_H_
+
+#include "edge.h"
+
+namespace pcl
+{
+  class Keypoint
+  {
+    private:
+      Edge edge_detection;
+      Convolution conv_2d;
+    public:
+      Keypoint  ()
+      {
+      }
+      
+      void 
+      harrisCorner  (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh);
+      
+      void 
+      hessianBlob  (ImageType &output, ImageType &input, const float sigma, bool SCALE);
+      
+      void 
+      hessianBlob  (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales);
+
+      void 
+      imageElementMultiply  (ImageType &output, ImageType &input1, ImageType &input2);
+  };
+}
+
+#include "impl/keypoint.hpp"
+
+#endif    // PCL_2D_KEYPOINT_H_
diff --git a/quasimodo/quasimodo_models/src/core/2d/morphology.h b/quasimodo/quasimodo_models/src/core/2d/morphology.h
new file mode 100644
index 00000000..d96618fb
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/2d/morphology.h
@@ -0,0 +1,200 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_MORPHOLOGY_H_
+#define PCL_2D_MORPHOLOGY_H_
+
+#include <pcl/pcl_base.h>
+
+namespace pcl
+{
+  template <typename PointT>
+  class Morphology : public PCLBase<PointT>
+  {
+    private:
+      typedef typename pcl::PointCloud<PointT> PointCloudIn;
+      typedef typename PointCloudIn::Ptr PointCloudInPtr;
+
+      PointCloudInPtr structuring_element_;
+
+    public:
+      using PCLBase<PointT>::input_;
+
+      Morphology () {}
+
+      /** \brief This function performs erosion followed by dilation. 
+        * It is useful for removing noise in the form of small blobs and patches.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      openingBinary (pcl::PointCloud<PointT> &output);
+
+      /** \brief This function performs dilation followed by erosion. 
+        * It is useful for filling up (holes/cracks/small discontinuities) in a binary 
+        * segmented region
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      closingBinary (pcl::PointCloud<PointT> &output);
+
+      /** \brief Binary dilation is similar to a logical disjunction of sets. 
+        * At each pixel having value 1, if for all pixels in the structuring element  having value 1, the corresponding
+        * pixels in the input image are also 1, the center pixel is set to 1. Otherwise, it is set to 0.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      erosionBinary (pcl::PointCloud<PointT> &output);
+
+      /** \brief Binary erosion is similar to a logical addition of sets. 
+        * At each pixel having value 1, if at least one pixel in the structuring  element is 1 and the corresponding point
+        * in the input image is 1, the center pixel is set to 1. Otherwise, it is set to 0.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      dilationBinary  (pcl::PointCloud<PointT> &output);
+
+      /** \brief Grayscale erosion followed by dilation.
+        * This is used to remove small bright artifacts from the image. Large bright objects are relatively undisturbed.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      openingGray (pcl::PointCloud<PointT> &output);
+
+      /** \brief Grayscale dilation followed by erosion.
+        * This is used to remove small dark artifacts from the image. Bright features or large dark features are relatively undisturbed.
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      closingGray (pcl::PointCloud<PointT> &output);
+
+      /** \brief Takes the min of the pixels where kernel is 1
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      erosionGray (pcl::PointCloud<PointT> &output);
+
+      /** \brief Takes the max of the pixels where kernel is 1
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      dilationGray (pcl::PointCloud<PointT> &output);
+
+      /** \brief Set operation
+        * output = input1 - input2
+        * \param[out] output Output point cloud passed by reference
+        * \param[in] input1
+        * \param[in] input2
+        */
+      void 
+      subtractionBinary (pcl::PointCloud<PointT> &output, 
+                         const pcl::PointCloud<PointT> &input1, 
+                         const pcl::PointCloud<PointT> &input2);
+
+      /** \brief Set operation
+        * \f$output = input1 \cup input2\f$
+        * \param[out] output Output point cloud passed by reference
+        * \param[in] input1
+        * \param[in] input2
+        */
+      void 
+      unionBinary (pcl::PointCloud<PointT> &output, 
+                   const pcl::PointCloud<PointT> &input1, 
+                   const pcl::PointCloud<PointT> &input2);
+
+      /** \brief Set operation \f$ output = input1 \cap input2 \f$
+        * \param[out] output Output point cloud passed by reference
+        * \param[in] input1
+        * \param[in] input2
+       */
+      void 
+      intersectionBinary (pcl::PointCloud<PointT> &output, 
+                          const pcl::PointCloud<PointT> &input1, 
+                          const pcl::PointCloud<PointT> &input2);
+
+      /** \brief Creates a circular structing element. The size of the kernel created is 2*radius x 2*radius.
+        * Center of the structuring element is the center of the circle.
+        * All values lying on the circle are 1 and the others are 0.
+        *
+        * \param[out] kernel structuring element kernel passed by reference
+        * \param[in] radius Radius of the circular structuring element.
+        */
+      void 
+      structuringElementCircular (pcl::PointCloud<PointT> &kernel, const int radius);
+
+      /** \brief Creates a rectangular structing element of size height x width.         *
+        * All values are 1.
+        *
+        * \param[out] kernel structuring element kernel passed by reference
+        * \param[in] height height number of rows in the structuring element
+        * \param[in] width number of columns in the structuring element
+        *
+        */
+      void 
+      structuringElementRectangle (pcl::PointCloud<PointT> &kernel, 
+                                   const int height, const int width);
+
+      enum MORPHOLOGICAL_OPERATOR_TYPE
+      {
+        EROSION_GRAY,
+        DILATION_GRAY,
+        OPENING_GRAY,
+        CLOSING_GRAY,
+        EROSION_BINARY,
+        DILATION_BINARY,
+        OPENING_BINARY,
+        CLOSING_BINARY
+      };
+
+      MORPHOLOGICAL_OPERATOR_TYPE operator_type;
+
+      /**
+        * \param[out] output Output point cloud passed by reference
+        */
+      void 
+      applyMorphologicalOperation (pcl::PointCloud<PointT> &output);
+      
+      /**
+        * \param[in] structuring_element The structuring element to be used for the morphological operation
+        */
+      void 
+      setStructuringElement (const PointCloudInPtr &structuring_element);
+  };
+}
+
+#include "impl/morphology.hpp"
+
+#endif    // PCL_2D_MORPHOLOGY_H_
diff --git a/quasimodo/quasimodo_models/src/core/Camera.cpp b/quasimodo/quasimodo_models/src/core/Camera.cpp
index 54dd231d..b83aaaa0 100644
--- a/quasimodo/quasimodo_models/src/core/Camera.cpp
+++ b/quasimodo/quasimodo_models/src/core/Camera.cpp
@@ -3,7 +3,6 @@
 namespace reglib
 {
 
-
 unsigned int camera_id_count = 0;
 
 Camera::Camera(){
@@ -17,24 +16,14 @@ Camera::Camera(){
 	cy = float(height-1)/2;
 	idepth_scale = 0.001/5.0;
 
-	bias = 500;
-
-	pixel_weights = new double[width*height];
-	pixel_sums = new double[width*height];
-	for(unsigned int i = 0; i < width*height; i++){
-		pixel_weights[i]	= bias;
-		pixel_sums[i]		= bias;
-	}
-
+	pixel_weights = 0;
+	pixel_sums = 0;
 }
 
-Camera::~Camera(){
-	delete[] pixel_weights;
-	delete[] pixel_sums;
-}
+Camera::~Camera(){}
 
 void Camera::save(std::string path){
-	printf("save camera to %s\n",path.c_str());
+	//printf("Camera::save(%s)\n",path.c_str());
 	unsigned long buffersize = 7*sizeof(double);
 	char* buffer = new char[buffersize];
 	double * buffer_double = (double *)buffer;
@@ -58,15 +47,28 @@ void Camera::save(std::string path){
 	delete[] buffer;
 }
 
-void Camera::print(){}
+void Camera::print(){
+	printf("Camera fx: %5.5f fy: %5.5f cx: %5.5f cy: %5.5f\n",fx,fy,cx,cy);
+}
+
+
+Camera * Camera::clone(){
+	Camera * cam = new Camera();
+	cam->fx = fx;
+	cam->fy = fy;
+	cam->cx = cx;
+	cam->cy = cy;
+	return cam;
+}
 
 Camera * Camera::load(std::string path){
+	//printf("Camera::load(%s)\n",path.c_str());
 	Camera * cam = new Camera();
 
 	std::streampos size;
 	char * buffer;
 
-	std::ifstream file (path, std::ios::in | std::ios::binary | std::ios::ate);
+	std::ifstream file (path+"_data.txt", std::ios::in | std::ios::binary | std::ios::ate);
 	if (file.is_open()){
 		size = file.tellg();
 		buffer = new char [size];
diff --git a/quasimodo/quasimodo_models/src/core/OcclusionScore.cpp b/quasimodo/quasimodo_models/src/core/OcclusionScore.cpp
new file mode 100644
index 00000000..f01fd22d
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/OcclusionScore.cpp
@@ -0,0 +1,17 @@
+#include "core/OcclusionScore.h"
+
+namespace reglib
+{
+
+OcclusionScore::OcclusionScore(){score = 0;occlusions = 0;}
+OcclusionScore::OcclusionScore(	double score_ ,double occlusions_){score = score_;occlusions = occlusions_;}
+OcclusionScore::~OcclusionScore(){}
+
+void OcclusionScore::add(OcclusionScore oc){
+	score += oc.score;
+	occlusions += oc.occlusions;
+}
+
+void OcclusionScore::print(){printf("score: %5.5f occlusions: %5.5f\n",score,occlusions);}
+
+}
diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp
index cfa495c5..89f8318c 100644
--- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp
+++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp
@@ -29,24 +29,320 @@
 
 #include <ctime>
 
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+#include "organized_edge_detection.hpp"
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/PCLPointCloud2.h>
+
+
+
 
 namespace reglib
 {
+
+int RGBDFrame::saveId = 0;
+
 unsigned long RGBDFrame_id_counter;
 RGBDFrame::RGBDFrame(){
 	id = RGBDFrame_id_counter++;
 	capturetime = 0;
 	pose = Eigen::Matrix4d::Identity();
+	keyval = "";
+	soma_id = "";
+    labels = 0;
 }
 
 bool updated = true;
 void on_trackbar( int, void* ){updated = true;}
 
-RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){
+float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy){
+	int ind = sourceH * width + sourceW;
+	return est[ind]+(targetW-float(sourceW))*dx[ind]+(targetH-float(sourceH))*dy[ind];
+}
+
+float weightFunc(int w0, int h0, int w1, int h1, int width, std::vector<cv::Mat> & est, std::vector<cv::Mat> & dx, std::vector<cv::Mat> & dy){
+	float weight = 1;
+	for(int c = 0; c < est.size(); c++){
+		float error = ((float*)(est[c].data))[h0*width+w0] - pred(w0,h0,w1,h1,width, (float*)(est[c].data), (float*)(dx[c].data), (float*)(dy[c].data));
+		weight *= exp(-0.5*error*error/(0.1*0.1));
+	}
+	return weight;
+}
+
+
+float weightFunc(int w0, int h0, int w1, int h1, int width, std::vector<float*> & est, std::vector<float*> & dx, std::vector<float*> & dy){
+	float weight = 1;
+	for(int c = 0; c < est.size(); c++){
+		float error = est[c][h0*width+w0] - pred(w0,h0,w1,h1,width, est[c], dx[c], dy[c]);
+		weight *= exp(-0.5*error*error/(0.1*0.1));
+	}
+	return weight;
+}
+
+std::vector< std::vector<double> > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){
+	cv::Mat src						= frame->rgb.clone();
+	unsigned short * depthdata		= (unsigned short	*)(frame->depth.data);
+	float		   * normalsdata	= (float			*)(frame->normals.data);
+	const float idepth				= frame->camera->idepth_scale;
+
+	cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT );
+
+	unsigned char * srcdata = (unsigned char *)src.data;
+	unsigned int width = src.cols;
+	unsigned int height = src.rows;
+
+	std::vector< std::vector<double> > probs;
+	unsigned int chans = 3;
+	for(unsigned int c = 0; c < chans;c++){
+		std::vector<double> Xvec;
+		int dir;
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				dir		= 1;
+				Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]));
+
+				dir		= width;
+				Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]));
+			}
+		}
+
+		Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size());
+		for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];}
+
+		double stdval = 0;
+		for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);}
+		stdval = sqrt(stdval/double(Xvec.size()));
+
+		DistanceWeightFunction2PPR3 * func = new DistanceWeightFunction2PPR3();
+		func->zeromean				= true;
+		func->maxp					= 0.99;
+		func->startreg				= 0.0;
+		func->debugg_print			= false;
+		func->maxd					= 256.0;
+		func->histogram_size		= 256;
+		//func->fixed_histogram_size	= true;
+		func->fixed_histogram_size	= false;
+		func->update_size       	= true;
+		func->startmaxd				= func->maxd;
+		func->starthistogram_size	= func->histogram_size;
+		func->blurval				= 0.005;
+		func->stdval2				= stdval;
+		func->maxnoise				= stdval;
+		func->reset();
+		func->computeModel(X);
+
+		std::vector<double> dx;  dx.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;}
+		std::vector<double> dy;	 dy.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;}
+		std::vector<double> dxy; dxy.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dxy[i] = 0.5;}
+		std::vector<double> dyx; dyx.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dyx[i] = 0.5;}
+		for(unsigned int w = 1; w < width;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+
+				dir		= 1;
+				dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]));
+
+				dir		= width;
+				dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]));
+			}
+		}
+		delete func;
+
+		probs.push_back(dx);
+		probs.push_back(dy);
+	}
+
+	{
+		std::vector<double> Xvec;
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+
+				if(w > 1){
+					int dir = -1;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					z2 = 2*z2-z3;
+
+					if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));}
+				}
+
+				if(h > 1){
+					int dir = -width;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
 
-	//printf("%s LINE:%i\n",__FILE__,__LINE__);
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					z2 = 2*z2-z3;
 
-    sweepid = -1;
+					if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));}
+				}
+			}
+		}
+
+		Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size());
+		for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];}
+
+		double stdval = 0;
+		for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);}
+		stdval = sqrt(stdval/double(Xvec.size()));
+
+		DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2();
+		funcZ->zeromean				= true;
+		funcZ->startreg				= 0.002;
+		funcZ->debugg_print			= false;
+		funcZ->bidir				= false;
+		funcZ->maxp					= 0.999999;
+		funcZ->maxd					= 0.1;
+		funcZ->histogram_size		= 100;
+		funcZ->fixed_histogram_size	= true;
+		funcZ->startmaxd			= funcZ->maxd;
+		funcZ->starthistogram_size	= funcZ->histogram_size;
+		funcZ->blurval				= 0.5;
+		funcZ->stdval2				= stdval;
+		funcZ->maxnoise				= stdval;
+		funcZ->reset();
+		funcZ->computeModel(X);
+
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+
+				if(w > 1){
+					int dir = -1;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					z2 = 2*z2-z3;
+
+					if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));}
+				}
+
+				if(h > 1){
+					int dir = -width;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					z2 = 2*z2-z3;
+
+					if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));}
+				}
+			}
+		}
+
+		std::vector<double> dx;  dx.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;}
+		std::vector<double> dy;	 dy.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;}
+
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+
+				if(w > 1){
+					int dir = -1;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+
+					float dz = fabs(z-z2);
+
+					if(z3 > 0){dz = std::min(float(dz),float(fabs(z- (2*z2-z3))));}
+
+					if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));}
+				}
+
+				if(h > 1){
+					int dir = -width;
+
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					float dz = fabs(z-z2);
+
+					if(z3 > 0){dz = std::min(float(dz),float(fabs(z- (2*z2-z3))));}
+					if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));}
+				}
+			}
+		}
+
+		delete funcZ;
+		probs.push_back(dx);
+		probs.push_back(dy);
+	}
+	return probs;
+}
+
+//RGBDFrame * RGBDFrame::clone(){
+//    return new RGBDFrame(camera->clone(), rgb.clone(),depth.clone(),capturetime, pose, true);
+//}
+
+RGBDFrame * RGBDFrame::clone(){
+	//    printf("clone()\n");
+	//    exit(0);
+	RGBDFrame * frame = new RGBDFrame();
+	frame->camera = camera->clone();
+	frame->rgb = rgb.clone();
+	frame->depth = depth.clone();
+	frame->keyval = keyval;
+	frame->capturetime = capturetime;
+	frame->pose = pose;
+	frame->sweepid = sweepid;
+	frame->det_dilate = det_dilate.clone();
+	frame->normals = normals.clone();
+	frame->depthedges = depthedges.clone();
+	frame->de = de.clone();
+	frame->ce = ce.clone();
+	frame->ce = ce.clone();
+	frame->ce = ce.clone();
+
+	unsigned int width = camera->width;
+	unsigned int height = camera->height;
+	unsigned int nr_pixels = width*height;
+
+	frame->connections = connections;
+	frame->intersections = intersections;
+	frame->nr_labels = nr_labels;
+	frame->labels = new int[nr_pixels];
+	for(int i = 0; i < nr_pixels; i++){frame->labels[i] = labels[i];}
+
+	return frame;//new RGBDFrame(camera->clone(), rgb.clone(),depth.clone(),capturetime, pose, true);
+}
+
+RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals, std::string savePath, bool compute_imgedges){
+	soma_id = "";
+    labels = 0;
+
+	//printf("savepath: %s\n",savePath.c_str());
+	bool verbose = false;
+	if(verbose)
+		printf("------------------------------\n");
+	double startTime = getTime();
+	double completeStartTime = getTime();
+	keyval = "";
+
+	sweepid = -1;
 	id = RGBDFrame_id_counter++;
 	camera = camera_;
 	rgb = rgb_;
@@ -54,20 +350,29 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt
 	capturetime = capturetime_;
 	pose = pose_;
 
+	char savestr [1024];
+	sprintf(savestr,"%s/frame%5.5i_",savePath.c_str(),saveId);
+	if(savePath.size() != 0){saveId++;}
+
+
+	if(savePath.size() != 0){
+		cv::imwrite( std::string(savestr)+"rgb.png", rgb );
+		cv::imwrite( std::string(savestr)+"depth.png", depth );
+	}
+
 	IplImage iplimg = rgb_;
 	IplImage* img = &iplimg;
 
-	//printf("%s LINE:%i\n",__FILE__,__LINE__);
-
-    int width = img->width;
-    int height = img->height;
-    int sz = height*width;
+	unsigned int width = img->width;
+	unsigned int height = img->height;
+	unsigned int nr_pixels = width*height;
 	const double idepth			= camera->idepth_scale;
 	const double cx				= camera->cx;
 	const double cy				= camera->cy;
 	const double ifx			= 1.0/camera->fx;
 	const double ify			= 1.0/camera->fy;
 
+
 	connections.resize(1);
 	connections[0].resize(1);
 	connections[0][0] = 0;
@@ -77,96 +382,416 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt
 	intersections[0][0] = 0;
 
 	nr_labels = 1;
-	labels = new int[width*height];
-    for(int i = 0; i < width*height; i++){labels[i] = 0;}
-
+	labels = new int[nr_pixels];
+	for(unsigned int i = 0; i < nr_pixels; i++){labels[i] = 0;}
 
-	//printf("%s LINE:%i\n",__FILE__,__LINE__);
 	unsigned short * depthdata = (unsigned short *)depth.data;
 	unsigned char * rgbdata = (unsigned char *)rgb.data;
 
+	if(verbose)
+		printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime();
+
+	de.create(height,width,CV_32FC3);
+	float * dedata = (float*)de.data;
+	for(unsigned int i = 0; i < 3*nr_pixels; i++){dedata[i] = 0;}
+
+	ce.create(height,width,CV_32FC3);
+	float * cedata = (float*)ce.data;
+	for(unsigned int i = 0; i < 3*nr_pixels; i++){cedata[i] = 0;}
+
 	depthedges.create(height,width,CV_8UC1);
 	unsigned char * depthedgesdata = (unsigned char *)depthedges.data;
+	for(unsigned int i = 0; i < nr_pixels; i++){depthedgesdata[i] = 0;}
+	if(compute_imgedges){
+		std::vector<double> Xvec;
+		int step = 1;
+		for(unsigned int w = step; w < width-step;w++){
+			for(unsigned int h = step; h < height-step;h++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+
+				if(w > 2 && w < width-1){
+					float z0 = idepth*float(depthdata[ind-2]);
+					float z1 = idepth*float(depthdata[ind-1]);
+					float z2 = z;
+					float z3 = idepth*float(depthdata[ind+1]);
+
+					float pred1 = z1 + (z1-z0);
+					float pred2 = z2 - (z3-z2);
+
+					if(pred1 > 0 || pred2 > 0){
+						double n0 = z0*z0;
+						double n1 = z1*z1;
+						double n2 = z2*z2;
+						double n3 = z3*z3;
+
+						double meanpred = 0.5*(fabs(z2-pred1) + fabs(z1-pred2));
+						double noise3 = sqrt(n1*n1+n2*n2);
+						float pred3 = std::min(meanpred,fabs(z1-z2));
+						dedata[3*ind+1] = pred3/noise3;
+						Xvec.push_back(meanpred/noise3);
+					}
+				}
 
-	double t = 0.01;
-    for(int w = 0; w < width; w++){
-        for(int h = 0; h < height;h++){
-			int ind = h*width+w;
-			depthedgesdata[ind] = 0;
-			double z = idepth*double(depthdata[ind]);
-			if(w > 0){
-				double z2 = idepth*double(depthdata[ind-1]);
-				double info = 1.0/(z*z+z2*z2);
-				double diff = fabs(z2-z)*info;
-				if(diff > t){depthedgesdata[ind] = 255;}
+				if(h > 2 && h < height-1){
+					float z0 = idepth*float(depthdata[ind-2*width]);
+					float z1 = idepth*float(depthdata[ind-1*width]);
+					float z2 = z;
+					float z3 = idepth*float(depthdata[ind+1*width]);
+
+					float pred1 = z1 + (z1-z0);
+					float pred2 = z2 - (z3-z2);
+
+					if(pred1 > 0 || pred2 > 0){
+						double n0 = z0*z0;
+						double n1 = z1*z1;
+						double n2 = z2*z2;
+						double n3 = z3*z3;
+
+						double meanpred = 0.5*(fabs(z2-pred1) + fabs(z1-pred2));
+						double noise3 = sqrt(n1*n1+n2*n2);
+						float pred3 = std::min(meanpred,fabs(z1-z2));
+						dedata[3*ind+2] = pred3/noise3;
+						Xvec.push_back(meanpred/noise3);
+					}
+				}
 			}
+		}
+
+		//	cv::namedWindow( "de", cv::WINDOW_AUTOSIZE );		cv::imshow( "de",	de);
+		//	cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE );			cv::imshow( "rgb",	rgb);
+		//	cv::waitKey(0);
+
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"depth_diff.png", 4.0*255.0*de );}
+
+		if(verbose)
+			printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime();
 
-			if(w < width-1){
-				double z2 = idepth*double(depthdata[ind+1]);
-				double info = 1.0/(z*z+z2*z2);
-				double diff = fabs(z2-z)*info;
-				if(diff > t){depthedgesdata[ind] = 255;}
+		Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size());
+		for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];}
+
+		double stdval = 0;
+		for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);}
+		stdval = sqrt(stdval/double(Xvec.size()));
+
+		DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3();
+		if(savePath.size() != 0){
+			funcZ->savePath = std::string(savestr)+"funcZ.m";
+		}
+		funcZ->zeromean				= true;
+		//funcZ->startreg				= 0.001;
+		funcZ->startreg				= 0.0005;
+		funcZ->debugg_print			= false;
+		funcZ->bidir				= false;
+		funcZ->maxp					= 0.999999;
+		funcZ->maxd					= 0.1;
+		funcZ->histogram_size		= 100;
+		funcZ->fixed_histogram_size	= true;
+		funcZ->startmaxd			= funcZ->maxd;
+		funcZ->starthistogram_size	= funcZ->histogram_size;
+		funcZ->blurval				= 0.5;
+		funcZ->stdval2				= stdval;
+		funcZ->maxnoise				= stdval;
+		funcZ->reset();
+		funcZ->computeModel(X);
+
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				unsigned int ind = h*width+w;
+				if(w > 1){dedata[3*ind+1] = 1-funcZ->getProb(dedata[3*ind+1]);}
+				if(h > 1){dedata[3*ind+2] = 1-funcZ->getProb(dedata[3*ind+2]);}
 			}
+		}
+
+
+		//	cv::namedWindow( "de", cv::WINDOW_AUTOSIZE );		cv::imshow( "de",	de);
+		//	cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE );			cv::imshow( "rgb",	rgb);
+		//	cv::waitKey(0);
+
+		//	cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE );			cv::imshow( "rgb", rgb );
+		//	cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE );		cv::imshow( "normals", normals );
+		//	cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE );		cv::imshow( "depth", depth );
+		//	cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE );	cv::imshow( "depthedges", depthedges );
+		//	if(stop){	cv::waitKey(0);}else{					cv::waitKey(30);}
+
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"depth_prob.png", 255.0*de );}
+
+		delete funcZ;
+
+		if(verbose)
+			printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime();
+
+		int blursize = 5;
+		cv::Mat blur_rgb				= rgb.clone();
+		cv::GaussianBlur( blur_rgb, blur_rgb, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT );
+
+
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"blur_rgb.png", blur_rgb );}
+		unsigned char * blurdata = (unsigned char *)blur_rgb.data;
 
-			if(h > 0){
-				double z2 = idepth*double(depthdata[ind-width]);
-				double info = 1.0/(z*z+z2*z2);
-				double diff = fabs(z2-z)*info;
-				if(diff > t){depthedgesdata[ind] = 255;}
+		cv::Mat re;
+		re.create(height,width,CV_32FC3);
+		float * redata = (float*)re.data;
+		for(unsigned int i = 0; i < 3*nr_pixels; i++){redata[i] = 0;}
+
+		cv::Mat ge;
+		ge.create(height,width,CV_32FC3);
+		float * gedata = (float*)ge.data;
+		for(unsigned int i = 0; i < 3*nr_pixels; i++){gedata[i] = 0;}
+
+		cv::Mat be;
+		be.create(height,width,CV_32FC3);
+		float * bedata = (float*)be.data;
+		for(unsigned int i = 0; i < 3*nr_pixels; i++){bedata[i] = 0;}
+
+		std::vector<double> XvecR;
+		std::vector<double> XvecG;
+		std::vector<double> XvecB;
+
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 0; h < height;h++){
+				unsigned int ind = h*width+w;
+				bedata[3*ind+1] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-1)+0]);
+				XvecB.push_back(bedata[3*ind+1]);
+
+				gedata[3*ind+1] = fabs(blurdata[3*ind+1] - blurdata[3*(ind-1)+1]);
+				XvecG.push_back(gedata[3*ind+1]);
+
+				redata[3*ind+1] = fabs(blurdata[3*ind+2] - blurdata[3*(ind-1)+2]);
+				XvecR.push_back(redata[3*ind+1]);
 			}
+		}
+		for(unsigned int w = 0; w < width;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				unsigned int ind = h*width+w;
+				bedata[3*ind+2] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-width)+0]);
+				XvecB.push_back(bedata[3*ind+1]);
+
+				gedata[3*ind+2] = fabs(blurdata[3*ind+1] - blurdata[3*(ind-width)+1]);
+				XvecG.push_back(gedata[3*ind+1]);
 
-			if(h < height-1){
-				double z2 = idepth*double(depthdata[ind+width]);
-				double info = 1.0/(z*z+z2*z2);
-				double diff = fabs(z2-z)*info;
-				if(diff > t){depthedgesdata[ind] = 255;}
+				redata[3*ind+2] = fabs(blurdata[3*ind+2] - blurdata[3*(ind-width)+2]);
+				XvecR.push_back(redata[3*ind+1]);
 			}
+		}
+
 
-			if(h > 0 && w > 0){
-				double z2 = idepth*double(depthdata[ind-width-1]);
-				double info = 1.0/(z*z+z2*z2);
-				double diff = fabs(z2-z)*info;
-				if(diff > t){depthedgesdata[ind] = 255;}
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_diff.png", 3.0*re );}
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_diff.png", 3.0*ge );}
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"be_diff.png", 3.0*be );}
+
+		std::vector<double> src_dxdata;
+		src_dxdata.resize(nr_pixels);
+		for(unsigned int i = 0; i < nr_pixels;i++){src_dxdata[i] = 0;}
+
+		std::vector<double> src_dydata;
+		src_dydata.resize(nr_pixels);
+		for(unsigned int i = 0; i < nr_pixels;i++){src_dydata[i] = 0;}
+
+		std::vector<bool> maxima_dxdata;
+		maxima_dxdata.resize(nr_pixels);
+		for(unsigned int i = 0; i < nr_pixels;i++){maxima_dxdata[i] = 0;}
+
+		std::vector<bool> maxima_dydata;
+		maxima_dydata.resize(nr_pixels);
+		for(unsigned int i = 0; i < nr_pixels;i++){maxima_dydata[i] = 0;}
+
+		for(unsigned int w = 0; w < width;w++){
+			for(unsigned int h = 0; h < height;h++){
+				unsigned int ind = h*width+w;
+				src_dxdata[ind] = 0;
+				src_dydata[ind] = 0;
 			}
+		}
 
-			if(w > 0 && h < height-1){
-				double z2 = idepth*double(depthdata[ind+width-1]);
-				double info = 1.0/(z*z+z2*z2);
-				double diff = fabs(z2-z)*info;
-				if(diff > t){depthedgesdata[ind] = 255;}
+		unsigned int chans = 3;
+		for(unsigned int c = 0; c < chans;c++){
+			for(unsigned int w = 1; w < width;w++){
+				for(unsigned int h = 1; h < height;h++){
+					int ind = h*width+w;
+					src_dxdata[ind] += fabs(float(blurdata[chans*ind+c] - blurdata[chans*(ind-1)	+c]) / 255.0)/3.0;
+					src_dydata[ind] += fabs(float(blurdata[chans*ind+c] - blurdata[chans*(ind-width)+c]) / 255.0)/3.0;
+				}
 			}
+		}
 
-			if(h > 0 && w < width-1){
-				double z2 = idepth*double(depthdata[ind-width+1]);
-				double info = 1.0/(z*z+z2*z2);
-				double diff = fabs(z2-z)*info;
-				if(diff > t){depthedgesdata[ind] = 255;}
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				maxima_dxdata[ind] = (src_dxdata[ind] > src_dxdata[ind-1]     && src_dxdata[ind] >= src_dxdata[ind+1]);
+				maxima_dydata[ind] = (src_dydata[ind] > src_dydata[ind-width] && src_dydata[ind] >= src_dydata[ind+width]);
 			}
+		}
+
+		double stdvalR = 0;
+		for(unsigned int i = 0; i < XvecR.size();i++){stdvalR += XvecR[i]*XvecR[i];}
+		stdvalR = sqrt(stdvalR/double(XvecR.size()));
+
+		double stdvalG = 0;
+		for(unsigned int i = 0; i < XvecG.size();i++){stdvalG += XvecG[i]*XvecG[i];}
+		stdvalG = sqrt(stdvalG/double(XvecG.size()));
+
+		double stdvalB = 0;
+		for(unsigned int i = 0; i < XvecB.size();i++){stdvalB += XvecB[i]*XvecB[i];}
+		stdvalB = sqrt(stdvalB/double(XvecB.size()));
+
+		if(verbose)
+			printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime();
+
+		GeneralizedGaussianDistribution * ggdfuncR	= new GeneralizedGaussianDistribution(true,false);
+		ggdfuncR->nr_refineiters					= 1;
+		GeneralizedGaussianDistribution * ggdfuncG	= new GeneralizedGaussianDistribution(true,false);
+		ggdfuncG->nr_refineiters					= 1;
+		GeneralizedGaussianDistribution * ggdfuncB	= new GeneralizedGaussianDistribution(true,false);
+		ggdfuncB->nr_refineiters					= 1;
+
+		DistanceWeightFunction2PPR3 * funcR = new DistanceWeightFunction2PPR3(ggdfuncR);
+		DistanceWeightFunction2PPR3 * funcG = new DistanceWeightFunction2PPR3(ggdfuncG);
+		DistanceWeightFunction2PPR3 * funcB = new DistanceWeightFunction2PPR3(ggdfuncB);
+
+
 
-			if(h < height-1 && w < width-1){
-				double z2 = idepth*double(depthdata[ind+width+1]);
-				double info = 1.0/(z*z+z2*z2);
-				double diff = fabs(z2-z)*info;
-				if(diff > t){depthedgesdata[ind] = 255;}
+		if(savePath.size() != 0){
+			funcR->savePath = std::string(savestr)+"funcR.m";
+			funcG->savePath = std::string(savestr)+"funcG.m";
+			funcB->savePath = std::string(savestr)+"funcB.m";
+		}
+
+
+		funcR->zeromean		= funcG->zeromean	= funcB->zeromean	= true;
+		funcR->maxp			= funcG->maxp		= funcB->maxp		= 0.999999999999999999;
+		funcR->startreg		= funcG->startreg	= funcB->startreg	= 0.5;
+		funcR->debugg_print = funcG->debugg_print = funcB->debugg_print = false;
+		funcR->maxd			= funcG->maxd		= funcB->maxd		= 256.0;
+		funcR->histogram_size = funcG->histogram_size = funcB->histogram_size = 256;
+		funcR->fixed_histogram_size	= funcG->fixed_histogram_size	= funcB->fixed_histogram_size	= true;
+		funcR->startmaxd	= funcG->startmaxd	= funcB->startmaxd	= funcR->maxd;
+		funcR->starthistogram_size	= funcG->starthistogram_size	= funcB->starthistogram_size	= funcR->histogram_size;
+		//funcR->blurval		= funcG->blurval	= funcB->blurval	= 0.005;
+		funcR->blurval		= funcG->blurval	= funcB->blurval	= 0.5;
+
+		funcR->stdval2		= stdvalR;
+		funcR->maxnoise		= stdvalR;
+		funcG->stdval2		= stdvalG;
+		funcG->maxnoise		= stdvalG;
+		funcB->stdval2		= stdvalB;
+		funcB->maxnoise		= stdvalB;
+
+		funcR->reset();
+		((DistanceWeightFunction2*)funcR)->computeModel(XvecR);
+		funcG->reset();
+		((DistanceWeightFunction2*)funcG)->computeModel(XvecG);
+		funcB->reset();
+		((DistanceWeightFunction2*)funcB)->computeModel(XvecB);
+
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				if(w > 1){
+					double pr = funcR->getProb(redata[3*ind+1]);
+					double pg = funcG->getProb(gedata[3*ind+1]);
+					double pb = funcB->getProb(bedata[3*ind+1]);
+					double pc = pr*pg*pb/(pr*pg*pb+(1-pr)*(1-pg)*(1-pb));
+					redata[3*ind+1] = 1-pr;
+					gedata[3*ind+1] = 1-pg;
+					bedata[3*ind+1] = 1-pb;
+					cedata[3*ind+1] = 1-pc;
+				}
+
+				if(h > 1){
+					double pr = funcR->getProb(redata[3*ind+2]);
+					double pg = funcG->getProb(gedata[3*ind+2]);
+					double pb = funcB->getProb(bedata[3*ind+2]);
+					double pc = pr*pg*pb/(pr*pg*pb+(1-pr)*(1-pg)*(1-pb));
+					redata[3*ind+2] = 1-pr;
+					gedata[3*ind+2] = 1-pg;
+					bedata[3*ind+2] = 1-pb;
+					cedata[3*ind+2] = 1-pc;
+				}
 			}
 		}
+
+		delete funcR;
+		delete funcG;
+		delete funcB;
+
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_prob.png", 127.0*re );}
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_prob.png", 127.0*ge );}
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"be_prob.png", 127.0*be );}
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ce_prob.png", 127.0*ce );}
+
+		if(verbose)
+			printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime();
+
+		cv::Mat det;
+		det.create(height,width,CV_8UC1);
+		unsigned char * detdata = (unsigned char*)det.data;
+		for(unsigned int i = 0; i < nr_pixels; i++){
+			detdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5));
+		}
+
+		cv::Mat cenms;
+		cenms.create(height,width,CV_32FC3);
+		float * cenmsdata = (float*)cenms.data;
+		for(unsigned int i = 0; i < nr_pixels; i++){
+			double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]);
+			edgep = std::max(0.000000000000000000001,edgep);
+			cenmsdata[3*i+0] = edgep;
+			cenmsdata[3*i+1] = edgep;
+			cenmsdata[3*i+2] = edgep;
+		}
+
+
+		if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ce_non_maxima_supress_prob.png", 255.0*cenms );}
+
+		//	for(int i = 0; i < nr_pixels; i++){
+		//		double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]);
+		//		edgep = std::max(0.00001,edgep);
+		//		cenmsdata[3*i+0] = 0;
+		//		cenmsdata[3*i+1] = std::max(0.00001,cenmsdata[3*i+1]);
+		//		cenmsdata[3*i+2] = std::max(0.00001,edgep);
+		//	}
+
+		//		cv::namedWindow( "colour", cv::WINDOW_AUTOSIZE );		cv::imshow( "colour",	ce);
+		//		cv::namedWindow( "colour+nms", cv::WINDOW_AUTOSIZE );	cv::imshow( "colour+nms",	cenms);
+		//		cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE );			cv::imshow( "rgb",	rgb);
+		//		cv::waitKey(0);
+
+		ce = cenms;
+
+		int dilation_size = 2;
+		cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) );
+		unsigned char * det_dilatedata = (unsigned char*)det_dilate.data;
+
+		//	depthedges.create(height,width,CV_8UC1);
+		//	unsigned char * depthedgesdata = (unsigned char *)depthedges.data;
+		for(unsigned int i = 0; i < nr_pixels; i++){
+			//depthedgesdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5) || (cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5));
+			depthedgesdata[i] = 255*(((cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)) && (det_dilatedata[i] == 0));
+		}
 	}
+	//	cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE );			cv::imshow( "depthedges",	depthedges);
+	//	cv::waitKey(0);
 
-	//printf("%s LINE:%i\n",__FILE__,__LINE__);
+	if(verbose)
+		printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime();
 	if(compute_normals){
 		normals.create(height,width,CV_32FC3);
 		float * normalsdata = (float *)normals.data;
 
-		pcl::PointCloud<pcl::PointXYZ>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::PointCloud<pcl::Normal>::Ptr	normals (new pcl::PointCloud<pcl::Normal>);
+		pcl::PointCloud<pcl::PointXYZRGBA>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGBA>);
+		pcl::PointCloud<pcl::Normal>::Ptr	normals_cloud (new pcl::PointCloud<pcl::Normal>);
 		cloud->width	= width;
 		cloud->height	= height;
-		cloud->points.resize(width*height);
+		cloud->points.resize(nr_pixels);
 
-        for(int w = 0; w < width; w++){
-            for(int h = 0; h < height;h++){
-				int ind = h*width+w;
-				pcl::PointXYZ & p = cloud->points[ind];
+		for(unsigned int w = 0; w < width; w++){
+			for(unsigned int h = 0; h < height;h++){
+				unsigned int ind = h*width+w;
+				pcl::PointXYZRGBA & p = cloud->points[ind];
+				p.b = rgbdata[3*ind+0];
+				p.g = rgbdata[3*ind+1];
+				p.r = rgbdata[3*ind+2];
 				double z = idepth*double(depthdata[ind]);
 				if(z > 0){
 					p.x = (double(w) - cx) * z * ifx;
@@ -180,15 +805,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt
 			}
 		}
 
-		//printf("%s LINE:%i\n",__FILE__,__LINE__);
-		pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
+		pcl::IntegralImageNormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
 		ne.setInputCloud(cloud);
 
-		bool tune = false;
-		unsigned char * combidata;
-		cv::Mat combined;
-
-		int NormalEstimationMethod = 0;
 		int MaxDepthChangeFactor = 20;
 		int NormalSmoothingSize = 7;
 		int depth_dependent_smoothing = 1;
@@ -196,12 +815,12 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt
 		ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor));
 		ne.setNormalSmoothingSize(NormalSmoothingSize);
 		ne.setDepthDependentSmoothing (depth_dependent_smoothing);
-		ne.compute(*normals);
+		ne.compute(*normals_cloud);
 
-        for(int w = 0; w < width; w++){
-            for(int h = 0; h < height;h++){
-				int ind = h*width+w;
-				pcl::Normal		p2		= normals->points[ind];
+		for(unsigned int w = 0; w < width; w++){
+			for(unsigned int h = 0; h < height;h++){
+				unsigned int ind = h*width+w;
+				pcl::Normal		p2		= normals_cloud->points[ind];
 				if(!isnan(p2.normal_x)){
 					normalsdata[3*ind+0]	= p2.normal_x;
 					normalsdata[3*ind+1]	= p2.normal_y;
@@ -214,86 +833,49 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt
 			}
 		}
 
-		//printf("%s LINE:%i\n",__FILE__,__LINE__);
-		if(tune){
-			combined.create(height,2*width,CV_8UC3);
-			combidata = (unsigned char *)combined.data;
-
-			cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE );
-			cv::namedWindow( "combined", cv::WINDOW_AUTOSIZE );
-
-			//cv::createTrackbar( "NormalEstimationMethod", "combined", &NormalEstimationMethod, 3, on_trackbar );
-			//cv::createTrackbar( "MaxDepthChangeFactor", "combined", &MaxDepthChangeFactor, 1000, on_trackbar );
-			//cv::createTrackbar( "NormalSmoothingSize", "combined", &NormalSmoothingSize, 100, on_trackbar );
-			//cv::createTrackbar( "depth_dependent_smoothing", "combined", &depth_dependent_smoothing, 1, on_trackbar );
-
-			//while(true){
-
-				if(NormalEstimationMethod == 0){ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);}
-				if(NormalEstimationMethod == 1){ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);}
-				if(NormalEstimationMethod == 2){ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);}
-				if(NormalEstimationMethod == 3){ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);}
-
-				ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor));
-				ne.setNormalSmoothingSize(NormalSmoothingSize);
-				ne.setDepthDependentSmoothing (depth_dependent_smoothing);
-				ne.compute(*normals);
-                for(int w = 0; w < width; w++){
-                    for(int h = 0; h < height;h++){
-						int ind = h*width+w;
-						pcl::Normal		p2		= normals->points[ind];
-						if(!isnan(p2.normal_x)){
-							normalsdata[3*ind+0]	= p2.normal_x;
-							normalsdata[3*ind+1]	= p2.normal_y;
-							normalsdata[3*ind+2]	= p2.normal_z;
-						}else{
-							normalsdata[3*ind+0]	= 2;
-							normalsdata[3*ind+1]	= 2;
-							normalsdata[3*ind+2]	= 2;
-						}
+		if(savePath.size() != 0){
+			cv::Mat out;
+			out.create(height,width,CV_8UC3);
+			unsigned char * outdata = out.data;
+
+			for(unsigned int w = 0; w < width; w++){
+				for(unsigned int h = 0; h < height;h++){
+					unsigned int ind = h*width+w;
+					pcl::Normal		p2		= normals_cloud->points[ind];
+					if(!isnan(p2.normal_x)){
+						outdata[3*ind+0]	= fabs(255.0*p2.normal_x);
+						outdata[3*ind+1]	= fabs(255.0*p2.normal_y);
+						outdata[3*ind+2]	= fabs(255.0*p2.normal_z);
+					}else{
+						outdata[3*ind+0]	= 0;
+						outdata[3*ind+1]	= 0;
+						outdata[3*ind+2]	= 0;
 					}
 				}
+			}
 
-
-                for(int w = 0; w < width; w++){
-                    for(int h = 0; h < height;h++){
-						int ind = h*width+w;
-						int indn = h*2*width+(w+width);
-						int indc = h*2*width+(w);
-						combidata[3*indc+0]	= rgbdata[3*ind+0];
-						combidata[3*indc+1]	= rgbdata[3*ind+1];
-						combidata[3*indc+2]	= rgbdata[3*ind+2];
-						pcl::Normal		p2		= normals->points[ind];
-						if(!isnan(p2.normal_x)){
-							combidata[3*indn+0]	= 255.0*fabs(p2.normal_x);
-							combidata[3*indn+1]	= 255.0*fabs(p2.normal_y);
-							combidata[3*indn+2]	= 255.0*fabs(p2.normal_z);
-						}else{
-							combidata[3*indn+0]	= 255;
-							combidata[3*indn+1]	= 255;
-							combidata[3*indn+2]	= 255;
-						}
-					}
-				}
-				char buf [1024];
-                sprintf(buf,"combined%i.png",int(id));
-				cv::imwrite( buf, combined );
-				printf("saving: %s\n",buf);
-				cv::imshow( "combined", combined );
-				cv::waitKey(0);
-				//while(!updated){cv::waitKey(50);}
-				updated = false;
-			//}
-		}
-
-		//printf("%s LINE:%i\n",__FILE__,__LINE__);
+			cv::imwrite( std::string(savestr)+"normals.png", out );
+		}
+	}
+	if(verbose)
+		printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime();
+	//printf("complete time to create RGBD image: %5.5fs\n",getTime()-completeStartTime);
+
+	//getImageProbs();
+	if(savePath.size() != 0){
+		saveCombinedImages(std::string(savestr)+"Combined.png");
+		saveCombinedProcessedImages(std::string(savestr)+"CombinedProcessed.png");
 	}
-	//show(true);
-
-	//printf("%s LINE:%i\n",__FILE__,__LINE__);
 }
 
-RGBDFrame::~RGBDFrame(){}
+RGBDFrame::~RGBDFrame(){
+	rgb.release();
+	normals.release();
+	depth.release();
+	depthedges.release();
+	//if(rgbdata != 0){delete[] rgbdata; rgbdata = 0;}
+	if(labels != 0){delete[] labels; labels = 0;}
+}
 
 void RGBDFrame::show(bool stop){
 	cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE );			cv::imshow( "rgb", rgb );
@@ -303,9 +885,155 @@ void RGBDFrame::show(bool stop){
 	if(stop){	cv::waitKey(0);}else{					cv::waitKey(30);}
 }
 
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RGBDFrame::getSmallPCLcloud(){
+	unsigned char * rgbdata = (unsigned char *)rgb.data;
+	unsigned short * depthdata = (unsigned short *)depth.data;
+
+	const unsigned int width	= camera->width; const unsigned int height	= camera->height;
+	const double idepth			= camera->idepth_scale;
+	const double cx				= camera->cx;		const double cy				= camera->cy;
+	const double ifx			= 1.0/camera->fx;	const double ify			= 1.0/camera->fy;
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
+	cloud->width	= 0;
+	cloud->height	= 1;
+	cloud->points.reserve(width*height);
+	for(unsigned int w = 0; w < width; w++){
+		for(unsigned int h = 0; h < height;h++){
+			int ind = h*width+w;
+			double z = idepth*double(depthdata[ind]);
+			if(z > 0){
+				pcl::PointXYZRGB p;
+				p.b = rgbdata[3*ind+0];
+				p.g = rgbdata[3*ind+1];
+				p.r = rgbdata[3*ind+2];
+				p.x = (double(w) - cx) * z * ifx;
+				p.y = (double(h) - cy) * z * ify;
+				p.z = z;
+				cloud->points.push_back(p);
+			}
+		}
+	}
+	cloud->width	= cloud->points.size();
+	return cloud;
+}
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RGBDFrame::getPCLcloud(){
 	unsigned char * rgbdata = (unsigned char *)rgb.data;
 	unsigned short * depthdata = (unsigned short *)depth.data;
+	float * normalsdata = (float *)normals.data;
+
+	const unsigned int width	= camera->width; const unsigned int height	= camera->height;
+	const double idepth			= camera->idepth_scale;
+	const double cx				= camera->cx;		const double cy				= camera->cy;
+	const double ifx			= 1.0/camera->fx;	const double ify			= 1.0/camera->fy;
+
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	cloud->width	= width;
+	cloud->height	= height;
+	cloud->points.resize(width*height);
+
+	for(unsigned int w = 0; w < width; w++){
+		for(unsigned int h = 0; h < height;h++){
+			unsigned int ind = h*width+w;
+			double z = idepth*double(depthdata[ind]);
+
+			pcl::PointXYZRGBNormal p;
+			p.b = rgbdata[3*ind+0];
+			p.g = rgbdata[3*ind+1];
+			p.r = rgbdata[3*ind+2];
+			if(z > 0){
+				p.x = (double(w) - cx) * z * ifx;
+				p.y = (double(h) - cy) * z * ify;
+				p.z = z;
+			}else{
+				p.x = NAN;
+				p.y = NAN;
+				p.z = NAN;
+			}
+			p.normal_x	= normalsdata[3*ind+0];
+			p.normal_y	= normalsdata[3*ind+1];
+			p.normal_z	= normalsdata[3*ind+2];
+			cloud->points[ind] = p;
+		}
+	}
+	return cloud;
+
+
+
+//	pcl::PointCloud<pcl::PointXYZRGB>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
+//	pcl::PointCloud<pcl::Normal>::Ptr		normals (new pcl::PointCloud<pcl::Normal>);
+//	cloud->width	= width;
+//	cloud->height	= height;
+//	cloud->points.resize(width*height);
+
+//	for(unsigned int w = 0; w < width; w++){
+//		for(unsigned int h = 0; h < height;h++){
+//			int ind = h*width+w;
+//			double z = idepth*double(depthdata[ind]);
+
+//			pcl::PointXYZRGB p;
+//			p.b = rgbdata[3*ind+0];
+//			p.g = rgbdata[3*ind+1];
+//			p.r = rgbdata[3*ind+2];
+//			if(z > 0){
+//				p.x = (double(w) - cx) * z * ifx;
+//				p.y = (double(h) - cy) * z * ify;
+//				p.z = z;
+//			}else{
+//				p.x = NAN;
+//				p.y = NAN;
+//				p.z = NAN;
+//			}
+//			cloud->points[ind] = p;
+//		}
+//	}
+
+//	pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
+//	ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
+//	ne.setMaxDepthChangeFactor(0.02f);
+//	ne.setNormalSmoothingSize(10.0f);
+//	ne.setInputCloud(cloud);
+//	ne.compute(*normals);
+
+//	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+//	cloud_ptr->width	= width;
+//	cloud_ptr->height	= height;
+//	cloud_ptr->points.resize(width*height);
+
+//	for(unsigned int w = 0; w < width; w++){
+//		for(unsigned int h = 0; h < height;h++){
+//			int ind = h*width+w;
+//			pcl::PointXYZRGBNormal & p0	= cloud_ptr->points[ind];
+//			pcl::PointXYZRGB p1			= cloud->points[ind];
+//			pcl::Normal p2				= normals->points[ind];
+
+//			p0.x		= p1.x;
+//			p0.y		= p1.y;
+//			p0.z		= p1.z;
+
+//			if(depthdata[ind] == 0){
+//				p0.x		= 0;
+//				p0.y		= 0;
+//				p0.z		= 0;
+//			}
+
+//			p0.r		= p1.r;
+//			p0.g		= p1.g;
+//			p0.b		= p1.b;
+//			p0.normal_x	= p2.normal_x;
+//			p0.normal_y	= p2.normal_y;
+//			p0.normal_z	= p2.normal_z;
+//		}
+//	}
+//	return cloud_ptr;
+}
+
+void RGBDFrame::savePCD(std::string path, Eigen::Matrix4d pose){
+	// printf("saving pcd: %s\n",path.c_str());
+	unsigned char * rgbdata = (unsigned char *)rgb.data;
+	unsigned short * depthdata = (unsigned short *)depth.data;
 
 	const unsigned int width	= camera->width; const unsigned int height	= camera->height;
 	const double idepth			= camera->idepth_scale;
@@ -313,7 +1041,6 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RGBDFrame::getPCLcloud(){
 	const double ifx			= 1.0/camera->fx;	const double ify			= 1.0/camera->fy;
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
-	pcl::PointCloud<pcl::Normal>::Ptr		normals (new pcl::PointCloud<pcl::Normal>);
 	cloud->width	= width;
 	cloud->height	= height;
 	cloud->points.resize(width*height);
@@ -335,91 +1062,88 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RGBDFrame::getPCLcloud(){
 		}
 	}
 
-	pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
-	ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
-	ne.setMaxDepthChangeFactor(0.02f);
-	ne.setNormalSmoothingSize(10.0f);
-	ne.setInputCloud(cloud);
-	ne.compute(*normals);
-
-	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-	cloud_ptr->width	= width;
-	cloud_ptr->height	= height;
-	cloud_ptr->points.resize(width*height);
+	//Mat4f2RotTrans(const Eigen::Matrix4f &tf, Eigen::Quaternionf &q, Eigen::Vector4f &trans)
+	Mat4f2RotTrans(pose.cast<float>(),cloud->sensor_orientation_,cloud->sensor_origin_);
+	int success = pcl::io::savePCDFileBinaryCompressed(path,*cloud);
+}
 
-	for(unsigned int w = 0; w < width; w++){
-		for(unsigned int h = 0; h < height;h++){
-			int ind = h*width+w;
-			pcl::PointXYZRGBNormal & p0	= cloud_ptr->points[ind];
-			pcl::PointXYZRGB p1			= cloud->points[ind];
-			pcl::Normal p2				= normals->points[ind];
-			p0.x		= p1.x;
-			p0.y		= p1.y;
-			p0.z		= p1.z;
-			p0.r		= p1.r;
-			p0.g		= p1.g;
-			p0.b		= p1.b;
-			p0.normal_x	= p2.normal_x;
-			p0.normal_y	= p2.normal_y;
-			p0.normal_z	= p2.normal_z;
-		}
+std::string type2str(int type) {
+	std::string r;
+
+	uchar depth = type & CV_MAT_DEPTH_MASK;
+	uchar chans = 1 + (type >> CV_CN_SHIFT);
+
+	switch ( depth ) {
+		case CV_8U:  r = "8U"; break;
+		case CV_8S:  r = "8S"; break;
+		case CV_16U: r = "16U"; break;
+		case CV_16S: r = "16S"; break;
+		case CV_32S: r = "32S"; break;
+		case CV_32F: r = "32F"; break;
+		case CV_64F: r = "64F"; break;
+		default:     r = "User"; break;
 	}
-	return cloud_ptr;
-}
 
-void RGBDFrame::savePCD(std::string path){
-    unsigned char * rgbdata = (unsigned char *)rgb.data;
-    unsigned short * depthdata = (unsigned short *)depth.data;
-
-    const unsigned int width	= camera->width; const unsigned int height	= camera->height;
-    const double idepth			= camera->idepth_scale;
-    const double cx				= camera->cx;		const double cy				= camera->cy;
-    const double ifx			= 1.0/camera->fx;	const double ify			= 1.0/camera->fy;
-
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
-    cloud->width	= width;
-    cloud->height	= height;
-    cloud->points.resize(width*height);
-
-    for(unsigned int w = 0; w < width; w++){
-        for(unsigned int h = 0; h < height;h++){
-            int ind = h*width+w;
-            double z = idepth*double(depthdata[ind]);
-            if(z > 0){
-                pcl::PointXYZRGB p;
-                p.x = (double(w) - cx) * z * ifx;
-                p.y = (double(h) - cy) * z * ify;
-                p.z = z;
-                p.b = rgbdata[3*ind+0];
-                p.g = rgbdata[3*ind+1];
-                p.r = rgbdata[3*ind+2];
-                cloud->points[ind] = p;
-            }
-        }
-    }
-    int success = pcl::io::savePCDFileBinaryCompressed(path,*cloud);
+	r += "C";
+	r += (chans+'0');
+
+	return r;
 }
 
+#include <iostream>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+
 void RGBDFrame::save(std::string path){
 	//printf("saving frame %i to %s\n",id,path.c_str());
 
 	cv::imwrite( path+"_rgb.png", rgb );
 	cv::imwrite( path+"_depth.png", depth );
+	cv::imwrite( path+"_det_dilate.png", det_dilate );
+	cv::imwrite( path+"_depthedges.png", depthedges );
+
+	const unsigned int width	= camera->width;
+	const unsigned int height	= camera->height;
+
+	std::ofstream normalsoutfile	(path+"_normalsdata.txt",std::ofstream::binary);
+	normalsoutfile.write ((char*)(normals.data),width*height*3*sizeof(float));
+	normalsoutfile.close();
 
-	unsigned long buffersize = 19*sizeof(double);
+	std::ofstream ceoutfile			(path+"_cedata.txt",std::ofstream::binary);
+	ceoutfile.write ((char*)(ce.data),width*height*3*sizeof(float));
+	ceoutfile.close();
+
+	std::ofstream deoutfile			(path+"_dedata.txt",std::ofstream::binary);
+	deoutfile.write ((char*)(de.data),width*height*3*sizeof(float));
+	deoutfile.close();
+
+	unsigned long buffersize = 22*sizeof(double)+keyval.length()+soma_id.length();
 	char* buffer = new char[buffersize];
 	double * buffer_double = (double *)buffer;
 	unsigned long * buffer_long = (unsigned long *)buffer;
 
 	int counter = 0;
-	buffer_double[counter++] = capturetime;
+	buffer_double[counter++] = capturetime; // CaptureTime
 	for(int i = 0; i < 4; i++){
 		for(int j = 0; j < 4; j++){
 			buffer_double[counter++] = pose(i,j);
 		}
+	}										// Pose
+	buffer_long[counter++] = sweepid;		// Sweepid
+	buffer_long[counter++] = camera->id;	// Cameraid
+	buffer_long[counter++] = id;	// id
+	buffer_long[counter++] = keyval.length();	// keyval.length()
+	buffer_long[counter++] = soma_id.length();	// soma_id.length()
+
+	unsigned int count4 = sizeof(double)*counter;
+	for(unsigned int i = 0; i < keyval.length();i++){
+		buffer[count4++] = keyval[i];
+	}
+	for(unsigned int i = 0; i < soma_id.length();i++){
+		buffer[count4++] = soma_id[i];
 	}
-	buffer_long[counter++] = sweepid;
-	buffer_long[counter++] = camera->id;
+
 	std::ofstream outfile (path+"_data.txt",std::ofstream::binary);
 	outfile.write (buffer,buffersize);
 	outfile.close();
@@ -427,7 +1151,10 @@ void RGBDFrame::save(std::string path){
 }
 
 RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){
-	printf("RGBDFrame * RGBDFrame::load(Camera * cam, std::string path)\n");
+	double startTime = getTime();
+
+	const unsigned int width	= cam->width;
+	const unsigned int height	= cam->height;
 
 	std::streampos size;
 	char * buffer;
@@ -454,16 +1181,310 @@ RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){
 		}
 		int sweepid = buffer_long[counter++];
 		int camera_id = buffer_long[counter++];
+		int id = buffer_long[counter++];
+		int keyvallength = buffer_long[counter++];
+		int soma_idlength = buffer_long[counter++];
+
+
+		std::string keyval;
+		keyval.resize(keyvallength);
+		std::string soma_id;
+		soma_id.resize(soma_idlength);
+
+		unsigned int count4 = sizeof(double)*counter;
+		for(unsigned int i = 0; i < keyvallength;i++){
+			keyval[i] = buffer[count4++];
+		}
+
+		for(unsigned int i = 0; i < soma_idlength;i++){
+			soma_id[i] = buffer[count4++];
+		}
 
 		cv::Mat rgb = cv::imread(path+"_rgb.png", -1);   // Read the file
 		cv::Mat depth = cv::imread(path+"_depth.png", -1);   // Read the file
-
-		RGBDFrame * frame = new RGBDFrame(cam,rgb,depth,capturetime,pose);
+		cv::Mat det_dilate = cv::imread(path+"_det_dilate.png", -1);   // Read the file
+		cv::Mat depthedges = cv::imread(path+"_depthedges.png", -1);   // Read the file
+
+		RGBDFrame * frame = new RGBDFrame();
+		frame->rgb = rgb;
+		frame->depth = depth;
+		frame->det_dilate = det_dilate;
+		frame->depthedges = depthedges;
+		frame->camera = cam;
+		frame->capturetime = capturetime;
 		frame->sweepid = sweepid;
-		//printf("sweepid: %i",sweepid);
+		frame->pose = pose;
+
+		frame->normals.create(height,width,CV_32FC3);
+		frame->ce.create(height,width,CV_32FC3);
+		frame->de.create(height,width,CV_32FC3);
+
+		std::ifstream normalsfile (path+"_normalsdata.txt", std::ios::in | std::ios::binary | std::ios::ate);
+		if (normalsfile.is_open()){
+			size = normalsfile.tellg();
+			normalsfile.seekg (0, std::ios::beg);
+			normalsfile.read ((char*)(frame->normals.data), size);
+			normalsfile.close();
+		}
 
+		std::ifstream cefile (path+"_cedata.txt", std::ios::in | std::ios::binary | std::ios::ate);
+		if (cefile.is_open()){
+			size = cefile.tellg();
+			cefile.seekg (0, std::ios::beg);
+			cefile.read ((char*)(frame->ce.data), size);
+			cefile.close();
+		}
+
+		std::ifstream defile (path+"_dedata.txt", std::ios::in | std::ios::binary | std::ios::ate);
+		if (defile.is_open()){
+			size = defile.tellg();
+			defile.seekg (0, std::ios::beg);
+			defile.read ((char*)(frame->de.data), size);
+			defile.close();
+		}
+
+//printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime();
 		return frame;
 	}else{printf("cant open %s\n",(path+"/data.txt").c_str());}
 }
 
+std::vector<ReprojectionResult> RGBDFrame::getReprojections(std::vector<superpoint> & spvec, Eigen::Matrix4d cp, bool * maskvec, bool useDet){
+	std::vector<ReprojectionResult> ret;
+
+	unsigned char  * dst_detdata		= (unsigned char	*)(det_dilate.data);
+	unsigned char  * dst_rgbdata		= (unsigned char	*)(rgb.data);
+	unsigned short * dst_depthdata		= (unsigned short	*)(depth.data);
+	float		   * dst_normalsdata	= (float			*)(normals.data);
+
+	float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3);
+	float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3);
+	float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3);
+
+	Camera * dst_camera				= camera;
+	const unsigned int dst_width	= dst_camera->width;
+	const unsigned int dst_height	= dst_camera->height;
+	const float dst_idepth			= dst_camera->idepth_scale;
+	const float dst_cx				= dst_camera->cx;
+	const float dst_cy				= dst_camera->cy;
+	const float dst_fx				= dst_camera->fx;
+	const float dst_fy				= dst_camera->fy;
+	const float dst_ifx				= 1.0/dst_camera->fx;
+	const float dst_ify				= 1.0/dst_camera->fy;
+	const unsigned int dst_width2	= dst_camera->width  - 2;
+	const unsigned int dst_height2	= dst_camera->height - 2;
+
+	unsigned long nr_data = spvec.size();
+	for(unsigned long src_ind = 0; src_ind < nr_data;src_ind++){
+		superpoint & sp = spvec[src_ind];
+		if(sp.point_information == 0){continue;}
+
+		float src_x = sp.point(0);
+		float src_y = sp.point(1);
+		float src_z = sp.point(2);
+
+		float src_nx = sp.normal(0);
+		float src_ny = sp.normal(1);
+		float src_nz = sp.normal(2);
+
+		float tx	= m00*src_x + m01*src_y + m02*src_z + m03;
+		float ty	= m10*src_x + m11*src_y + m12*src_z + m13;
+		float tz	= m20*src_x + m21*src_y + m22*src_z + m23;
+
+		float itz	= 1.0/tz;
+		float dst_w	= dst_fx*tx*itz + dst_cx;
+		float dst_h	= dst_fy*ty*itz + dst_cy;
+
+		if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
+			unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
+			if(maskvec != 0 && maskvec[dst_ind] == 0){continue;}
+
+			float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+			float dst_nx = dst_normalsdata[3*dst_ind+0];
+			if(dst_z > 0 && dst_nx != 2){
+				if(useDet && dst_detdata[dst_ind] != 0){continue;}
+				float dst_ny = dst_normalsdata[3*dst_ind+1];
+				float dst_nz = dst_normalsdata[3*dst_ind+2];
+
+				float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
+				float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
+
+				float tnx	= m00*src_nx + m01*src_ny + m02*src_nz;
+				float tny	= m10*src_nx + m11*src_ny + m12*src_nz;
+				float tnz	= m20*src_nx + m21*src_ny + m22*src_nz;
+
+				double residualZ = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz));
+				double residualD2 = (dst_x-tx)*(dst_x-tx) + (dst_y-ty)*(dst_y-ty) + (dst_z-tz)*(dst_z-tz);
+				double residualR =  dst_rgbdata[3*dst_ind + 2] - sp.feature(0);
+				double residualG =  dst_rgbdata[3*dst_ind + 1] - sp.feature(1);
+				double residualB =  dst_rgbdata[3*dst_ind + 0] - sp.feature(2);
+				double angle = tnx*dst_nx + tny*dst_ny + tnz*dst_nz;
+				ret.push_back(ReprojectionResult (src_ind, dst_ind, angle, residualZ,residualD2, residualR, residualG, residualB, getNoise(dst_z), 1.0));
+			}
+		}
+	}
+	return ret;
+}
+
+std::vector<superpoint> RGBDFrame::getSuperPoints(Eigen::Matrix4d cp, unsigned int step, bool zeroinclude){
+	unsigned char  * rgbdata		= (unsigned char	*)(rgb.data);
+	unsigned short * depthdata		= (unsigned short	*)(depth.data);
+	float		   * normalsdata	= (float			*)(normals.data);
+
+	float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3);
+	float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3);
+	float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3);
+
+	const unsigned int width	= camera->width;
+	const unsigned int height	= camera->height;
+	const float idepth			= camera->idepth_scale;
+	const float cx				= camera->cx;
+	const float cy				= camera->cy;
+	const float ifx				= 1.0/camera->fx;
+	const float ify				= 1.0/camera->fy;
+
+	std::vector<superpoint> ret;
+	ret.resize((width/step)*(height/step));
+	unsigned long count = 0;
+
+	for(unsigned long h = 0; h < height;h += step){
+		for(unsigned long w = 0; w < width;w += step){
+			unsigned int ind = h * width + w;
+			Eigen::Vector3f rgb (rgbdata[3*ind+0],rgbdata[3*ind+1],rgbdata[3*ind+2]);
+			float z		= idepth*float(depthdata[ind]);
+			float nx	= normalsdata[3*ind+0];
+			if(z > 0 && nx != 2){
+				float x = (float(w) - cx) * z * ifx;
+				float y = (float(h) - cy) * z * ify;
+				float ny = normalsdata[3*ind+1];
+				float nz = normalsdata[3*ind+2];
+
+				float tx	= m00*x + m01*y + m02*z + m03;
+				float ty	= m10*x + m11*y + m12*z + m13;
+				float tz	= m20*x + m21*y + m22*z + m23;
+
+				float tnx	= m00*nx + m01*ny + m02*nz;
+				float tny	= m10*nx + m11*ny + m12*nz;
+				float tnz	= m20*nx + m21*ny + m22*nz;
+
+				ret[count++]	= superpoint(Vector3f(tx,ty,tz),Vector3f(tnx,tny,tnz),rgb, getInformation(z), 1, 0);
+			}else{
+				if(zeroinclude){
+					ret[count++]	= superpoint(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,0,0),rgb, 0, 1, 0);
+				}
+			}
+		}
+	}
+	ret.resize(count);
+	return ret;
+}
+
+std::vector< std::vector<float> > RGBDFrame::getImageProbs(bool depthonly){
+	const unsigned int width = camera->width;
+	const unsigned int height = camera->height;
+	const unsigned int nr_pixels = width*height;
+
+
+	float * cedata = (float*)ce.data;
+	float * dedata = (float*)de.data;
+	unsigned char * det_dilatedata = det_dilate.data;
+
+	std::vector<float> dxc;
+	dxc.resize(nr_pixels);
+
+	std::vector<float> dyc;
+	dyc.resize(nr_pixels);
+
+	for(unsigned int i = 0; i < nr_pixels;i++){
+		//dxc[i] = 1.0-std::max(dedata[3*i+1],cedata[3*i+1]);
+		//dyc[i] = 1.0-std::max(dedata[3*i+2],cedata[3*i+2]);
+		//ProcessedImages
+
+
+
+		if(!depthonly){
+			if(!det_dilatedata[i]){
+				dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]);
+				dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]);
+			}else{
+				dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]);
+				dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]);
+			}
+		}else{
+			dxc[i] = 1.0-dedata[3*i+1];
+			dyc[i] = 1.0-dedata[3*i+2];
+		}
+
+	}
+
+	//	cv::namedWindow( "Colour edges"	, cv::WINDOW_AUTOSIZE );	cv::imshow( "Colour edges",	ce);
+	//	cv::namedWindow( "Depth edges"	, cv::WINDOW_AUTOSIZE );	cv::imshow( "Depth edges",	de);
+	//	cv::namedWindow( "rgb"			, cv::WINDOW_AUTOSIZE );	cv::imshow( "rgb",			rgb );
+	//	cv::namedWindow( "det_dilate"	, cv::WINDOW_AUTOSIZE );	cv::imshow( "det_dilate",	det_dilate );
+	//	cv::waitKey(0);
+
+	std::vector< std::vector<float> > probs2;
+	probs2.push_back(dxc);
+	probs2.push_back(dyc);
+	return probs2;
+}
+
+void RGBDFrame::saveCombinedImages(std::string path){
+	unsigned int width = camera->width;
+	unsigned int height = camera->height;
+	cv::Mat combined;
+	combined.create(camera->height,2*camera->width,CV_8UC3);
+	unsigned char   * combineddata   = combined.data;
+	unsigned char   * rgbdata   = rgb.data;
+	unsigned short  * depthdata = (unsigned short *)(depth.data);
+
+	for(unsigned long h = 0; h < height;h++){
+		for(unsigned long w = 0; w < width;w++){
+			combineddata[3*(h*(2*width)+w)+0] = rgbdata[3*(h*width+w)+0];
+			combineddata[3*(h*(2*width)+w)+1] = rgbdata[3*(h*width+w)+1];
+			combineddata[3*(h*(2*width)+w)+2] = rgbdata[3*(h*width+w)+2];
+			combineddata[3*(h*(2*width)+w+width)+0] = depthdata[h*width+w]/256;
+			combineddata[3*(h*(2*width)+w+width)+1] = depthdata[h*width+w]/256;
+			combineddata[3*(h*(2*width)+w+width)+2] = depthdata[h*width+w]/256;
+		}
+	}
+
+	cv::imwrite( path, combined );
+}
+void RGBDFrame::saveCombinedProcessedImages(std::string path){
+	unsigned int width = camera->width;
+	unsigned int height = camera->height;
+	cv::Mat combined;
+	combined.create(camera->height,3*camera->width,CV_8UC3);
+	unsigned char   * combineddata   = combined.data;
+	float  * normalsdata = (float *)(normals.data);
+	float  * cedata = (float *)(ce.data);
+	float  * dedata = (float *)(de.data);
+
+	for(unsigned long h = 0; h < height;h++){
+		for(unsigned long w = 0; w < width;w++){
+			if(normalsdata[3*(h*width+w)+0] <= 1){
+				combineddata[3*(h*(3*width)+w+0*width)+0] = 255.0*fabs(normalsdata[3*(h*width+w)+0]);
+				combineddata[3*(h*(3*width)+w+0*width)+1] = 255.0*fabs(normalsdata[3*(h*width+w)+1]);
+				combineddata[3*(h*(3*width)+w+0*width)+2] = 255.0*fabs(normalsdata[3*(h*width+w)+2]);
+			}else{
+				combineddata[3*(h*(3*width)+w+0*width)+0] = 0;
+				combineddata[3*(h*(3*width)+w+0*width)+1] = 0;
+				combineddata[3*(h*(3*width)+w+0*width)+2] = 0;
+			}
+			combineddata[3*(h*(3*width)+w+1*width)+0] = 255.0*cedata[3*(h*width+w)+0];
+			combineddata[3*(h*(3*width)+w+1*width)+1] = 255.0*cedata[3*(h*width+w)+1];
+			combineddata[3*(h*(3*width)+w+1*width)+2] = 255.0*cedata[3*(h*width+w)+2];
+			combineddata[3*(h*(3*width)+w+2*width)+0] = 255.0*dedata[3*(h*width+w)+0];
+			combineddata[3*(h*(3*width)+w+2*width)+1] = 255.0*dedata[3*(h*width+w)+1];
+			combineddata[3*(h*(3*width)+w+2*width)+2] = 255.0*dedata[3*(h*width+w)+2];
+		}
+	}
+
+	cv::imwrite( path, combined );
+
+	//    cv::namedWindow( "combined"	, cv::WINDOW_AUTOSIZE );
+	//    cv::imshow( "combined",	combined );
+	//    cv::waitKey(0);
+}
+
 }
diff --git a/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp b/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp
new file mode 100644
index 00000000..c1d052bc
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp
@@ -0,0 +1,24 @@
+#include "../../include/core/ReprojectionResult.h"
+
+namespace reglib
+{
+
+ReprojectionResult::ReprojectionResult(){}
+ReprojectionResult::ReprojectionResult(unsigned long si, unsigned long di, double a, double rz, double rE2, double rr, double rg, double rb, double nz, double nrgb){
+	src_ind		= si;
+	dst_ind		= di;
+	angle		= a;
+
+	residualZ	= rz;
+	residualE2	= rE2;
+	residualR	= rr;
+	residualG	= rg;
+	residualB	= rb;
+}
+ReprojectionResult::~ReprojectionResult(){}
+
+void ReprojectionResult::print(){
+	printf("src_ind	= %i, dst_ind = %i, angle = %f, residualZ = %f, residualR = %f, residualG = %f, residualB = %f\n",src_ind, dst_ind, angle, residualZ, residualR, residualG, residualB);
+}
+}
+
diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp
index 5fe0d122..668699dc 100644
--- a/quasimodo/quasimodo_models/src/core/Util.cpp
+++ b/quasimodo/quasimodo_models/src/core/Util.cpp
@@ -1,12 +1,176 @@
 #include "core/Util.h"
 namespace reglib{
 
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr getPointCloudFromVector(std::vector<superpoint> & spvec, int colortype, int r, int g, int b){
+		unsigned long nr_points = spvec.size();
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		cloud_ptr->width	= nr_points;
+		cloud_ptr->height	= 1;
+		cloud_ptr->points.resize(nr_points);
+		for(unsigned long ind = 0; ind < nr_points; ind++){
+			superpoint & p				= spvec[ind];
+			pcl::PointXYZRGBNormal & p0	= cloud_ptr->points[ind];
+			p0.x		= p.point(0);
+			p0.y		= p.point(1);
+			p0.z		= p.point(2);
+			if(colortype == 0){
+				p0.r		= p.feature(0);
+				p0.g		= p.feature(1);
+				p0.b		= p.feature(2);
+			}
+			if(colortype == 1){
+				p0.r		= 255.0*fabs(p.normal(0));
+				p0.g		= 255.0*fabs(p.normal(1));
+				p0.b		= 255.0*fabs(p.normal(2));
+			}
+			if(colortype == 2){
+				p0.r		= 255.0*std::min(1.0,1.0/sqrt(p.point_information));
+				p0.g		= 255.0*std::min(1.0,std::max(0.0,1.0-sqrt(p.point_information)));
+				p0.b		= 0;
+			}
+			if(colortype == 3){
+				if(r >= 0){
+					p0.r		= r;
+					p0.g		= g;
+					p0.b		= b;
+				}
+			}
+			p0.normal_x	= p.normal(0);
+			p0.normal_y	= p.normal(1);
+			p0.normal_z	= p.normal(2);
+		}
+		return cloud_ptr;
+	}
+
+	double mysign(double v){
+		if(v < 0){return -1;}
+		return 1;
+	}
+
 	double getTime(){
 		struct timeval start1;
 		gettimeofday(&start1, NULL);
 		return double(start1.tv_sec+(start1.tv_usec/1000000.0));
 	}
 
+	void pn(double p, unsigned int len){
+		char buf1 [64];
+		if(p >= 0){
+			sprintf(buf1,"  %i.%if",len,len);
+			buf1[1] = '%';
+		}else{
+			sprintf(buf1," %i.%if",len,len);
+			buf1[0] = '%';
+		}
+		printf(buf1,p);
+	}
+
+	float graph_cut(std::vector<Graph*>& graphs_out,std::vector<std::vector<int>>& second_graphinds, Graph& graph_in, std::vector<int> graph_inds){
+		using adjacency_iterator = boost::graph_traits<Graph>::adjacency_iterator;
+		typename boost::property_map<Graph, boost::vertex_index_t>::type vertex_id		= boost::get(boost::vertex_index, graph_in);
+		typename boost::property_map<Graph, boost::edge_weight_t>::type  edge_id		= boost::get(boost::edge_weight, graph_in);
+		typename boost::property_map<Graph, boost::vertex_name_t>::type  vertex_name	= boost::get(boost::vertex_name, graph_in);
+
+		BOOST_AUTO(parities, boost::make_one_bit_color_map(boost::num_vertices(graph_in), boost::get(boost::vertex_index, graph_in)));
+
+		float w = boost::stoer_wagner_min_cut(graph_in, boost::get(boost::edge_weight, graph_in), boost::parity_map(parities));
+
+		std::unordered_map<VertexIndex, VertexIndex> mappings;
+		VertexIndex counters[2] = {0, 0};
+
+		graphs_out.push_back(new Graph(1));
+		graphs_out.push_back(new Graph(1));
+		second_graphinds.push_back(std::vector<int>());
+		second_graphinds.push_back(std::vector<int>());
+		//std::cout << "One set of vertices consists of:" << std::endl;
+		bool flag;
+		Edge edge;
+		for (size_t i = 0; i < boost::num_vertices(graph_in); ++i) {
+			int first = boost::get(parities, i);
+			second_graphinds[first].push_back(graph_inds[i]);
+			// iterate adjacent edges
+			adjacency_iterator ai, ai_end;
+			for (tie(ai, ai_end) = boost::adjacent_vertices(i, graph_in);  ai != ai_end; ++ai) {
+				VertexIndex neighbor_index = boost::get(vertex_id, *ai);
+				int second = boost::get(parities, neighbor_index);
+				if (first == second && neighbor_index < i) {
+					tie(edge, flag) = boost::edge(i, neighbor_index, graph_in);
+					edge_weight_property weight = boost::get(edge_id, edge);
+					if (mappings.count(i) == 0) {
+						mappings[i] = counters[first]++;
+					}
+					if (mappings.count(neighbor_index) == 0) {
+						mappings[neighbor_index] = counters[first]++;
+					}
+					tie(edge, flag) = boost::add_edge(mappings[neighbor_index], mappings[i], weight, *graphs_out[first]);
+
+					typename boost::property_map<Graph, boost::vertex_name_t>::type vertex_name_first = boost::get(boost::vertex_name, *graphs_out[first]);
+					boost::get(vertex_name_first, mappings[i]) = boost::get(vertex_name, i);
+					boost::get(vertex_name_first, mappings[neighbor_index]) = boost::get(vertex_name, *ai);
+				}
+			}
+		}
+		return w;
+	}
+
+	float recursive_split(std::vector<Graph*> * graphs_out,std::vector<std::vector<int>> * graphinds_out, Graph * graph, std::vector<int> graph_inds){
+		if(boost::num_vertices(*graph) == 1){
+			graphs_out->push_back(graph);
+			graphinds_out->push_back(graph_inds);
+			return 0;
+		}
+
+		std::vector<Graph*> second_graphs;
+		std::vector<std::vector<int>> second_graphinds;
+		float w = graph_cut(second_graphs,second_graphinds,*graph,graph_inds);
+		if(w <= 0){
+			delete graph;
+			return 2*w + recursive_split(graphs_out, graphinds_out,second_graphs.front(),second_graphinds.front()) + recursive_split(graphs_out, graphinds_out, second_graphs.back(),second_graphinds.back());
+		}else{
+			graphs_out->push_back(graph);
+			graphinds_out->push_back(graph_inds);
+			delete second_graphs.front();
+			delete second_graphs.back();
+			return 0;
+		}
+	}
+
+	std::vector<int> partition_graph(std::vector< std::vector< float > > & scores){
+		int nr_data = scores.size();
+		Graph* graph = new Graph(nr_data);
+		std::vector<int> graph_inds;
+		graph_inds.resize(nr_data);
+
+		typename boost::property_map<Graph, boost::vertex_name_t>::type vertex_name = boost::get(boost::vertex_name, *graph);
+
+		float sum = 0;
+		for(int i = 0; i < nr_data; i++){
+			graph_inds[i] = i;
+			for(int j = i+1; j < nr_data; j++){
+				float weight = scores[i][j];
+				if(weight != 0){
+					sum += 2*weight;
+					edge_weight_property e = weight;
+					boost::add_edge(i, j, e, *graph);
+				}
+			}
+		}
+
+		std::vector<Graph*> * graphs_out = new std::vector<Graph*>();
+		std::vector<std::vector<int>> * graphinds_out = new std::vector<std::vector<int>>();
+		float best = sum-recursive_split(graphs_out,graphinds_out, graph,graph_inds );
+
+		std::vector<int> part;
+		part.resize(nr_data);
+		//int * part = new int[nr_data];
+		for(unsigned int i = 0; i < graphinds_out->size(); i++){
+			for(unsigned int j = 0; j < graphinds_out->at(i).size(); j++){
+				part[graphinds_out->at(i).at(j)] = i;
+			}
+		}
+		return part;
+	}
+
 	Eigen::Matrix4d getMatTest(const double * const camera, int mode){
 		Eigen::Matrix4d ret = Eigen::Matrix4d::Identity();
 		double rr [9];
@@ -97,6 +261,7 @@ namespace reglib{
 			const double & nx = Xn(0,i);
 			const double & ny = Xn(1,i);
 			const double & nz = Xn(2,i);
+
 			const double & weight = W(i);
 
 			double a = nz*sy - ny*sz;
@@ -151,8 +316,12 @@ namespace reglib{
 		ATA.coeffRef (33) = ATA.coeff (23);
 		ATA.coeffRef (34) = ATA.coeff (29);
 
+		for(int k = 0; k < 6; k++){
+			ATA(k,k) += 1;
+		}
 		// Solve A*x = b
 		Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
+		//Vector6d x = static_cast<Vector6d> (ATA.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(ATb));
 		Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)));
 
 		X = transformation*X;
@@ -162,7 +331,7 @@ namespace reglib{
 		Xn = transformation*Xn;
 	}
 
-	bool isconverged(std::vector<Eigen::Matrix4d> before, std::vector<Eigen::Matrix4d> after, double stopvalr, double stopvalt){
+    bool isconverged(std::vector<Eigen::Matrix4d> before, std::vector<Eigen::Matrix4d> after, double stopvalr, double stopvalt, bool verbose){
 		double change_trans = 0;
 		double change_rot = 0;
 		unsigned int nr_frames = after.size();
@@ -186,6 +355,10 @@ namespace reglib{
 		change_trans /= double(nr_frames*(nr_frames-1));
 		change_rot	 /= double(nr_frames*(nr_frames-1));
 
+        if(verbose){
+            printf("change_trans: %10.10f change_rot: %10.10f\n",change_trans,change_rot);
+        }
+
 		if(change_trans < stopvalt && change_rot < stopvalr){return true;}
 		else{return false;}
 	}
diff --git a/quasimodo/quasimodo_models/src/core/organized_edge_detection.h b/quasimodo/quasimodo_models/src/core/organized_edge_detection.h
new file mode 100644
index 00000000..41b54cda
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/organized_edge_detection.h
@@ -0,0 +1,431 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_
+#define PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_
+
+#include <pcl/pcl_base.h>
+#include <pcl/PointIndices.h>
+
+namespace pcl
+{
+  /** \brief OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, 
+    * and OrganizedEdgeFromRGBNormals find 3D edges from an organized point 
+    * cloud data. Given an organized point cloud, they will output a PointCloud 
+    * of edge labels and a vector of PointIndices.
+    * OrganizedEdgeBase accepts PCL_XYZ_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, and EDGELABEL_OCCLUDED.
+    * OrganizedEdgeFromRGB accepts PCL_RGB_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_RGB_CANNY.
+    * OrganizedEdgeFromNormals accepts PCL_XYZ_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_HIGH_CURVATURE.
+    * OrganizedEdgeFromRGBNormals accepts PCL_RGB_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, EDGELABEL_HIGH_CURVATURE, and EDGELABEL_RGB_CANNY.
+    *
+    * \author Changhyun Choi
+    */
+  template <typename PointT, typename PointLT>
+  class OrganizedEdgeBase : public PCLBase<PointT>
+  {
+    typedef typename pcl::PointCloud<PointT> PointCloud;
+    typedef typename PointCloud::Ptr PointCloudPtr;
+    typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+      
+    typedef typename pcl::PointCloud<PointLT> PointCloudL;
+    typedef typename PointCloudL::Ptr PointCloudLPtr;
+    typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
+
+    public:
+      typedef boost::shared_ptr<OrganizedEdgeBase<PointT, PointLT> > Ptr;
+      typedef boost::shared_ptr<const OrganizedEdgeBase<PointT, PointLT> > ConstPtr;
+      using PCLBase<PointT>::input_;
+      using PCLBase<PointT>::indices_;
+      using PCLBase<PointT>::initCompute;
+      using PCLBase<PointT>::deinitCompute;
+
+      /** \brief Constructor for OrganizedEdgeBase */
+      OrganizedEdgeBase ()
+        : th_depth_discon_ (0.02f)
+        , max_search_neighbors_ (50)
+		, detecting_edge_types_ (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_HIGH_CURVATURE)
+      {
+      }
+
+      /** \brief Destructor for OrganizedEdgeBase */
+      virtual
+      ~OrganizedEdgeBase ()
+      {
+      }
+
+      /** \brief Perform the 3D edge detection (edges from depth discontinuities)
+        * \param[out] labels a PointCloud of edge labels
+        * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+        */
+      void
+      compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+      
+      /** \brief Set the tolerance in meters for difference in depth values between neighboring points. */
+      inline void
+      setDepthDisconThreshold (const float th)
+      {
+        th_depth_discon_ = th;
+      }
+
+      /** \brief Get the tolerance in meters for difference in depth values between neighboring points. */
+      inline float
+      getDepthDisconThreshold () const
+      {
+        return (th_depth_discon_);
+      }
+
+      /** \brief Set the max search distance for deciding occluding and occluded edges. */
+      inline void
+      setMaxSearchNeighbors (const int max_dist)
+      {
+        max_search_neighbors_ = max_dist;
+      }
+
+      /** \brief Get the max search distance for deciding occluding and occluded edges. */
+      inline int
+      getMaxSearchNeighbors () const
+      {
+        return (max_search_neighbors_);
+      }
+
+      /** \brief Set the detecting edge types. */
+      inline void
+      setEdgeType (int edge_types)
+      {
+        detecting_edge_types_ = edge_types;
+      }
+
+      /** \brief Get the detecting edge types. */
+      inline int
+      getEdgeType () const
+      {
+        return detecting_edge_types_;
+      }
+      
+      enum {EDGELABEL_NAN_BOUNDARY=1, EDGELABEL_OCCLUDING=2, EDGELABEL_OCCLUDED=4, EDGELABEL_HIGH_CURVATURE=8, EDGELABEL_RGB_CANNY=16};
+      static const int num_of_edgetype_ = 5;
+
+    protected:
+      /** \brief Perform the 3D edge detection (edges from depth discontinuities) and assign point indices for each edge label
+        * \param[out] labels a PointCloud of edge labels
+        */
+      void
+      extractEdges (pcl::PointCloud<PointLT>& labels) const;
+      
+      /** \brief Assign point indices for each edge label
+        * \param[out] labels a PointCloud of edge labels
+        * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+        */
+      void
+      assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+      
+      struct Neighbor
+      {
+        Neighbor (int dx, int dy, int didx)
+        : d_x (dx)
+        , d_y (dy)
+        , d_index (didx)
+        {}
+        
+        int d_x;
+        int d_y;
+        int d_index; // = dy * width + dx: pre-calculated
+      };
+
+      /** \brief The tolerance in meters for difference in depth values between neighboring points 
+        * (The value is set for 1 meter and is adapted with respect to depth value linearly. 
+        * (e.g. 2.0*th_depth_discon_ in 2 meter depth)) 
+        */
+      float th_depth_discon_;
+
+      /** \brief The max search distance for deciding occluding and occluded edges */
+      int max_search_neighbors_;
+
+      /** \brief The bit encoded value that represents edge types to detect */
+      int detecting_edge_types_;
+  };
+
+  template <typename PointT, typename PointLT>
+  class OrganizedEdgeFromRGB : virtual public OrganizedEdgeBase<PointT, PointLT>
+  {
+    typedef typename pcl::PointCloud<PointT> PointCloud;
+    typedef typename PointCloud::Ptr PointCloudPtr;
+    typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+      
+    typedef typename pcl::PointCloud<PointLT> PointCloudL;
+    typedef typename PointCloudL::Ptr PointCloudLPtr;
+    typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
+
+    public:
+      using OrganizedEdgeBase<PointT, PointLT>::input_;
+      using OrganizedEdgeBase<PointT, PointLT>::indices_;
+      using OrganizedEdgeBase<PointT, PointLT>::initCompute;
+      using OrganizedEdgeBase<PointT, PointLT>::deinitCompute;
+      using OrganizedEdgeBase<PointT, PointLT>::detecting_edge_types_;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_RGB_CANNY;
+
+      /** \brief Constructor for OrganizedEdgeFromRGB */
+      OrganizedEdgeFromRGB ()
+        : OrganizedEdgeBase<PointT, PointLT> ()
+        , th_rgb_canny_low_ (40.0)
+        , th_rgb_canny_high_ (100.0)
+      {
+        this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY);
+      }
+
+      /** \brief Destructor for OrganizedEdgeFromRGB */
+      virtual
+      ~OrganizedEdgeFromRGB ()
+      {
+      }
+
+      /** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) and assign point indices for each edge label
+        * \param[out] labels a PointCloud of edge labels
+        * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+        */
+      void
+      compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+      
+      /** \brief Set the low threshold value for RGB Canny edge detection */
+      inline void
+      setRGBCannyLowThreshold (const float th)
+      {
+        th_rgb_canny_low_ = th;
+      }
+
+      /** \brief Get the low threshold value for RGB Canny edge detection */
+      inline float
+      getRGBCannyLowThreshold () const
+      {
+        return (th_rgb_canny_low_);
+      }
+
+      /** \brief Set the high threshold value for RGB Canny edge detection */
+      inline void
+      setRGBCannyHighThreshold (const float th)
+      {
+        th_rgb_canny_high_ = th;
+      }
+
+      /** \brief Get the high threshold value for RGB Canny edge detection */
+      inline float
+      getRGBCannyHighThreshold () const
+      {
+        return (th_rgb_canny_high_);
+      }
+
+    protected:
+      /** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge)
+        * \param[out] labels a PointCloud of edge labels
+        */
+      void
+      extractEdges (pcl::PointCloud<PointLT>& labels) const;
+
+      /** \brief The low threshold value for RGB Canny edge detection (default: 40.0) */
+      float th_rgb_canny_low_;
+
+      /** \brief The high threshold value for RGB Canny edge detection (default: 100.0) */
+      float th_rgb_canny_high_;
+  };
+
+  template <typename PointT, typename PointNT, typename PointLT>
+  class OrganizedEdgeFromNormals : virtual public OrganizedEdgeBase<PointT, PointLT>
+  {
+    typedef typename pcl::PointCloud<PointT> PointCloud;
+    typedef typename PointCloud::Ptr PointCloudPtr;
+    typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+      
+    typedef typename pcl::PointCloud<PointNT> PointCloudN;
+    typedef typename PointCloudN::Ptr PointCloudNPtr;
+    typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
+
+    typedef typename pcl::PointCloud<PointLT> PointCloudL;
+    typedef typename PointCloudL::Ptr PointCloudLPtr;
+    typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
+
+    public:
+      using OrganizedEdgeBase<PointT, PointLT>::input_;
+      using OrganizedEdgeBase<PointT, PointLT>::indices_;
+      using OrganizedEdgeBase<PointT, PointLT>::initCompute;
+      using OrganizedEdgeBase<PointT, PointLT>::deinitCompute;
+      using OrganizedEdgeBase<PointT, PointLT>::detecting_edge_types_;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_HIGH_CURVATURE;
+
+      /** \brief Constructor for OrganizedEdgeFromNormals */
+      OrganizedEdgeFromNormals () 
+        : OrganizedEdgeBase<PointT, PointLT> ()
+        , normals_ ()
+        , th_hc_canny_low_ (0.4f)
+        , th_hc_canny_high_ (1.1f)
+      {
+        this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_HIGH_CURVATURE);
+      }
+
+      /** \brief Destructor for OrganizedEdgeFromNormals */
+      virtual
+      ~OrganizedEdgeFromNormals ()
+      {
+      }
+
+      /** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) and assign point indices for each edge label
+        * \param[out] labels a PointCloud of edge labels
+        * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+        */
+      void
+      compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+
+      /** \brief Provide a pointer to the input normals.
+        * \param[in] normals the input normal cloud
+        */
+      inline void
+      setInputNormals (const PointCloudNConstPtr &normals) 
+      {
+        normals_ = normals;
+      }
+
+      /** \brief Get the input normals. */
+      inline PointCloudNConstPtr
+      getInputNormals () const
+      {
+        return (normals_);
+      }
+
+      /** \brief Set the low threshold value for high curvature Canny edge detection */
+      inline void
+      setHCCannyLowThreshold (const float th)
+      {
+        th_hc_canny_low_ = th;
+      }
+
+      /** \brief Get the low threshold value for high curvature Canny edge detection */
+      inline float
+      getHCCannyLowThreshold () const
+      {
+        return (th_hc_canny_low_);
+      }
+
+      /** \brief Set the high threshold value for high curvature Canny edge detection */
+      inline void
+      setHCCannyHighThreshold (const float th)
+      {
+        th_hc_canny_high_ = th;
+      }
+
+      /** \brief Get the high threshold value for high curvature Canny edge detection */
+      inline float
+      getHCCannyHighThreshold () const
+      {
+        return (th_hc_canny_high_);
+      }
+      
+    protected:
+      /** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions)
+        * \param[out] labels a PointCloud of edge labels
+        */
+      void
+      extractEdges (pcl::PointCloud<PointLT>& labels) const;
+
+      /** \brief A pointer to the input normals */
+      PointCloudNConstPtr normals_;
+
+      /** \brief The low threshold value for high curvature Canny edge detection (default: 0.4) */
+      float th_hc_canny_low_;
+
+      /** \brief The high threshold value for high curvature Canny edge detection (default: 1.1) */
+      float th_hc_canny_high_;
+  };
+
+  template <typename PointT, typename PointNT, typename PointLT>
+  class OrganizedEdgeFromRGBNormals : public OrganizedEdgeFromRGB<PointT, PointLT>, public OrganizedEdgeFromNormals<PointT, PointNT, PointLT>
+  {
+    typedef typename pcl::PointCloud<PointT> PointCloud;
+    typedef typename PointCloud::Ptr PointCloudPtr;
+    typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+      
+    typedef typename pcl::PointCloud<PointNT> PointCloudN;
+    typedef typename PointCloudN::Ptr PointCloudNPtr;
+    typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
+
+    typedef typename pcl::PointCloud<PointLT> PointCloudL;
+    typedef typename PointCloudL::Ptr PointCloudLPtr;
+    typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
+
+    public:
+      using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::input_;
+      using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::indices_;
+      using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::initCompute;
+      using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::deinitCompute;
+      using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::detecting_edge_types_;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_HIGH_CURVATURE;
+      using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_RGB_CANNY;
+      
+      /** \brief Constructor for OrganizedEdgeFromRGBNormals */
+      OrganizedEdgeFromRGBNormals () 
+        : OrganizedEdgeFromRGB<PointT, PointLT> ()
+        , OrganizedEdgeFromNormals<PointT, PointNT, PointLT> ()
+      {
+        this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY | EDGELABEL_HIGH_CURVATURE);
+      }
+
+      /** \brief Destructor for OrganizedEdgeFromRGBNormals */
+      virtual
+      ~OrganizedEdgeFromRGBNormals ()
+      {
+      }
+
+      /** \brief Perform the 3D edge detection (edges from depth discontinuities, RGB Canny edge, and high curvature regions) and assign point indices for each edge label
+        * \param[out] labels a PointCloud of edge labels
+        * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+        */
+      void
+      compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+  };
+}
+
+
+#include "organized_edge_detection.hpp"
+
+#endif //#ifndef PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_
diff --git a/quasimodo/quasimodo_models/src/core/organized_edge_detection.hpp b/quasimodo/quasimodo_models/src/core/organized_edge_detection.hpp
new file mode 100644
index 00000000..e3f2f95c
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/organized_edge_detection.hpp
@@ -0,0 +1,354 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
+#define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
+
+#include "2d/edge.h"
+#include "organized_edge_detection.h"
+#include <pcl/console/print.h>
+#include <pcl/console/time.h>
+
+/**
+ *  Directions: 1 2 3
+ *              0 x 4
+ *              7 6 5
+ * e.g. direction y means we came from pixel with label y to the center pixel x
+ */
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeBase<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+  pcl::Label invalid_pt;
+  invalid_pt.label = unsigned (0);
+  labels.points.resize (input_->points.size (), invalid_pt);
+  labels.width = input_->width;
+  labels.height = input_->height;
+  
+  extractEdges (labels);
+
+  assignLabelIndices (labels, label_indices);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeBase<PointT, PointLT>::assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+  const unsigned invalid_label = unsigned (0);
+  label_indices.resize (num_of_edgetype_);
+  for (unsigned idx = 0; idx < input_->points.size (); idx++)
+  {
+    if (labels[idx].label != invalid_label)
+    {
+      for (int edge_type = 0; edge_type < num_of_edgetype_; edge_type++)
+      {
+        if ((labels[idx].label >> edge_type) & 1)
+          label_indices[edge_type].indices.push_back (idx);
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeBase<PointT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
+{
+  if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED))
+  {
+    // Fill lookup table for next points to visit
+    const int num_of_ngbr = 8;
+    Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1),
+      Neighbor(-1, -1, -labels.width - 1), 
+      Neighbor( 0, -1, -labels.width    ),
+      Neighbor( 1, -1, -labels.width + 1),
+      Neighbor( 1,  0,                 1),
+      Neighbor( 1,  1,  labels.width + 1),
+      Neighbor( 0,  1,  labels.width    ),
+      Neighbor(-1,  1,  labels.width - 1)};
+
+    for (int row = 1; row < int(input_->height) - 1; row++)
+    {
+      for (int col = 1; col < int(input_->width) - 1; col++)
+      {
+        int curr_idx = row*int(input_->width) + col;
+        if (!pcl_isfinite (input_->points[curr_idx].z))
+          continue;
+
+        float curr_depth = fabsf (input_->points[curr_idx].z);
+
+        // Calculate depth distances between current point and neighboring points
+        std::vector<float> nghr_dist;
+        nghr_dist.resize (8);
+        bool found_invalid_neighbor = false;
+        for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
+        {
+          int nghr_idx = curr_idx + directions[d_idx].d_index;
+          assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
+          if (!pcl_isfinite (input_->points[nghr_idx].z))
+          {
+            found_invalid_neighbor = true;
+            break;
+          }
+          nghr_dist[d_idx] = curr_depth - fabsf (input_->points[nghr_idx].z);
+        }
+
+        if (!found_invalid_neighbor)
+        {
+          // Every neighboring points are valid
+          std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
+          std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
+          float nghr_dist_min = *min_itr;
+          float nghr_dist_max = *max_itr;
+          float dist_dominant = fabsf (nghr_dist_min) > fabsf (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
+		  if (fabsf (dist_dominant) > th_depth_discon_*fabsf (curr_depth*curr_depth))
+          {
+            // Found a depth discontinuity
+            if (dist_dominant > 0.f)
+            {
+              if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
+                labels[curr_idx].label |= EDGELABEL_OCCLUDED;
+            }
+            else
+            {
+              if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
+                labels[curr_idx].label |= EDGELABEL_OCCLUDING;
+            }
+          }
+        }
+        else
+        {
+          // Some neighboring points are not valid (nan points)
+          // Search for corresponding point across invalid points
+          // Search direction is determined by nan point locations with respect to current point
+          int dx = 0;
+          int dy = 0;
+          int num_of_invalid_pt = 0;
+          for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
+          {
+            int nghr_idx = curr_idx + directions[d_idx].d_index;
+            assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
+            if (!pcl_isfinite (input_->points[nghr_idx].z))
+            {
+              dx += directions[d_idx].d_x;
+              dy += directions[d_idx].d_y;
+              num_of_invalid_pt++;
+            }
+          }
+
+          // Search directions
+          assert (num_of_invalid_pt > 0);
+          float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
+          float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);
+
+          // Search for corresponding point across invalid points
+          float corr_depth = std::numeric_limits<float>::quiet_NaN ();
+          for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
+          {
+            int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
+            int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));
+
+            if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
+              break;
+
+            if (pcl_isfinite (input_->points[s_row*int(input_->width)+s_col].z))
+            {
+              corr_depth = fabsf (input_->points[s_row*int(input_->width)+s_col].z);
+              break;
+            }
+          }
+
+          if (!pcl_isnan (corr_depth))
+          {
+            // Found a corresponding point
+            float dist = curr_depth - corr_depth;
+            if (fabsf (dist) > th_depth_discon_*fabsf (curr_depth))
+            {
+              // Found a depth discontinuity
+              if (dist > 0.f)
+              {
+                if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
+                  labels[curr_idx].label |= EDGELABEL_OCCLUDED;
+              }
+              else
+              {
+                if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
+                  labels[curr_idx].label |= EDGELABEL_OCCLUDING;
+              }
+            }
+          } 
+          else
+          {
+            // Not found a corresponding point, just nan boundary edge
+            if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY)
+              labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY;
+          }
+        }
+      }
+    }
+  }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeFromRGB<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+  pcl::Label invalid_pt;
+  invalid_pt.label = unsigned (0);
+  labels.points.resize (input_->points.size (), invalid_pt);
+  labels.width = input_->width;
+  labels.height = input_->height;
+
+  OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
+  extractEdges (labels);
+
+  this->assignLabelIndices (labels, label_indices);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeFromRGB<PointT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
+{
+  if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
+  {
+    pcl::PointCloud<PointXYZI>::Ptr gray (new pcl::PointCloud<PointXYZI>);
+    gray->width = input_->width;
+    gray->height = input_->height;
+    gray->resize (input_->height*input_->width);
+
+    for (size_t i = 0; i < input_->size (); ++i)
+      (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3);
+
+    pcl::PointCloud<pcl::PointXYZIEdge> img_edge_rgb;
+    pcl::Edge<PointXYZI, pcl::PointXYZIEdge> edge;
+    edge.setInputCloud (gray);
+    edge.setHysteresisThresholdLow (th_rgb_canny_low_);
+    edge.setHysteresisThresholdHigh (th_rgb_canny_high_);
+    edge.detectEdgeCanny (img_edge_rgb);
+    
+    for (uint32_t row=0; row<labels.height; row++)
+    {
+      for (uint32_t col=0; col<labels.width; col++)
+      {
+        if (img_edge_rgb (col, row).magnitude == 255.f)
+          labels[row * labels.width + col].label |= EDGELABEL_RGB_CANNY;
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointNT, typename PointLT> void
+pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+  pcl::Label invalid_pt;
+  invalid_pt.label = unsigned (0);
+  labels.points.resize (input_->points.size (), invalid_pt);
+  labels.width = input_->width;
+  labels.height = input_->height;
+  
+  OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
+  extractEdges (labels);
+
+  this->assignLabelIndices (labels, label_indices);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointNT, typename PointLT> void
+pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
+{
+  if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
+  {
+
+    pcl::PointCloud<PointXYZI> nx, ny;
+    nx.width = normals_->width;
+    nx.height = normals_->height;
+    nx.resize (normals_->height*normals_->width);
+
+    ny.width = normals_->width;
+    ny.height = normals_->height;
+    ny.resize (normals_->height*normals_->width);
+
+    for (uint32_t row=0; row<normals_->height; row++)
+    {
+      for (uint32_t col=0; col<normals_->width; col++)
+      {
+        nx (col, row).intensity = normals_->points[row*normals_->width + col].normal_x;
+        ny (col, row).intensity = normals_->points[row*normals_->width + col].normal_y;
+      }
+    }
+
+    pcl::PointCloud<pcl::PointXYZIEdge> img_edge;
+    pcl::Edge<PointXYZI, pcl::PointXYZIEdge> edge;
+    edge.setHysteresisThresholdLow (th_hc_canny_low_);
+    edge.setHysteresisThresholdHigh (th_hc_canny_high_);
+    edge.canny (nx, ny, img_edge);
+
+    for (uint32_t row=0; row<labels.height; row++)
+    {
+      for (uint32_t col=0; col<labels.width; col++)
+      {
+        if (img_edge (col, row).magnitude == 255.f)
+          labels[row * labels.width + col].label |= EDGELABEL_HIGH_CURVATURE;
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointNT, typename PointLT> void
+pcl::OrganizedEdgeFromRGBNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+  pcl::Label invalid_pt;
+  invalid_pt.label = unsigned (0);
+  labels.points.resize (input_->points.size (), invalid_pt);
+  labels.width = input_->width;
+  labels.height = input_->height;
+  
+  OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
+  OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::extractEdges (labels);
+  OrganizedEdgeFromRGB<PointT, PointLT>::extractEdges (labels);
+
+  this->assignLabelIndices (labels, label_indices);
+}
+
+#define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT)               template class PCL_EXPORTS pcl::OrganizedEdgeBase<T,LT>;
+#define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT)            template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB<T,LT>;
+#define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT)     template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals<T,NT,LT>;
+#define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT)  template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals<T,NT,LT>;
+
+#endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
diff --git a/quasimodo/quasimodo_models/src/core/superpoint.cpp b/quasimodo/quasimodo_models/src/core/superpoint.cpp
new file mode 100644
index 00000000..d0b5c98e
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/core/superpoint.cpp
@@ -0,0 +1,29 @@
+#include "../../include/core/superpoint.h"
+
+namespace reglib
+{
+
+superpoint::superpoint(Eigen::Vector3f p, Eigen::Vector3f n, Eigen::VectorXf f, double pi, double fi, int id){
+	point = p;
+	normal = n;
+	feature = f;
+	point_information = pi;
+	feature_information = fi;
+	last_update_frame_id = id;
+}
+
+superpoint::~superpoint(){}
+
+void superpoint::merge(superpoint p, double weight){
+	double newpweight = weight*p.point_information		+ point_information;
+	double newfweight = weight*p.feature_information	+ feature_information;
+	point	= weight*p.point_information*p.point		+ point_information*point;
+	normal	= weight*p.point_information*p.normal		+ point_information*normal;
+	normal.normalize();
+	point /= newpweight;
+	point_information = newpweight;
+	feature_information = newfweight;
+	last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id);
+}
+}
+
diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp
index 7a88f823..3bd032af 100644
--- a/quasimodo/quasimodo_models/src/model/Model.cpp
+++ b/quasimodo/quasimodo_models/src/model/Model.cpp
@@ -1,5 +1,6 @@
 #include "model/Model.h"
 #include <map>
+#include <sys/stat.h>
 
 namespace reglib
 {
@@ -12,6 +13,10 @@ Model::Model(){
     score = 0;
     id = model_id_counter++;
     last_changed = -1;
+	savePath = "";
+	soma_id = "";
+	pointspath = "";
+	parrent = 0;
 }
 
 Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){
@@ -27,19 +32,288 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){
 
     relativeposes.push_back(pose);
     frames.push_back(frame);
-    modelmasks.push_back(new ModelMask(mask));
-    recomputeModelPoints();
+	modelmasks.push_back(new ModelMask(mask));
+
+	savePath = "";
+	soma_id = "";
+	pointspath = "";
+	parrent = 0;
+    //recomputeModelPoints();
 }
 
-void Model::recomputeModelPoints(){
-//	for(unsigned int i = 0; i < frames.size(); i++){
-//		bool res = testFrame(i);
-//	}
+void Model::getData(std::vector<Eigen::Matrix4d> & po, std::vector<RGBDFrame*> & fr, std::vector<ModelMask*> & mm, Eigen::Matrix4d p){
+	for(unsigned int i = 0; i < frames.size(); i++){
+		fr.push_back(frames[i]);
+		mm.push_back(modelmasks[i]);
+		po.push_back(p*relativeposes[i]);
+	}
 
-    points.clear();
-    for(unsigned int i = 0; i < frames.size(); i++){
-        addPointsToModel(frames[i],modelmasks[i],relativeposes[i]);
-    }
+	for(unsigned int i = 0; i < submodels.size(); i++){
+		submodels[i]->getData(po,fr,mm, p*submodels_relativeposes[i]);
+	}
+}
+
+void Model::addSuperPoints(vector<superpoint> & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer){
+	Camera * camera				= frame->camera;
+	const unsigned int width	= camera->width;
+	const unsigned int height	= camera->height;
+	unsigned long nr_pixels		= width*height;
+
+	std::vector<float> sumw;
+	sumw.resize(nr_pixels);
+	for(unsigned long ind = 0; ind < nr_pixels;ind++){sumw[ind] = 0;}
+
+	std::vector<bool> isfused;
+	isfused.resize(nr_pixels);
+	for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;}
+
+	bool * maskvec							= modelmask->maskvec;
+	std::vector<ReprojectionResult> rr_vec	= frame->getReprojections(spvec,p.inverse(),modelmask->maskvec,false);
+	std::vector<superpoint> framesp			= frame->getSuperPoints(p);
+
+	unsigned long nr_rr = rr_vec.size();
+	if(nr_rr > 10){
+		std::vector<double> residualsZ;
+		double stdval = 0;
+		for(unsigned long ind = 0; ind < nr_rr;ind++){
+			ReprojectionResult & rr = rr_vec[ind];
+			superpoint & src_p =   spvec[rr.src_ind];
+			superpoint & dst_p = framesp[rr.dst_ind];
+			double src_variance = 1.0/src_p.point_information;
+			double dst_variance = 1.0/dst_p.point_information;
+			double total_variance = src_variance+dst_variance;
+			double total_stdiv = sqrt(total_variance);
+			double d = rr.residualZ;
+			residualsZ.push_back(d/total_stdiv);
+			stdval += residualsZ.back()*residualsZ.back();
+		}
+		stdval = sqrt(stdval/double(nr_rr));
+
+		DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2();
+		func->zeromean				= true;
+		func->maxp					= 0.99;
+		func->startreg				= 0.001;
+		func->debugg_print			= false;
+		func->maxd					= 0.1;
+		func->startmaxd				= func->maxd;
+		func->histogram_size		= 100;
+		func->starthistogram_size	= func->histogram_size;
+		func->stdval2				= stdval;
+		func->maxnoise				= stdval;
+		func->reset();
+		((DistanceWeightFunction2*)func)->computeModel(residualsZ);
+
+		for(unsigned long ind = 0; ind < nr_rr;ind++){
+			double p = func->getProb(residualsZ[ind]);
+			if(p > 0.5){sumw[rr_vec[ind].dst_ind] += p;}
+		}
+
+		for(unsigned long ind = 0; ind < nr_rr;ind++){
+			double p = func->getProb(residualsZ[ind]);
+			if(p > 0.5){
+				ReprojectionResult & rr = rr_vec[ind];
+				superpoint & src_p =   spvec[rr.src_ind];
+				superpoint & dst_p = framesp[rr.dst_ind];
+				float weight = p/sumw[rr.dst_ind];
+				src_p.merge(dst_p,weight);
+			}
+		}
+		delete func;
+	}
+
+	for(unsigned long ind = 0; ind < nr_pixels;ind++){
+		if(maskvec[ind] != 0 && sumw[ind] < 0.5){
+			superpoint & sp = framesp[ind];
+			if(sp.point_information > 0){
+				spvec.push_back(sp);
+			}
+		}
+	}
+
+	if(viewer != 0){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = getPointCloudFromVector(spvec,1);
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud), "model");
+		viewer->spin();
+		viewer->removeAllPointClouds();
+	}
+}
+
+void Model::addAllSuperPoints(std::vector<superpoint> & spvec, Eigen::Matrix4d pose, boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer){
+	for(unsigned int i = 0; i < frames.size(); i++){
+		//printf("addAllSuperPoints: %i\n",i);
+		addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i], viewer);
+	}
+
+	for(unsigned int i = 0; i < submodels.size(); i++){
+		submodels[i]->addAllSuperPoints(spvec, pose*submodels_relativeposes[i], viewer);
+	}
+}
+
+void Model::recomputeModelPoints(Eigen::Matrix4d pose, boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer){
+	double startTime = getTime();
+	points.clear();
+	addAllSuperPoints(points,pose,viewer);
+	printf("recomputeModelPoints time: %5.5fs total points: %i \n",getTime()-startTime,int(points.size()));
+}
+
+void Model::showHistory(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer){
+	//viewer->setBackgroundColor(0.01*(rand()%100),0.01*(rand()%100),0.01*(rand()%100));
+	for(unsigned int i = 0; i < submodels.size(); i++){
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr room_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+
+		for(unsigned int j = 0; j < submodels[i]->frames.size(); j++){
+			bool * maskvec					= submodels[i]->modelmasks[j]->maskvec;
+			unsigned char  * rgbdata		= (unsigned char	*)(submodels[i]->frames[j]->rgb.data);
+			unsigned short * depthdata		= (unsigned short	*)(submodels[i]->frames[j]->depth.data);
+			float		   * normalsdata	= (float			*)(submodels[i]->frames[j]->normals.data);
+
+			Camera * camera				= submodels[i]->frames[j]->camera;
+			const unsigned int width	= camera->width;
+			const unsigned int height	= camera->height;
+			const float idepth			= camera->idepth_scale;
+			const float cx				= camera->cx;
+			const float cy				= camera->cy;
+			const float ifx				= 1.0/camera->fx;
+			const float ify				= 1.0/camera->fy;
+
+			Eigen::Matrix4d p = submodels[i]->relativeposes[j];
+
+			float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+			float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+			float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+			for(unsigned int w = 0; w < width; w++){
+				for(unsigned int h = 0; h < height;h++){
+					int ind = h*width+w;
+					if(!maskvec[ind]){
+						float z = idepth*float(depthdata[ind]);
+						float nx = normalsdata[3*ind+0];
+
+						if(z > 0 && nx != 2){
+							float ny = normalsdata[3*ind+1];
+							float nz = normalsdata[3*ind+2];
+
+							float x = (w - cx) * z * ifx;
+							float y = (h - cy) * z * ify;
+
+							pcl::PointXYZRGB po;
+							po.x	= m00*x + m01*y + m02*z + m03;
+							po.y	= m10*x + m11*y + m12*z + m13;
+							po.z	= m20*x + m21*y + m22*z + m23;
+//							float pnx	= m00*nx + m01*ny + m02*nz;
+//							float pny	= m10*nx + m11*ny + m12*nz;
+//							float pnz	= m20*nx + m21*ny + m22*nz;
+
+							po.b = rgbdata[3*ind+0];
+							po.g = rgbdata[3*ind+1];
+							po.r = rgbdata[3*ind+2];
+
+
+							room_cloud->points.push_back(po);
+						}
+					}
+				}
+			}
+		}
+
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGB> (room_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(room_cloud), "room_cloud");
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = submodels[i]->getPCLcloud(1, true);
+		for(unsigned int j = 0; j < cloud->points.size(); j++){
+			cloud->points[j].r = 255;
+			cloud->points[j].g = 0;
+			cloud->points[j].b = 0;
+		}
+		viewer->addPointCloud<pcl::PointXYZRGB> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(cloud), "submodel");
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2 = getPCLcloud(1, true);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tcloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
+		pcl::transformPointCloud (*cloud2, *tcloud, Eigen::Affine3d(submodels_relativeposes[i]).inverse());
+		viewer->addPointCloud<pcl::PointXYZRGB> (tcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(tcloud), "model");
+		viewer->spin();
+	}
+}
+
+std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> Model::getHistory(){
+std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> ret;
+	for(unsigned int i = 0; i < submodels.size(); i++){
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr room_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+		for(unsigned int j = 0; j < submodels[i]->frames.size(); j++){
+			bool * maskvec					= submodels[i]->modelmasks[j]->maskvec;
+			unsigned char  * rgbdata		= (unsigned char	*)(submodels[i]->frames[j]->rgb.data);
+			unsigned short * depthdata		= (unsigned short	*)(submodels[i]->frames[j]->depth.data);
+			float		   * normalsdata	= (float			*)(submodels[i]->frames[j]->normals.data);
+
+			Camera * camera				= submodels[i]->frames[j]->camera;
+			const unsigned int width	= camera->width;
+			const unsigned int height	= camera->height;
+			const float idepth			= camera->idepth_scale;
+			const float cx				= camera->cx;
+			const float cy				= camera->cy;
+			const float ifx				= 1.0/camera->fx;
+			const float ify				= 1.0/camera->fy;
+
+			Eigen::Matrix4d p = submodels[i]->relativeposes[j];
+
+			float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+			float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+			float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+			for(unsigned int w = 0; w < width; w++){
+				for(unsigned int h = 0; h < height;h++){
+					int ind = h*width+w;
+					if(!maskvec[ind]){
+						float z = idepth*float(depthdata[ind]);
+						float nx = normalsdata[3*ind+0];
+
+						if(z > 0 && nx != 2){
+							float ny = normalsdata[3*ind+1];
+							float nz = normalsdata[3*ind+2];
+
+							float x = (w - cx) * z * ifx;
+							float y = (h - cy) * z * ify;
+
+							pcl::PointXYZRGB po;
+							po.x	= m00*x + m01*y + m02*z + m03;
+							po.y	= m10*x + m11*y + m12*z + m13;
+							po.z	= m20*x + m21*y + m22*z + m23;
+//							float pnx	= m00*nx + m01*ny + m02*nz;
+//							float pny	= m10*nx + m11*ny + m12*nz;
+//							float pnz	= m20*nx + m21*ny + m22*nz;
+
+							po.b = rgbdata[3*ind+0];
+							po.g = rgbdata[3*ind+1];
+							po.r = rgbdata[3*ind+2];
+
+
+							room_cloud->points.push_back(po);
+						}
+					}
+				}
+			}
+		}
+
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = submodels[i]->getPCLcloud(1, true);
+		for(unsigned int j = 0; j < cloud->points.size(); j++){
+			cloud->points[j].r = 255;
+			cloud->points[j].g = 0;
+			cloud->points[j].b = 0;
+		}
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2 = getPCLcloud(1, true);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tcloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
+		pcl::transformPointCloud (*cloud2, *tcloud, Eigen::Affine3d(submodels_relativeposes[i]).inverse());
+
+
+		*room_cloud += *cloud;
+		*room_cloud += *tcloud;
+		ret.push_back(room_cloud);
+	}
+	return ret;
 }
 
 void Model::addPointsToModel(RGBDFrame * frame, ModelMask * modelmask, Eigen::Matrix4d p){
@@ -193,6 +467,13 @@ void Model::merge(Model * model, Eigen::Matrix4d p){
         frames.push_back(model->frames[i]);
         modelmasks.push_back(model->modelmasks[i]);
     }
+
+	for(unsigned int i = 0; i < model->submodels.size(); i++){
+		submodels_relativeposes.push_back(p * model->submodels_relativeposes[i]);
+		submodels.push_back(model->submodels[i]);
+		model->submodels[i]->parrent = this;
+	}
+
     recomputeModelPoints();
 }
 
@@ -242,6 +523,38 @@ CloudData * Model::getCD(unsigned int target_points){
 
 Model::~Model(){}
 
+void Model::fullDelete(){
+    points.clear();
+    all_keypoints.clear();
+    all_descriptors.clear();
+    relativeposes.clear();
+	for(size_t i = 0; i < frames.size(); i++){
+        delete frames[i]->camera;
+        delete frames[i];
+    }
+    frames.clear();
+
+    for(size_t i = 0; i < modelmasks.size(); i++){delete modelmasks[i];}
+    modelmasks.clear();
+
+    rep_relativeposes.clear();
+    rep_frames.clear();
+    rep_modelmasks.clear();
+
+    total_scores = 0;
+    scores.clear();
+
+    for(size_t i = 0; i < submodels.size(); i++){
+        submodels[i]->fullDelete();
+        delete submodels[i];
+    }
+    submodels.clear();
+
+    submodels_relativeposes.clear();
+    submodels_scores.clear();
+//	delete this;
+}
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr Model::getPCLnormalcloud(int step, bool color){
     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
     for(unsigned int i = 0; i < points.size(); i+=step){
@@ -259,9 +572,9 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr Model::getPCLnormalcloud(int step,
             p.g = 255;
             p.r =   0;
         }else{
-            p.r = sp.feature(0);
+			p.b = sp.feature(0);
             p.g = sp.feature(1);
-            p.b = sp.feature(2);
+			p.r = sp.feature(2);
         }
         cloud_ptr->points.push_back(p);
     }
@@ -270,115 +583,37 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr Model::getPCLnormalcloud(int step,
 
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr Model::getPCLcloud(int step, bool color){
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
-    //	if(color){
-    //		for(unsigned int i = 0; i < points.size(); i+=step){
-    //			superpoint & sp = points[i];
-    //			pcl::PointXYZRGB p;
-    //			p.x = sp.point(0);
-    //			p.y = sp.point(1);
-    //			p.z = sp.point(2);
-    //			if(color){
-    //				p.b =   0;
-    //				p.g = 255;
-    //				p.r =   0;
-    //			}else{
-    //				p.r = sp.feature(0);
-    //				p.g = sp.feature(1);
-    //				p.b = sp.feature(2);
-    //			}
-    //			cloud_ptr->points.push_back(p);
-    //		}
-    //	}else{
-    std::map<int,int> mymapR;
-    std::map<int,int> mymapG;
-    std::map<int,int> mymapB;
-    for(unsigned int f = 0; f < frames.size(); f++){
-        //unsigned char  * maskdata		= (unsigned char	*)(masks[f].data);
-        bool * maskvec = modelmasks[f]->maskvec;
-        unsigned char  * rgbdata		= (unsigned char	*)(frames[f]->rgb.data);
-        unsigned short * depthdata		= (unsigned short	*)(frames[f]->depth.data);
-        float		   * normalsdata	= (float			*)(frames[f]->normals.data);
-
-        Eigen::Matrix4d p = relativeposes[f];
-
-        int sweepid = modelmasks[f]->sweepid;
-
-        int pr,pg,pb;
-        if(sweepid == -1){
-            pr = rand()%256;
-            pg = rand()%256;
-            pb = rand()%256;
-        }else{
-            if (mymapR.count(sweepid)==0){
-                mymapR[sweepid] = rand()%256;
-                mymapG[sweepid] = rand()%256;
-                mymapB[sweepid] = rand()%256;
-            }
-            pr = mymapR[sweepid];
-            pg = mymapG[sweepid];
-            pb = mymapB[sweepid];
-        }
-
-        float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
-        float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
-        float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
-
-        Camera * camera				= frames[f]->camera;
-        const unsigned int width	= camera->width;
-        const unsigned int height	= camera->height;
-        const float idepth			= camera->idepth_scale;
-        const float cx				= camera->cx;
-        const float cy				= camera->cy;
-        const float ifx				= 1.0/camera->fx;
-        const float ify				= 1.0/camera->fy;
-
-
-        for(unsigned int w = 0; w < width; w++){
-            for(unsigned int h = 0; h < height;h++){
-                int ind = h*width+w;
-                //if(maskdata[ind] == 255){
-                if(maskvec[ind]){
-                    float z = idepth*float(depthdata[ind]);
-                    if(z > 0){
-                        float x = (w - cx) * z * ifx;
-                        float y = (h - cy) * z * ify;
-
-                        float px	= m00*x + m01*y + m02*z + m03;
-                        float py	= m10*x + m11*y + m12*z + m13;
-                        float pz	= m20*x + m21*y + m22*z + m23;
-
-                        pcl::PointXYZRGB p;
-                        p.x = px;
-                        p.y = py;
-                        p.z = pz;
-                        if(color){
-                            p.b = rgbdata[3*ind+0];
-                            p.g = rgbdata[3*ind+1];
-                            p.r = rgbdata[3*ind+2];
-                        }else{
-                            p.b = pb;
-                            p.g = pg;
-                            p.r = pr;
-                        }
-                        cloud_ptr->points.push_back(p);
-                    }
-                }
-            }
-        }
-    }
-    //}
-    return cloud_ptr;
+	for(unsigned int i = 0; i < points.size(); i+=step){
+		superpoint & sp = points[i];
+		pcl::PointXYZRGB p;
+		p.x = sp.point(0);
+		p.y = sp.point(1);
+		p.z = sp.point(2);
+
+		if(color){
+			p.b =   0;
+			p.g = 255;
+			p.r =   0;
+		}else{
+			p.b = sp.feature(0);
+			p.g = sp.feature(1);
+			p.r = sp.feature(2);
+		}
+		cloud_ptr->points.push_back(p);
+	}
+	return cloud_ptr;
 }
 
 void Model::save(std::string path){
-    unsigned long buffersize = (1+1+16*frames.size()+1+frames.size()*frames.size()+1*frames.size())*sizeof(double);
+    /*
+    unsigned long buffersize = (1+1+16*frames.size());//+1+frames.size()*frames.size()+1*frames.size())*sizeof(double);
     char* buffer = new char[buffersize];
     double * buffer_double = (double *)buffer;
     unsigned long * buffer_long = (unsigned long *)buffer;
 
     int counter = 0;
     buffer_long[counter++] = frames.size();
-    buffer_double[counter++] = score;
+    //buffer_double[counter++] = score;
     for(unsigned int f = 0; f < frames.size(); f++){
         Eigen::Matrix4d pose = relativeposes[f];
         for(int i = 0; i < 4; i++){
@@ -405,11 +640,76 @@ void Model::save(std::string path){
     outfile.write (buffer,buffersize);
     outfile.close();
     delete[] buffer;
+*/
+printf("%i\n",__LINE__);
+    char buf [1024];
+    struct stat sb;
 
+    if (stat(path.c_str(), &sb) == 0 && S_ISDIR(sb.st_mode)){
+        sprintf(buf,"rm -r %s",path.c_str());
+        system(buf);
+    }
+
+    sprintf(buf,"mkdir %s",path.c_str());
+    system(buf);
 
     ofstream posesfile;
-    posesfile.open (path+"/poses.txt");
+    posesfile.open (path+"/relativeposes.txt");
+    for(unsigned int f = 0; f < frames.size(); f++){
+        posesfile << relativeposes[f] << endl <<endl;
+
+printf("%i\n",__LINE__);
+        sprintf(buf,"%s/frame_%08i",path.c_str(),f);
+        frames[f]->save(std::string(buf));
+
+        sprintf(buf,"%s/frame_%08i.pcd",path.c_str(),f);
+        frames[f]->savePCD(std::string(buf),relativeposes[f]);
+printf("%i\n",__LINE__);
+        sprintf(buf,"%s/modelmask_%08i.png",path.c_str(),f);
+        cv::imwrite( buf, modelmasks[f]->getMask() );
+    }
+    posesfile.close();
+printf("%i\n",__LINE__);
+
+    ofstream submodels_posesfile;
+    submodels_posesfile.open (path+"/submodels_relativeposes.txt");
+    for(unsigned int i = 0; i < submodels.size(); i++){
+        submodels_posesfile << submodels_relativeposes[i] << endl <<endl;
+        sprintf(buf,"%s/submodel%08i",path.c_str(),i);
+		printf("%i\n",__LINE__);
+        submodels[i]->save(std::string(buf));
+		printf("%i\n",__LINE__);
+    }
+    submodels_posesfile.close();
+    /*
+    for(unsigned int f = 0; f < frames.size(); f++){
+        char buf [1024];
+
+        sprintf(buf,"%s/views/cloud_%08i.pcd",path.c_str(),f);
+        frames[f]->savePCD(std::string(buf));
+
+        sprintf(buf,"%s/views/pose_%08i.txt",path.c_str(),f);
+        ofstream posefile;
+        posefile.open (buf);
+
+        Eigen::Matrix4f eigen_tr(relativeposes[f].cast<float>());
+        Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " ", "", "", "", "");
+        posefile << eigen_tr.format(CommaInitFmt)<<endl;
+        posefile.close();
 
+        bool * maskvec = modelmasks[f]->maskvec;
+        sprintf(buf,"%s/views/object_indices_%08i.txt",path.c_str(),f);
+        ofstream indfile;
+        indfile.open (buf);
+        for(int i = 0; i < 640*480; i++){
+            if(maskvec[i]){indfile << i << std::endl;}
+        }
+        indfile.close();
+        //object_indices_00000030.txt
+
+    }
+    */
+/*
     ofstream raresfile;
     raresfile.open (path+"/raresposes.txt");
     Eigen::Matrix4f eigen_tr(Eigen::Matrix4f::Identity() );
@@ -461,6 +761,7 @@ void Model::save(std::string path){
         //object_indices_00000030.txt
 
     }
+    */
 }
 
 Model * Model::load(Camera * cam, std::string path){
diff --git a/quasimodo/quasimodo_models/src/model/ModelMask.cpp b/quasimodo/quasimodo_models/src/model/ModelMask.cpp
index c3294bce..d1da1c71 100644
--- a/quasimodo/quasimodo_models/src/model/ModelMask.cpp
+++ b/quasimodo/quasimodo_models/src/model/ModelMask.cpp
@@ -9,8 +9,9 @@ namespace reglib
 
 int ModelMask_id = 0;
 
-ModelMask::ModelMask(cv::Mat mask_){
+ModelMask::ModelMask(cv::Mat mask_, std::string label_){
 	mask = mask_;
+	label = label_;
 	sweepid = -1;
 
 	id = ModelMask_id++;
@@ -25,24 +26,57 @@ ModelMask::ModelMask(cv::Mat mask_){
 
 
 
-	for(unsigned int w = 1; w < 639; w++){
-		for(unsigned int h = 1; h < 479; h++){
-			if(maskdata[h*640+w] != 0){
-				testw.push_back(w);
-				testh.push_back(h);
-			}
-		}
-	}
+//	for(unsigned int w = 1; w < 639; w++){
+//		for(unsigned int h = 1; h < 479; h++){
+//			if(maskdata[h*640+w] != 0){
+//				testw.push_back(w);
+//				testh.push_back(h);
+//			}
+//		}
+//	}
 
-	for(unsigned int w = 1; w < 639; w++){
-		for(unsigned int h = 1; h < 479; h++){
+//	cv::Mat combined;
+//	combined.create(height,width,CV_8UC3);
+//	unsigned char * combineddata = (unsigned char *)combined.data;
+//	for(int j = 0; j < 3*width*height; j++){combineddata[j] = 0;}
+	//cv::Mat combined = mask.clone();
+
+
+	int step = 0;
+	for(int w = step; w < 639-step; w++){
+		for(int h = step; h < 479-step; h++){
 			if(maskdata[h*640+w] != 0){
-				testw.push_back(w);
-				testh.push_back(h);
+//				combineddata[3*(h*640+w) + 0] = 0;
+//				combineddata[3*(h*640+w) + 1] = 0;
+//				combineddata[3*(h*640+w) + 2] = 255;
+//				printf("%i %i-> ",w,h);
+				bool isok =  true;
+				for(int w2 = w-step; w2 <= w+step; w2++){
+					for(int h2 = h-step; h2 <= h+step; h2++){
+//						printf("(%i %i , %i -> %i) ",w2,h2,isok,maskdata[h2*640+w2] != 0);
+						isok = isok && (maskdata[h2*640+w2] != 0);
+					}
+				}
+//				printf("\n");
+//				printf("\n");
+//				printf("\n");
+//				printf("\n");
+				if(isok){
+//					combineddata[3*(h*640+w) + 0] = 0;
+//					combineddata[3*(h*640+w) + 1] = 255;
+//					combineddata[3*(h*640+w) + 2] = 0;
+					testw.push_back(w);
+					testh.push_back(h);
+				}
 			}
 		}
 	}
 
+//	cv::imshow( "combined", combined );
+//	cv::waitKey(0);
+
+
+//exit(0);
 	for(unsigned int i = 0; i < testw.size(); i++){
 		int ind = rand() % testw.size();
 		int tw = testw[i];
diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp
index 4b6fe1b9..bdba461e 100644
--- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp
+++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp
@@ -1,365 +1,4709 @@
 #include "modelupdater/ModelUpdater.h"
 
-#include <boost/graph/incremental_components.hpp>
-#include <boost/graph/graphviz.hpp>
-#include <boost/graph/graph_utility.hpp>
-#include <boost/graph/adjacency_list.hpp>
-#include <boost/graph/iteration_macros.hpp>
-#include <boost/graph/graph_traits.hpp>
-#include <boost/graph/one_bit_color_map.hpp>
-#include <boost/graph/stoer_wagner_min_cut.hpp>
-#include <boost/range/adaptor/reversed.hpp>
-#include <boost/graph/copy.hpp>
 #include <unordered_map>
+#include "opencv2/core/core.hpp"
+#include "opencv2/features2d/features2d.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+#include "core/Util.h"
 
-typedef boost::property<boost::edge_weight_t, float> edge_weight_property;
-typedef boost::property<boost::vertex_name_t, size_t> vertex_name_property;
-using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, vertex_name_property, edge_weight_property>;
-using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
-using VertexIndex = boost::graph_traits<Graph>::vertices_size_type;
-using Edge = boost::graph_traits<Graph>::edge_descriptor;
-using Components = boost::component_index<VertexIndex>;
+
+namespace gc
+{
+#include "graphcuts/graph.cpp"
+#include "graphcuts/maxflow.cpp"
+}
 
 using namespace std;
 
-namespace reglib
-{
+namespace reglib
+{
+
+std::vector<int> ModelUpdater::getPartition(std::vector< std::vector< float > > & scores, int dims, int nr_todo, double timelimit){return partition_graph(scores);}
+
+ModelUpdater::ModelUpdater(){
+	occlusion_penalty = 10;
+	massreg_timeout = 60;
+	model = 0;
+	show_init_lvl = 0;//init show
+	show_refine_lvl = 0;//refine show
+	show_scoring = false;//fuse scoring show
+}
+
+ModelUpdater::ModelUpdater(Model * model_){
+	occlusion_penalty = 10;
+	massreg_timeout = 60;
+	show_init_lvl = 0;//init show
+	show_refine_lvl = 0;//refine show
+	show_scoring = false;//fuse scoring show
+	model = model_;
+}
+ModelUpdater::~ModelUpdater(){}
+
+FusionResults ModelUpdater::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){return FusionResults();}
+
+void ModelUpdater::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanity){
+	for(unsigned int i = 0; i < model2->frames.size();i++){
+		model->frames.push_back(model2->frames[i]);
+		model->modelmasks.push_back(model2->modelmasks[i]);
+		model->relativeposes.push_back(guess*model2->relativeposes[i]);
+	}
+}
+
+UpdatedModels ModelUpdater::fuseData(FusionResults * f, Model * model1,Model * model2){return UpdatedModels();}
+
+bool ModelUpdater::isRefinementNeeded(){
+	vector<vector < OcclusionScore > > occlusionScores;
+	occlusionScores.resize(model->frames.size());
+	for(unsigned int i = 0; i < model->frames.size(); i++){occlusionScores[i].resize(model->frames.size());}
+	for(unsigned int i = 0; i < model->frames.size(); i++){
+		for(unsigned int j = i+1; j < model->frames.size(); j++){
+			Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j];
+			occlusionScores[j][i]		= computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,false);
+			occlusionScores[i][j]		= computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,false);
+		}
+	}
+	std::vector<std::vector < float > > scores = getScores(occlusionScores);
+	std::vector<int> partition = getPartition(scores,2,5,2);
+	bool failed = false;
+	for(unsigned int i = 0; i < partition.size(); i++){failed = failed | (partition[i]!=0);}
+	return failed;
+}
+
+OcclusionScore ModelUpdater::computeOcclusionScore(vector<superpoint> & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step,  bool debugg){
+	OcclusionScore oc;
+
+	unsigned char  * dst_maskdata		= (unsigned char	*)(cm->mask.data);
+	unsigned char  * dst_rgbdata		= (unsigned char	*)(cf->rgb.data);
+	unsigned short * dst_depthdata		= (unsigned short	*)(cf->depth.data);
+	float		   * dst_normalsdata	= (float			*)(cf->normals.data);
+
+	float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3);
+	float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3);
+	float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3);
+
+	Camera * dst_camera				= cf->camera;
+	const unsigned int dst_width	= dst_camera->width;
+	const unsigned int dst_height	= dst_camera->height;
+	const float dst_idepth			= dst_camera->idepth_scale;
+	const float dst_cx				= dst_camera->cx;
+	const float dst_cy				= dst_camera->cy;
+	const float dst_fx				= dst_camera->fx;
+	const float dst_fy				= dst_camera->fy;
+	const float dst_ifx				= 1.0/dst_camera->fx;
+	const float dst_ify				= 1.0/dst_camera->fy;
+	const unsigned int dst_width2	= dst_camera->width  - 2;
+	const unsigned int dst_height2	= dst_camera->height - 2;
+
+	int nr_data = spvec.size();
+
+	std::vector<float> residuals;
+	std::vector<int> debugg_src_inds;
+	std::vector<int> debugg_dst_inds;
+	std::vector<float> weights;
+	residuals.reserve(nr_data);
+	if(debugg){
+		debugg_src_inds.reserve(nr_data);
+		debugg_dst_inds.reserve(nr_data);
+	}
+	weights.reserve(nr_data);
+
+	for(unsigned int ind = 0; ind < spvec.size();ind+=step){
+		superpoint & sp = spvec[ind];
+
+		float src_x = sp.point(0);
+		float src_y = sp.point(1);
+		float src_z = sp.point(2);
+
+		float src_nx = sp.normal(0);
+		float src_ny = sp.normal(1);
+		float src_nz = sp.normal(2);
+
+		float point_information = sp.point_information;
+
+
+		float tx	= m00*src_x + m01*src_y + m02*src_z + m03;
+		float ty	= m10*src_x + m11*src_y + m12*src_z + m13;
+		float tz	= m20*src_x + m21*src_y + m22*src_z + m23;
+
+		float itz	= 1.0/tz;
+		float dst_w	= dst_fx*tx*itz + dst_cx;
+		float dst_h	= dst_fy*ty*itz + dst_cy;
+
+		if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
+			unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
+
+			float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+			float dst_nx = dst_normalsdata[3*dst_ind+0];
+			if(dst_z > 0 && dst_nx != 2){
+				//if(dst_detdata[dst_ind] != 0){continue;}
+				float dst_ny = dst_normalsdata[3*dst_ind+1];
+				float dst_nz = dst_normalsdata[3*dst_ind+2];
+
+				float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
+				float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
+
+				float tnx	= m00*src_nx + m01*src_ny + m02*src_nz;
+				float tny	= m10*src_nx + m11*src_ny + m12*src_nz;
+				float tnz	= m20*src_nx + m21*src_ny + m22*src_nz;
+
+				double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz));
+				double dst_noise = dst_z * dst_z;
+				double point_noise = 1.0/sqrt(point_information);
+
+				double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise);
+				d *= compare_mul;
+
+				double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z);
+				double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst);
+
+				residuals.push_back(d);
+				weights.push_back(angle_dst*angle_dst*angle_dst);
+				if(debugg){
+					debugg_src_inds.push_back(ind);
+					debugg_dst_inds.push_back(dst_ind);
+				}
+			}
+		}
+	}
+
+	//	DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2();
+	//	func->maxp			= 1.0;
+	//	func->update_size	= true;
+	//	func->zeromean      = true;
+	//	func->startreg		= 0.0001;
+	//	func->debugg_print	= debugg;
+	//	func->bidir			= true;
+	//	func->maxnoise      = pred;
+	//	func->reset();
+
+	DistanceWeightFunction2 * func = new DistanceWeightFunction2();
+	func->f = THRESHOLD;
+	func->p = 0.02;
+
+	Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size());
+	for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];}
+	func->computeModel(X);
+
+	Eigen::VectorXd  W = func->getProbs(X);
+
+	delete func;
+
+	for(unsigned int i = 0; i < residuals.size(); i++){
+		float r = residuals[i];
+		float weight = W(i);
+		float ocl = 0;
+		if(r > 0){ocl += 1-weight;}
+		oc.score		+= weight*weights.at(i);
+		oc.occlusions	+= ocl*weights.at(i);
+	}
+
+
+	if(debugg){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		dcloud->points.resize(dst_width*dst_height);
+		for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){
+			for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){
+				unsigned int dst_ind = dst_h*dst_width+dst_w;
+				float z = dst_idepth*float(dst_depthdata[dst_ind]);
+				if(z > 0){
+					float x = (float(dst_w) - dst_cx) * z * dst_ifx;
+					float y = (float(dst_h) - dst_cy) * z * dst_ify;
+					dcloud->points[dst_ind].x = x;
+					dcloud->points[dst_ind].y = y;
+					dcloud->points[dst_ind].z = z;
+					dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2];
+					dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1];
+					dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0];
+					if(dst_maskdata[dst_ind] == 255){
+						dcloud->points[dst_ind].r = 255;
+						dcloud->points[dst_ind].g = 000;
+						dcloud->points[dst_ind].b = 255;
+					}
+				}
+			}
+		}
+
+		oc.print();
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		for(unsigned int i = 0; i < spvec.size(); i++){
+			superpoint & sp = spvec[i];
+			pcl::PointXYZRGBNormal p;
+			p.x = sp.point(0);
+			p.y = sp.point(1);
+			p.z = sp.point(2);
+
+			float tx	= m00*p.x + m01*p.y + m02*p.z + m03;
+			float ty	= m10*p.x + m11*p.y + m12*p.z + m13;
+			float tz	= m20*p.x + m21*p.y + m22*p.z + m23;
+
+			p.x = tx;
+			p.y = ty;
+			p.z = tz;
+
+			p.normal_x = sp.normal(0);
+			p.normal_y = sp.normal(1);
+			p.normal_z = sp.normal(2);
+			p.r = sp.feature(0);
+			p.g = sp.feature(1);
+			p.b = sp.feature(2);
+
+			scloud->points.push_back(p);
+		}
+
+		//		viewer->removeAllPointClouds();
+		//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+		//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+		//		viewer->spin();
+
+		viewer->removeAllPointClouds();
+		viewer->removeAllShapes();
+
+		for(unsigned int i = 0; i < residuals.size(); i++){
+			float r = residuals[i];
+			float weight = W(i);
+			float ocl = 0;
+			if(r > 0){ocl += 1-weight;}
+			if(debugg){
+				unsigned int src_ind = debugg_src_inds[i];
+				unsigned int dst_ind = debugg_dst_inds[i];
+				if(ocl > 0.01 || weight > 0.01){
+					scloud->points[src_ind].r = 255.0*ocl*weights.at(i);
+					scloud->points[src_ind].g = 255.0*weight*weights.at(i);
+					scloud->points[src_ind].b = 0;
+
+					if(i % 300 == 0){
+						char buf [1024];
+						sprintf(buf,"line%i",i);
+						viewer->addLine<pcl::PointXYZRGBNormal> (scloud->points[src_ind], dcloud->points[dst_ind],buf);
+					}
+				}else{
+					scloud->points[src_ind].x = 0;
+					scloud->points[src_ind].y = 0;
+					scloud->points[src_ind].z = 0;
+				}
+			}
+		}
+
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud");
+
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+		viewer->spin();
+		viewer->removeAllPointClouds();
+		viewer->removeAllShapes();
+
+	}
+	//printf("stop :: %s::%i\n",__FILE__,__LINE__);
+	return oc;
+}
+
+OcclusionScore ModelUpdater::computeOcclusionScore(Model * mod, vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<ModelMask*> cm, Matrix4d rp, int step,  bool debugg){
+	OcclusionScore ocs;
+	for(unsigned int i = 0; i < cp.size(); i++){
+		ocs.add(computeOcclusionScore(mod->points,cp[i].inverse()*rp,cf[i],cm[i],step,debugg));
+	}
+	return ocs;
+}
+
+
+OcclusionScore ModelUpdater::computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp, int step, bool debugg){
+	OcclusionScore ocs;
+	//	ocs.add(computeOcclusionScore(model1, model2->rep_relativeposes,model2->rep_frames,model2->rep_modelmasks,rp.inverse(),step,debugg));
+	//	ocs.add(computeOcclusionScore(model2, model1->rep_relativeposes,model1->rep_frames,model1->rep_modelmasks,rp,step,debugg));
+	ocs.add(computeOcclusionScore(model1, model2->relativeposes,model2->frames,model2->modelmasks,rp.inverse(),step,debugg));
+	ocs.add(computeOcclusionScore(model2, model1->relativeposes,model1->frames,model1->modelmasks,rp,step,debugg));
+	return ocs;
+}
+
+vector<vector < OcclusionScore > > ModelUpdater::computeOcclusionScore(vector<Model *> models, vector<Matrix4d> rps, int step, bool debugg){
+	unsigned int nr_models = models.size();
+	vector<vector < OcclusionScore > > occlusionScores;
+	occlusionScores.resize(nr_models);
+	for(unsigned int i = 0; i < nr_models; i++){occlusionScores[i].resize(nr_models);}
+
+	for(unsigned int i = 0; i < nr_models; i++){
+		for(unsigned int j = i+1; j < nr_models; j++){
+			//printf("%5.5f %5.5f\n",double(i+1)/double(nr_models),double(j+1)/double(nr_models));
+			Eigen::Matrix4d relative_pose = rps[i].inverse() * rps[j];
+			occlusionScores[i][j] = computeOcclusionScore(models[i],models[j], relative_pose, step, debugg);
+			occlusionScores[j][i] = occlusionScores[i][j];
+		}
+	}
+
+	return occlusionScores;
+}
+
+double ModelUpdater::computeOcclusionScoreCosts(vector<Model *> models){
+	double total = 0;
+
+	unsigned int nr_models = models.size();
+	for(unsigned int i = 0; i < nr_models; i++){
+		for(unsigned int j = 0; j < nr_models; j++){
+			if(i == j){continue;}
+			total += models[j]->rep_frames.size() * models[i]->points.size();
+		}
+	}
+
+	return total;
+}
+
+void ModelUpdater::addModelsToVector(vector<Model *> & models, vector<Matrix4d> & rps, Model * model, Matrix4d rp){
+	if(model->frames.size() > 0){
+		models.push_back(model);
+		rps.push_back(rp);
+	}
+
+	for(unsigned int i = 0; i < model->submodels.size(); i++){
+		addModelsToVector(models,rps,model->submodels[i], rp*model->submodels_relativeposes[i]);
+	}
+}
+
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ModelUpdater::getPCLnormalcloud(vector<superpoint> & points){
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	for(unsigned int i = 0; i < points.size(); i++){
+		superpoint & sp = points[i];
+		pcl::PointXYZRGBNormal p;
+		p.x = sp.point(0);
+		p.y = sp.point(1);
+		p.z = sp.point(2);
+
+		p.normal_x = sp.normal(0);
+		p.normal_y = sp.normal(1);
+		p.normal_z = sp.normal(2);
+		p.r = sp.feature(0);
+		p.g = sp.feature(1);
+		p.b = sp.feature(2);
+
+		cloud_ptr->points.push_back(p);
+	}
+	return cloud_ptr;
+}
+
+bool ModelUpdater::refineIfNeeded(){
+
+	if(isRefinementNeeded()){
+		MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1);
+		massreg->timeout = massreg_timeout;
+		massreg->viewer = viewer;
+		massreg->visualizationLvl = 1;
+		massreg->maskstep = 4;
+		massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size())));
+
+		massreg->nomask = true;
+		massreg->stopval = 0.001;
+
+		massreg->setData(model->frames,model->modelmasks);
+		MassFusionResults mfr = massreg->getTransforms(model->relativeposes);
+		model->relativeposes = mfr.poses;
+
+		isRefinementNeeded();
+	}
+
+
+	MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1);
+	massreg->timeout = massreg_timeout;
+	massreg->viewer = viewer;
+	massreg->visualizationLvl = 1;
+	massreg->maskstep = 4;
+	massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size())));
+
+	massreg->nomask = true;
+	massreg->stopval = 0.001;
+
+	massreg->setData(model->frames,model->modelmasks);
+	MassFusionResults mfr = massreg->getTransforms(model->relativeposes);
+	model->relativeposes = mfr.poses;
+
+	vector<superpoint> spvec = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false);
+	vector<Matrix4d> cp;
+	vector<RGBDFrame*> cf;
+	vector<ModelMask*> cm;
+	for(unsigned int i = 0; i < model->relativeposes.size(); i++){
+		cp.push_back(model->relativeposes[i]);
+		cf.push_back(model->frames[i]);
+		cm.push_back(model->modelmasks[i]);
+	}
+	getGoodCompareFrames(cp,cf,cm);
+
+	vector<superpoint> spvec2 = getSuperPoints(cp,cf,cm,1,true);
+	return true;
+}
+
+void ModelUpdater::makeInitialSetup(){
+
+	if(model->relativeposes.size() <= 1){
+		model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false);
+
+		vector<Matrix4d> cp;
+		vector<RGBDFrame*> cf;
+		vector<ModelMask*> cm;
+		for(unsigned int i = 0; i < model->relativeposes.size(); i++){
+			cp.push_back(model->relativeposes[i]);
+			cf.push_back(model->frames[i]);
+			cm.push_back(model->modelmasks[i]);
+		}
+		getGoodCompareFrames(cp,cf,cm);
+
+		model->rep_relativeposes = cp;
+		model->rep_frames = cf;
+		model->rep_modelmasks = cm;
+		return ;
+	}
+
+	MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.0);
+	massreg->timeout = 4*massreg_timeout;
+	massreg->viewer = viewer;
+	massreg->visualizationLvl = show_init_lvl;
+
+	massreg->maskstep = 1;//std::max(1,int(0.25*double(model->frames.size())));
+	massreg->nomaskstep = std::max(5,int(0.5+0.3*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size())));
+	massreg->nomask = true;
+	massreg->stopval = 0.0005;
+
+	massreg->setData(model->frames,model->modelmasks);
+	MassFusionResults mfr = massreg->getTransforms(model->relativeposes);
+
+	model->relativeposes = mfr.poses;
+
+
+
+	model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false);
+	//printf("getSuperPoints done\n");
+	vector<Matrix4d> cp;
+	vector<RGBDFrame*> cf;
+	vector<ModelMask*> cm;
+	for(unsigned int i = 0; i < model->relativeposes.size(); i++){
+		cp.push_back(model->relativeposes[i]);
+		cf.push_back(model->frames[i]);
+		cm.push_back(model->modelmasks[i]);
+	}
+
+	getGoodCompareFrames(cp,cf,cm);
+
+	model->rep_relativeposes = cp;
+	model->rep_frames = cf;
+	model->rep_modelmasks = cm;
+
+	//printf("model->rep_relativeposes: %i\n",model->rep_relativeposes.size());
+}
+
+void ModelUpdater::addSuperPoints(vector<superpoint> & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, int type, bool debugg){
+	bool * maskvec = modelmask->maskvec;
+	unsigned char  * rgbdata		= (unsigned char	*)(frame->rgb.data);
+	unsigned short * depthdata		= (unsigned short	*)(frame->depth.data);
+	float		   * normalsdata	= (float			*)(frame->normals.data);
+
+	unsigned int frameid = frame->id;
+
+	Matrix4d ip = p.inverse();
+
+	float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+	float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+	float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+	float im00 = ip(0,0); float im01 = ip(0,1); float im02 = ip(0,2); float im03 = ip(0,3);
+	float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3);
+	float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3);
+
+	Camera * camera				= frame->camera;
+	const unsigned int width	= camera->width;
+	const unsigned int height	= camera->height;
+
+	const unsigned int dst_width2	= camera->width  - 2;
+	const unsigned int dst_height2	= camera->height - 2;
+
+	const float idepth			= camera->idepth_scale;
+	const float cx				= camera->cx;
+	const float cy				= camera->cy;
+	const float fx				= camera->fx;
+	const float fy				= camera->fy;
+	const float ifx				= 1.0/camera->fx;
+	const float ify				= 1.0/camera->fy;
+
+	//bool * isfused = new bool[width*height];
+	std::vector<bool> isfused;
+	//printf("wh: %i %i -> %i\n",width,height,width*height);
+	isfused.resize(width*height);
+	for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;}
+
+
+	for(unsigned int ind = 0; ind < spvec.size();ind++){
+		superpoint & sp = spvec[ind];
+
+		float src_x = sp.point(0);
+		float src_y = sp.point(1);
+		float src_z = sp.point(2);
+
+		float src_nx = sp.normal(0);
+		float src_ny = sp.normal(1);
+		float src_nz = sp.normal(2);
+
+		float src_r = sp.feature(0);
+		float src_g = sp.feature(1);
+		float src_b = sp.feature(2);
+
+		double point_information = sp.point_information;
+		double feature_information = sp.feature_information;
+
+		float tx	= im00*src_x + im01*src_y + im02*src_z + im03;
+		float ty	= im10*src_x + im11*src_y + im12*src_z + im13;
+		float tz	= im20*src_x + im21*src_y + im22*src_z + im23;
+
+		float itz	= 1.0/tz;
+		float dst_w	= fx*tx*itz + cx;
+		float dst_h	= fy*ty*itz + cy;
+
+		if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
+			unsigned int dst_ind = unsigned(dst_h+0.5) * width + unsigned(dst_w+0.5);
+
+			float dst_z = idepth*float(depthdata[dst_ind]);
+			float dst_nx = normalsdata[3*dst_ind+0];
+			if(!isfused[dst_ind] && dst_z > 0 && dst_nx != 2){
+				float dst_ny = normalsdata[3*dst_ind+1];
+				float dst_nz = normalsdata[3*dst_ind+2];
+
+				float dst_x = (float(dst_w) - cx) * dst_z * ifx;
+				float dst_y = (float(dst_h) - cy) * dst_z * ify;
+
+				double dst_noise = dst_z * dst_z;
+
+				double point_noise = 1.0/sqrt(point_information);
+
+				float tnx	= im00*src_nx + im01*src_ny + im02*src_nz;
+				float tny	= im10*src_nx + im11*src_ny + im12*src_nz;
+				float tnz	= im20*src_nx + im21*src_ny + im22*src_nz;
+
+				double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz));
+
+				double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise);
+				d *= compare_mul;
+
+				double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz;
+
+				if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE
+					float px	= m00*dst_x + m01*dst_y + m02*dst_z + m03;
+					float py	= m10*dst_x + m11*dst_y + m12*dst_z + m13;
+					float pz	= m20*dst_x + m21*dst_y + m22*dst_z + m23;
+
+					float pnx	= m00*dst_nx + m01*dst_ny + m02*dst_nz;
+					float pny	= m10*dst_nx + m11*dst_ny + m12*dst_nz;
+					float pnz	= m20*dst_nx + m21*dst_ny + m22*dst_nz;
+
+					float pb = rgbdata[3*dst_ind+0];
+					float pg = rgbdata[3*dst_ind+1];
+					float pr = rgbdata[3*dst_ind+2];
+
+					Vector3f	pxyz	(px	,py	,pz );
+					Vector3f	pnxyz	(pnx,pny,pnz);
+					Vector3f	prgb	(pr	,pg	,pb );
+					float		weight	= 1.0/(dst_noise*dst_noise);
+					superpoint sp2 = superpoint(pxyz,pnxyz,prgb, weight, weight, frameid);
+					sp.merge(sp2);
+					isfused[dst_ind] = true;
+				}
+			}
+		}
+	}
+
+	int nr_fused = 0;
+	int nr_mask = 0;
+	for(unsigned int w = 0; w < width; w++){
+		for(unsigned int h = 0; h < height;h++){
+			int ind = h*width+w;
+			nr_fused += isfused[ind];
+			nr_mask += maskvec[ind] > 0;
+		}
+	}
+
+	for(unsigned int w = 0; w < width; w++){
+		for(unsigned int h = 0; h < height;h++){
+			int ind = h*width+w;
+			if(!isfused[ind] && maskvec[ind]){
+				float z = idepth*float(depthdata[ind]);
+				float nx = normalsdata[3*ind+0];
+
+				if(z > 0 && nx != 2){
+					float ny = normalsdata[3*ind+1];
+					float nz = normalsdata[3*ind+2];
+
+					float x = (w - cx) * z * ifx;
+					float y = (h - cy) * z * ify;
+
+					double noise = z * z;
+
+					float px	= m00*x + m01*y + m02*z + m03;
+					float py	= m10*x + m11*y + m12*z + m13;
+					float pz	= m20*x + m21*y + m22*z + m23;
+					float pnx	= m00*nx + m01*ny + m02*nz;
+					float pny	= m10*nx + m11*ny + m12*nz;
+					float pnz	= m20*nx + m21*ny + m22*nz;
+
+					float pb = rgbdata[3*ind+0];
+					float pg = rgbdata[3*ind+1];
+					float pr = rgbdata[3*ind+2];
+
+					Vector3f	pxyz	(px	,py	,pz );
+					Vector3f	pnxyz	(pnx,pny,pnz);
+					Vector3f	prgb	(pr	,pg	,pb );
+					float		weight	= 1.0/(noise*noise);
+					spvec.push_back(superpoint(pxyz,pnxyz,prgb, weight, weight, frameid));
+				}
+			}
+		}
+	}
+
+	//delete[] isfused;
+
+	if(debugg){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = getPCLnormalcloud(spvec);
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud), "cloud");
+		viewer->spin();
+	}
+}
+
+vector<superpoint> ModelUpdater::getSuperPoints(vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<ModelMask*> cm, int type, bool debugg){
+	vector<superpoint> spvec;
+	for(unsigned int i = 0; i < cp.size(); i++){addSuperPoints(spvec, cp[i], cf[i], cm[i], type,debugg);}
+	return spvec;
+}
+
+double ModelUpdater::getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector<Matrix4d> & cp, vector<RGBDFrame*> & cf, vector<ModelMask*> & cm){
+	unsigned char  * src_maskdata		= (unsigned char	*)(mask->mask.data);
+	unsigned char  * src_rgbdata		= (unsigned char	*)(frame->rgb.data);
+	unsigned short * src_depthdata		= (unsigned short	*)(frame->depth.data);
+	float		   * src_normalsdata	= (float			*)(frame->normals.data);
+
+	Camera * src_camera				= frame->camera;
+	const unsigned int src_width	= src_camera->width;
+	const unsigned int src_height	= src_camera->height;
+	const float src_idepth			= src_camera->idepth_scale;
+	const float src_cx				= src_camera->cx;
+	const float src_cy				= src_camera->cy;
+	const float src_ifx				= 1.0/src_camera->fx;
+	const float src_ify				= 1.0/src_camera->fy;
+
+	std::vector<float> test_x;
+	std::vector<float> test_y;
+	std::vector<float> test_z;
+	std::vector<float> test_nx;
+	std::vector<float> test_ny;
+	std::vector<float> test_nz;
+
+	std::vector<int> & testw = mask->testw;
+	std::vector<int> & testh = mask->testh;
+
+	for(unsigned int ind = 0; ind < testw.size();ind++){
+		unsigned int src_w = testw[ind];
+		unsigned int src_h = testh[ind];
+
+		int src_ind = src_h*src_width+src_w;
+		if(src_maskdata[src_ind] == 255){
+			float src_z = src_idepth*float(src_depthdata[src_ind]);
+			float src_nx = src_normalsdata[3*src_ind+0];
+			if(src_z > 0 && src_nx != 2){
+				float src_ny = src_normalsdata[3*src_ind+1];
+				float src_nz = src_normalsdata[3*src_ind+2];
+
+				float src_x = (float(src_w) - src_cx) * src_z * src_ifx;
+				float src_y = (float(src_h) - src_cy) * src_z * src_ify;
+
+				test_x.push_back(src_x);
+				test_y.push_back(src_y);
+				test_z.push_back(src_z);
+				test_nx.push_back(src_nx);
+				test_ny.push_back(src_ny);
+				test_nz.push_back(src_nz);
+			}
+		}
+	}
+
+	bool debugg = false;
+	unsigned int nrdata_start = test_x.size();
+
+	for(unsigned int c = 0; c < cp.size();c++){
+		RGBDFrame* dst = cf[c];
+		if(dst == frame){continue;}
+		ModelMask* dst_modelmask = cm[c];
+		unsigned char  * dst_maskdata		= (unsigned char	*)(dst_modelmask->mask.data);
+		unsigned char  * dst_rgbdata		= (unsigned char	*)(dst->rgb.data);
+		unsigned short * dst_depthdata		= (unsigned short	*)(dst->depth.data);
+		float		   * dst_normalsdata	= (float			*)(dst->normals.data);
+
+		Matrix4d rp = cp[c].inverse()*p;
+
+		float m00 = rp(0,0); float m01 = rp(0,1); float m02 = rp(0,2); float m03 = rp(0,3);
+		float m10 = rp(1,0); float m11 = rp(1,1); float m12 = rp(1,2); float m13 = rp(1,3);
+		float m20 = rp(2,0); float m21 = rp(2,1); float m22 = rp(2,2); float m23 = rp(2,3);
+
+		Camera * dst_camera				= dst->camera;
+		const unsigned int dst_width	= dst_camera->width;
+		const unsigned int dst_height	= dst_camera->height;
+		const float dst_idepth			= dst_camera->idepth_scale;
+		const float dst_cx				= dst_camera->cx;
+		const float dst_cy				= dst_camera->cy;
+		const float dst_fx				= dst_camera->fx;
+		const float dst_fy				= dst_camera->fy;
+		const float dst_ifx				= 1.0/dst_camera->fx;
+		const float dst_ify				= 1.0/dst_camera->fy;
+		const unsigned int dst_width2	= dst_camera->width  - 2;
+		const unsigned int dst_height2	= dst_camera->height - 2;
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		if(debugg){
+			viewer->removeAllPointClouds();
+
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud2 = dst->getPCLcloud();
+			viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud2, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud2), "cloud2");
+		}
+
+		for(unsigned int ind = 0; ind < test_x.size();ind++){
+			float src_nx = test_nx[ind];
+			float src_ny = test_ny[ind];
+			float src_nz = test_nz[ind];
+
+			float src_x = test_x[ind];
+			float src_y = test_y[ind];
+			float src_z = test_z[ind];
+
+			float tx	= m00*src_x + m01*src_y + m02*src_z + m03;
+			float ty	= m10*src_x + m11*src_y + m12*src_z + m13;
+			float tz	= m20*src_x + m21*src_y + m22*src_z + m23;
+			float itz	= 1.0/tz;
+			float dst_w	= dst_fx*tx*itz + dst_cx;
+			float dst_h	= dst_fy*ty*itz + dst_cy;
+
+			pcl::PointXYZRGBNormal point;
+			if(debugg){
+				point.x = tx;
+				point.y = ty;
+				point.z = tz;
+				point.r = 255;
+				point.g = 0;
+				point.b = 0;
+			}
+
+
+			if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
+				unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
+
+				float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+				float dst_nx = dst_normalsdata[3*dst_ind+0];
+				if(dst_z > 0 && dst_nx != 2){
+					float dst_ny = dst_normalsdata[3*dst_ind+1];
+					float dst_nz = dst_normalsdata[3*dst_ind+2];
+
+					float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
+					float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
+
+					double dst_noise = dst_z * dst_z;
+					double point_noise = src_z * src_z;
+
+					float tnx	= m00*src_nx + m01*src_ny + m02*src_nz;
+					float tny	= m10*src_nx + m11*src_ny + m12*src_nz;
+					float tnz	= m20*src_nx + m21*src_ny + m22*src_nz;
+
+					double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz));
+
+					double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise);
+					d *= compare_mul;
+
+					double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz;
+
+					if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE
+						//if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE
+						test_nx[ind] = test_nx.back(); test_nx.pop_back();
+						test_ny[ind] = test_ny.back(); test_ny.pop_back();
+						test_nz[ind] = test_nz.back(); test_nz.pop_back();
+						test_x[ind] = test_x.back(); test_x.pop_back();
+						test_y[ind] = test_y.back(); test_y.pop_back();
+						test_z[ind] = test_z.back(); test_z.pop_back();
+						ind--;
+						if(debugg){
+							point.r = 0;
+							point.g = 255;
+							point.b = 0;
+						}
+					}
+				}
+			}
+			if(debugg){
+				cloud->points.push_back(point);
+			}
+		}
+
+		if(debugg){
+			viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud), "cloud");
+			viewer->spin();
+			viewer->removeAllPointClouds();
+			viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud), "cloud");
+			viewer->spin();
+		}
+	}
+	return double(test_x.size())/double(nrdata_start);
+}
+
+void ModelUpdater::getGoodCompareFrames(vector<Matrix4d> & cp, vector<RGBDFrame*> & cf, vector<ModelMask*> & cm){
+	while(true){
+		double worst = 1;
+		int worst_id = -1;
+		for(unsigned int i = 0; i < cp.size(); i++){
+			double score = getCompareUtility(cp[i], cf[i], cm[i], cp, cf, cm);
+			if(score < worst){
+				worst = score;
+				worst_id = i;
+			}
+		}
+
+		if(worst_id < 0){break;}
+		if(worst < 0.1){//worst frame can to 90% be represented by other frames
+			cp[worst_id] = cp.back();
+			cp.pop_back();
+
+			cf[worst_id] = cf.back();
+			cf.pop_back();
+
+			cm[worst_id] = cm.back();
+			cm.pop_back();
+		}else{break;}
+	}
+}
+
+void ModelUpdater::refine(double reg,bool useFullMask, int visualization){
+
+
+	vector<vector < OcclusionScore > > ocs = computeOcclusionScore(model->submodels,model->submodels_relativeposes,1,false);
+
+	std::vector<std::vector < float > > scores = getScores(ocs);
+
+
+	double sumscore_bef = 0;
+	for(unsigned int i = 0; i < scores.size(); i++){
+		for(unsigned int j = 0; j < scores.size(); j++){
+			sumscore_bef += scores[i][j];
+		}
+	}
+
+	MassFusionResults mfr;
+	MassRegistrationPPR * massreg = new MassRegistrationPPR(reg);
+	massreg->timeout = massreg_timeout;
+	massreg->viewer = viewer;
+	massreg->visualizationLvl = 0;
+	massreg->maskstep = 4;//std::max(1,int(0.5+0.02*double(model->frames.size())));
+	massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size())));
+	printf("maskstep: %i nomaskstep: %i\n",massreg->maskstep,massreg->nomaskstep);
+	massreg->nomask = true;
+	massreg->stopval = 0.001;
+	double step = model->submodels_relativeposes.size()*model->submodels_relativeposes.size();
+	step *= 0.25;
+	step += 0.5;
+	massreg->maskstep = std::max(1.5,step);
+	massreg->addModelData(model);
+	mfr = massreg->getTransforms(model->submodels_relativeposes);
+	vector<vector < OcclusionScore > > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false);
+	std::vector<std::vector < float > > scores2 = getScores(ocs2);
+
+	double sumscore_aft = 0;
+	for(unsigned int i = 0; i < scores2.size(); i++){
+		for(unsigned int j = 0; j < scores2.size(); j++){
+			sumscore_aft += scores2[i][j];
+		}
+	}
+
+	printf("bef %f after %f\n",sumscore_bef,sumscore_aft);
+	if(sumscore_aft >= sumscore_bef){
+		model->submodels_relativeposes = mfr.poses;
+	}
+}
+
+void ModelUpdater::show(bool stop){
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = model->getPCLcloud(1,false);
+	viewer->removeAllPointClouds();
+	viewer->addPointCloud<pcl::PointXYZRGB>(cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(cloud), "cloud");
+	if(stop){	viewer->spin();}
+	else{		viewer->spinOnce();}
+	viewer->removeAllPointClouds();
+}
+
+//Backproject and prune occlusions
+void ModelUpdater::pruneOcclusions(){}
+
+void ModelUpdater::computeResiduals(std::vector<float> & residuals, std::vector<float> & weights,
+									RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask,
+									RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask,
+									Eigen::Matrix4d p, bool debugg){
+
+	unsigned char  * src_maskdata		= (unsigned char	*)(src_modelmask->mask.data);
+	unsigned char  * src_rgbdata		= (unsigned char	*)(src->rgb.data);
+	unsigned short * src_depthdata		= (unsigned short	*)(src->depth.data);
+	float		   * src_normalsdata	= (float			*)(src->normals.data);
+
+	unsigned char  * dst_maskdata		= (unsigned char	*)(dst_modelmask->mask.data);
+	unsigned char  * dst_rgbdata		= (unsigned char	*)(dst->rgb.data);
+	unsigned short * dst_depthdata		= (unsigned short	*)(dst->depth.data);
+	float		   * dst_normalsdata	= (float			*)(dst->normals.data);
+
+	float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+	float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+	float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+	Camera * src_camera				= src->camera;
+	const unsigned int src_width	= src_camera->width;
+	const unsigned int src_height	= src_camera->height;
+	const float src_idepth			= src_camera->idepth_scale;
+	const float src_cx				= src_camera->cx;
+	const float src_cy				= src_camera->cy;
+	const float src_ifx				= 1.0/src_camera->fx;
+	const float src_ify				= 1.0/src_camera->fy;
+
+	Camera * dst_camera				= dst->camera;
+	const unsigned int dst_width	= dst_camera->width;
+	const unsigned int dst_height	= dst_camera->height;
+	const float dst_idepth			= dst_camera->idepth_scale;
+	const float dst_cx				= dst_camera->cx;
+	const float dst_cy				= dst_camera->cy;
+	const float dst_fx				= dst_camera->fx;
+	const float dst_fy				= dst_camera->fy;
+
+	const unsigned int dst_width2	= dst_camera->width  - 2;
+	const unsigned int dst_height2	= dst_camera->height - 2;
+
+	std::vector<int> & testw = src_modelmask->testw;
+	std::vector<int> & testh = src_modelmask->testh;
+
+	unsigned int test_nrdata = testw.size();
+	for(unsigned int ind = 0; ind < test_nrdata;ind++){
+		unsigned int src_w = testw[ind];
+		unsigned int src_h = testh[ind];
+
+		int src_ind = src_h*src_width+src_w;
+		if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){
+			float z = src_idepth*float(src_depthdata[src_ind]);
+			float nx = src_normalsdata[3*src_ind+0];
+
+			if(z > 0 && nx != 2){
+				float ny = src_normalsdata[3*src_ind+1];
+				float nz = src_normalsdata[3*src_ind+2];
+
+				float x = (float(src_w) - src_cx) * z * src_ifx;
+				float y = (float(src_h) - src_cy) * z * src_ify;
+
+				float tx	= m00*x + m01*y + m02*z + m03;
+				float ty	= m10*x + m11*y + m12*z + m13;
+				float tz	= m20*x + m21*y + m22*z + m23;
+				//float tnx	= m00*nx + m01*ny + m02*nz;
+				//float tny	= m10*nx + m11*ny + m12*nz;
+				float tnz	= m20*nx + m21*ny + m22*nz;
+
+				float itz	= 1.0/tz;
+				float dst_w	= dst_fx*tx*itz + dst_cx;
+				float dst_h	= dst_fy*ty*itz + dst_cy;
+
+				if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
+					unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
+
+					float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+					if(dst_z > 0){
+						float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0
+						residuals.push_back(diff_z);
+						weights.push_back(1.0);
+					}
+				}
+			}
+		}
+	}
+}
+
+void ModelUpdater::recomputeScores(){
+	//	printf("recomputeScores\n");
+	std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(model->relativeposes,model->frames,model->modelmasks,false);
+	model->scores = getScores(ocs);
+
+	model->total_scores = 0;
+	for(unsigned int i = 0; i < model->scores.size(); i++){
+		for(unsigned int j = 0; j < model->scores.size(); j++){
+			model->total_scores += model->scores[i][j];
+		}
+	}
+}
+
+void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2){
+	unsigned char  * src_rgbdata		= (unsigned char	*)(frame1->rgb.data);
+	unsigned short * src_depthdata		= (unsigned short	*)(frame1->depth.data);
+	float		   * src_normalsdata	= (float			*)(frame1->normals.data);
+
+	Camera * src_camera				= frame1->camera;
+	const unsigned int src_width	= src_camera->width;
+	const unsigned int src_height	= src_camera->height;
+	const float src_idepth			= src_camera->idepth_scale;
+	const float src_cx				= src_camera->cx;
+	const float src_cy				= src_camera->cy;
+	const float src_ifx				= 1.0/src_camera->fx;
+	const float src_ify				= 1.0/src_camera->fy;
+
+	unsigned char  * dst_rgbdata		= (unsigned char	*)(frame2->rgb.data);
+	unsigned short * dst_depthdata		= (unsigned short	*)(frame2->depth.data);
+	float		   * dst_normalsdata	= (float			*)(frame2->normals.data);
+
+	Camera * dst_camera				= frame2->camera;
+	const unsigned int dst_width	= dst_camera->width;
+	const unsigned int dst_height	= dst_camera->height;
+	const float dst_idepth			= dst_camera->idepth_scale;
+	const float dst_cx				= dst_camera->cx;
+	const float dst_cy				= dst_camera->cy;
+	const float dst_ifx				= 1.0/dst_camera->fx;
+	const float dst_ify				= 1.0/dst_camera->fy;
+	const float dst_fx				= dst_camera->fx;
+	const float dst_fy				= dst_camera->fy;
+
+	const unsigned int dst_width2	= dst_camera->width  - 2;
+	const unsigned int dst_height2	= dst_camera->height - 2;
+
+	float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+	float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+	float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+	bool debugg = false;
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr src_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dst_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	if(debugg){
+
+		src_cloud->points.resize(src_width*src_height);
+		for(unsigned int src_w = 0; src_w < src_width;src_w++){
+			for(unsigned int src_h = 0; src_h < src_height;src_h++){
+				int src_ind = src_h*src_width+src_w;
+				float src_z = src_idepth*float(src_depthdata[src_ind]);
+				float src_nx = src_normalsdata[3*src_ind+0];
+				if(src_z > 0 && src_nx != 2){
+					float src_ny = src_normalsdata[3*src_ind+1];
+					float src_nz = src_normalsdata[3*src_ind+2];
+
+					float src_x = (float(src_w) - src_cx) * src_z * src_ifx;
+					float src_y = (float(src_h) - src_cy) * src_z * src_ify;
+
+
+					float tx	= m00*src_x + m01*src_y + m02*src_z + m03;
+					float ty	= m10*src_x + m11*src_y + m12*src_z + m13;
+					float tz	= m20*src_x + m21*src_y + m22*src_z + m23;
+
+					pcl::PointXYZRGBNormal point;
+					point.x = tx;
+					point.y = ty;
+					point.z = tz;
+					point.r = 255;
+					point.g = 0;
+					point.b = 0;
+					src_cloud->points[src_ind] = point;
+				}
+			}
+		}
+
+		dst_cloud->points.resize(dst_width*dst_height);
+		for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){
+			for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){
+				int dst_ind = dst_h*dst_width+dst_w;
+				float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+				float dst_nx = dst_normalsdata[3*dst_ind+0];
+				if(dst_z > 0 && dst_nx != 2){
+					float dst_ny = dst_normalsdata[3*dst_ind+1];
+					float dst_nz = dst_normalsdata[3*dst_ind+2];
+
+					float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
+					float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
+
+					pcl::PointXYZRGBNormal point;
+					point.x = dst_x;
+					point.y = dst_y;
+					point.z = dst_z;
+					point.r = 0;
+					point.g = 0;
+					point.b = 255;
+					dst_cloud->points[dst_ind] = point;
+				}
+			}
+		}
+	}
+
+	for(unsigned int src_w = 0; src_w < src_width;src_w++){
+		for(unsigned int src_h = 0; src_h < src_height;src_h++){
+			int src_ind = src_h*src_width+src_w;
+			float src_z = src_idepth*float(src_depthdata[src_ind]);
+			float src_nx = src_normalsdata[3*src_ind+0];
+			if(src_z > 0 && src_nx != 2){
+				float src_ny = src_normalsdata[3*src_ind+1];
+				float src_nz = src_normalsdata[3*src_ind+2];
+
+				float src_x = (float(src_w) - src_cx) * src_z * src_ifx;
+				float src_y = (float(src_h) - src_cy) * src_z * src_ify;
+
+
+				float tx	= m00*src_x + m01*src_y + m02*src_z + m03;
+				float ty	= m10*src_x + m11*src_y + m12*src_z + m13;
+				float tz	= m20*src_x + m21*src_y + m22*src_z + m23;
+				float itz	= 1.0/tz;
+				float dst_w	= dst_fx*tx*itz + dst_cx;
+				float dst_h	= dst_fy*ty*itz + dst_cy;
+
+				if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
+					unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
+
+					float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+					float dst_nx = dst_normalsdata[3*dst_ind+0];
+					if(dst_z > 0 && dst_nx != 2){
+						float dst_ny = dst_normalsdata[3*dst_ind+1];
+						float dst_nz = dst_normalsdata[3*dst_ind+2];
+
+						float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
+						float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
+
+						double dst_noise = dst_z * dst_z;
+						double point_noise = src_z * src_z;
+
+						float tnx	= m00*src_nx + m01*src_ny + m02*src_nz;
+						float tny	= m10*src_nx + m11*src_ny + m12*src_nz;
+						float tnz	= m20*src_nx + m21*src_ny + m22*src_nz;
+
+						double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z);
+						//double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise);
+						//d *= src_z;//compare_mul;
+						//double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz;
+
+						if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE
+							overlaps1[src_ind] += weights2[dst_ind];
+							overlaps2[dst_ind] += weights1[src_ind];
+
+							total1[src_ind] += weights2[dst_ind];
+							total2[dst_ind] += weights1[src_ind];
+							if(debugg){
+								dst_cloud->points[dst_ind].r = 0;
+								dst_cloud->points[dst_ind].g = 255;
+								dst_cloud->points[dst_ind].b = 0;
+								src_cloud->points[src_ind].r = 0;
+								src_cloud->points[src_ind].g = 255;
+								src_cloud->points[src_ind].b = 0;
+							}
+						}else if(tz < dst_z){
+							total1[src_ind] += weights2[dst_ind];
+							total2[dst_ind] += weights1[src_ind];
+							if(debugg){
+								src_cloud->points[src_ind].r = 255;
+								src_cloud->points[src_ind].g = 0;
+								src_cloud->points[src_ind].b = 255;
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+
+	if(debugg){
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(src_cloud), "scloud");
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dst_cloud), "dcloud");
+		viewer->spin();
+	}
+}
+
+double interpolate( double val, double y0, double x0, double y1, double x1 ) {return (val-x0)*(y1-y0)/(x1-x0) + y0;}
+double base( double val ) {
+	if ( val <= -0.75 ) return 0;
+	else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 );
+	else if ( val <= 0.25 ) return 1.0;
+	else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 );
+	else return 0.0;
+}
+double red(		double gray )	{ return base( gray - 0.5 ); }
+double green(	double gray )	{ return base( gray );}
+double blue(	double gray )	{ return base( gray + 0.5 );}
+
+cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr){
+	double maxval = 0;
+	for(unsigned int i = 0; i < width*height; i++){maxval = std::max(maxval,arr[i]);}
+	maxval = 1;
+
+	cv::Mat m;
+	m.create(height,width,CV_8UC3);
+	unsigned char * data = m.data;
+	for(unsigned int i = 0; i < width*height; i++){
+		double d = arr[i]/maxval;
+		if(d < 0){
+			data[3*i+0] = 255.0;//blue(d);
+			data[3*i+1] = 0;//green(d);
+			data[3*i+2] = 255.0;//red(d);
+		}else{
+			data[3*i+0] = 255.0*d;//blue(d);
+			data[3*i+1] = 255.0*d;//green(d);
+			data[3*i+2] = 255.0*d;//red(d);
+		}
+	}
+
+	//	cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE );
+	//	cv::imshow( "getImageFromArray",m);
+	//	cv::waitKey(0);
+
+	return m;
+}
+
+void ModelUpdater::computeOcclusionAreas(vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<ModelMask*> cm){
+
+	//double ** weights_old = new double*[];
+	std::vector<double **> weights_old;
+	double ** wo = new double*[cf.size()];
+	for(unsigned int i = 0; i < cf.size(); i++){
+		unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height;
+		wo[i] = new double[nr_pixels];
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			wo[i][j] = 1;
+		}
+	}
+
+	weights_old.push_back(wo);
+
+	double ** overlaps = new double*[cf.size()];
+	double ** total = new double*[cf.size()];
+	for(unsigned int i = 0; i < cf.size(); i++){
+		unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height;
+		overlaps[i] = new double[nr_pixels];
+		total[i] = new double[nr_pixels];
+	}
+
+
+
+
+
+
+
+	for(unsigned int iter = 0; iter < 10; iter++){
+		for(unsigned int i = 0; i < cf.size(); i++){
+			unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height;
+			for(unsigned int j = 0; j < nr_pixels; j++){
+				overlaps[i][j] = 0;
+				total[i][j] = 0;
+			}
+		}
+		printf("iter %i\n",iter);
+		for(unsigned int i = 0; i < cf.size(); i++){
+			for(unsigned int j = 0; j < cf.size(); j++){
+				if(i == j){continue;}
+				//weightedocclusioncounts
+				Eigen::Matrix4d p = cp[i].inverse() * cp[j];
+				getAreaWeights(p.inverse(), cf[i], wo[i], overlaps[i], total[i], cf[j], wo[j], overlaps[j], total[j]);
+			}
+		}
+
+		wo = new double*[cf.size()];
+		for(unsigned int i = 0; i < cf.size(); i++){
+			unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height;
+			wo[i] = new double[nr_pixels];
+			for(unsigned int j = 0; j < nr_pixels; j++){
+				if(total[i][j] < 0.00001){
+					wo[i][j] = 0.0;
+				}else{
+					double fails	= occlusion_penalty*(total[i][j]-overlaps[i][j]);
+					double others	= overlaps[i][j];
+					wo[i][j] = others/(fails+others);
+				}
+			}
+		}
+		weights_old.push_back(wo);
+	}
+
+	for(unsigned int i = 0; i < cf.size(); i++){
+		unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height;
+		double sum = 0;
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			sum += wo[i][j];
+		}
+		sum /= double(nr_pixels);
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			if(total[i][j] == 0){
+				wo[i][j] = sum;
+			}
+		}
+	}
+	for(unsigned int i = 0; i < cf.size(); i++){
+		for(unsigned int j = 0; j < weights_old.size(); j++){
+			getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, weights_old[j][i]);
+		}
+	}
+}
+
+
+void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<double> > & interframe_connectionStrength, double debugg){
+	unsigned char  * src_rgbdata		= (unsigned char	*)(frame1->rgb.data);
+	unsigned short * src_depthdata		= (unsigned short	*)(frame1->depth.data);
+	float		   * src_normalsdata	= (float			*)(frame1->normals.data);
+
+	unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data);
+	unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data);
+
+	Camera * src_camera				= frame1->camera;
+	const unsigned int src_width	= src_camera->width;
+	const unsigned int src_height	= src_camera->height;
+	const float src_idepth			= src_camera->idepth_scale;
+	const float src_cx				= src_camera->cx;
+	const float src_cy				= src_camera->cy;
+	const float src_ifx				= 1.0/src_camera->fx;
+	const float src_ify				= 1.0/src_camera->fy;
+
+	unsigned char  * dst_rgbdata		= (unsigned char	*)(frame2->rgb.data);
+	unsigned short * dst_depthdata		= (unsigned short	*)(frame2->depth.data);
+	float		   * dst_normalsdata	= (float			*)(frame2->normals.data);
+	unsigned char  * dst_maskdata       = (unsigned char	*)(mask.data);
+
+	Camera * dst_camera				= frame2->camera;
+	const unsigned int dst_width	= dst_camera->width;
+	const unsigned int dst_height	= dst_camera->height;
+	const float dst_idepth			= dst_camera->idepth_scale;
+	const float dst_cx				= dst_camera->cx;
+	const float dst_cy				= dst_camera->cy;
+	const float dst_ifx				= 1.0/dst_camera->fx;
+	const float dst_ify				= 1.0/dst_camera->fy;
+	const float dst_fx				= dst_camera->fx;
+	const float dst_fy				= dst_camera->fy;
+
+	const unsigned int dst_width2	= dst_camera->width  - 2;
+	const unsigned int dst_height2	= dst_camera->height - 2;
+
+	float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+	float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+	float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+	//bool debugg = true;
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr src_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dst_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	if(debugg){
+
+
+		//		cv::namedWindow( "src_det_dilate", cv::WINDOW_AUTOSIZE );	cv::imshow( "src_det_dilate",	frame1->det_dilate);
+		//		cv::namedWindow( "dst_det_dilate", cv::WINDOW_AUTOSIZE );	cv::imshow( "dst_det_dilate",	frame2->det_dilate);
+		//		cv::waitKey(0);
+
+		src_cloud->points.resize(src_width*src_height);
+		for(unsigned int src_w = 0; src_w < src_width;src_w++){
+			for(unsigned int src_h = 0; src_h < src_height;src_h++){
+				int src_ind = src_h*src_width+src_w;
+				float src_z = src_idepth*float(src_depthdata[src_ind]);
+				float src_nx = src_normalsdata[3*src_ind+0];
+				if(src_z > 0 && src_nx != 2){
+					float src_ny = src_normalsdata[3*src_ind+1];
+					float src_nz = src_normalsdata[3*src_ind+2];
+
+					float src_x = (float(src_w) - src_cx) * src_z * src_ifx;
+					float src_y = (float(src_h) - src_cy) * src_z * src_ify;
+
+
+					float tx	= m00*src_x + m01*src_y + m02*src_z + m03;
+					float ty	= m10*src_x + m11*src_y + m12*src_z + m13;
+					float tz	= m20*src_x + m21*src_y + m22*src_z + m23;
+
+					pcl::PointXYZRGBNormal point;
+					point.x = tx;
+					point.y = ty;
+					point.z = tz;
+					//point.r = 255*src_detdata[src_ind] != 0;
+					//                    point.g = 0;
+					//                    point.b = 0;
+
+					point.r = src_detdata[src_ind];
+					point.g = 0;
+					point.b = 0;
+
+					src_cloud->points[src_ind] = point;
+				}
+			}
+		}
+
+		dst_cloud->points.resize(dst_width*dst_height);
+		for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){
+			for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){
+				int dst_ind = dst_h*dst_width+dst_w;
+				float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+				float dst_nx = dst_normalsdata[3*dst_ind+0];
+				if(dst_z > 0 && dst_nx != 2){
+					float dst_ny = dst_normalsdata[3*dst_ind+1];
+					float dst_nz = dst_normalsdata[3*dst_ind+2];
+
+					float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
+					float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
+
+					pcl::PointXYZRGBNormal point;
+					point.x = dst_x;
+					point.y = dst_y;
+					point.z = dst_z;
+					point.r = 0;
+					point.g = 0;
+					point.b = dst_detdata[dst_ind];
+					dst_cloud->points[dst_ind] = point;
+				}
+			}
+		}
+
+
+		//		viewer->removeAllPointClouds();
+		//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(src_cloud), "scloud");
+		//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dst_cloud), "dcloud");
+		//		viewer->spin();
+	}
+
+	int informations = 0;
+
+	for(unsigned int src_w = 0; src_w < src_width;src_w++){
+		for(unsigned int src_h = 0; src_h < src_height;src_h++){
+			int src_ind = src_h*src_width+src_w;
+			float src_z = src_idepth*float(src_depthdata[src_ind]);
+			float src_nx = src_normalsdata[3*src_ind+0];
+			if(src_z > 0 && src_nx != 2){
+				float src_ny = src_normalsdata[3*src_ind+1];
+				float src_nz = src_normalsdata[3*src_ind+2];
+
+				float src_x = (float(src_w) - src_cx) * src_z * src_ifx;
+				float src_y = (float(src_h) - src_cy) * src_z * src_ify;
+
+				float tz	= m20*src_x + m21*src_y + m22*src_z + m23;
+				if(tz < 0){continue;}
+
+				float tx	= m00*src_x + m01*src_y + m02*src_z + m03;
+				float ty	= m10*src_x + m11*src_y + m12*src_z + m13;
+				float itz	= 1.0/tz;
+				float dst_w	= dst_fx*tx*itz + dst_cx;
+				float dst_h	= dst_fy*ty*itz + dst_cy;
+
+				if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
+					unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
+
+					float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+					float dst_nx = dst_normalsdata[3*dst_ind+0];
+					if(dst_z > 0 && dst_nx != 2){
+						float dst_ny = dst_normalsdata[3*dst_ind+1];
+						float dst_nz = dst_normalsdata[3*dst_ind+2];
+
+						float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
+						float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
+
+						double dst_noise = dst_z * dst_z;
+						double point_noise = src_z * src_z;
+
+						float tnx	= m00*src_nx + m01*src_ny + m02*src_nz;
+						float tny	= m10*src_nx + m11*src_ny + m12*src_nz;
+						float tnz	= m20*src_nx + m21*src_ny + m22*src_nz;
+
+						double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z);
+						//double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise);
+						//d *= src_z;//compare_mul;
+						double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz;
+
+						if(fabs(d) <= 0.01){//If close, according noises, and angle of the surfaces similar: FUSE
+							if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){
+								if(interframe_connectionId.size() > 0){
+									double p_same = 0.99;
+									double not_p_same = 1-p_same;
+									double weight = -log(not_p_same);
+
+									interframe_connectionId[src_ind+offset1].push_back(dst_ind+offset2);
+									interframe_connectionStrength[src_ind+offset1].push_back(weight);
+								}
+								if(dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){
+									overlaps[src_ind]++;
+									if(debugg){
+										informations++;
+										dst_cloud->points[dst_ind].r = 0;
+										dst_cloud->points[dst_ind].g = 255;
+										dst_cloud->points[dst_ind].b = 0;
+										src_cloud->points[src_ind].r = 0;
+										src_cloud->points[src_ind].g = 255;
+										src_cloud->points[src_ind].b = 0;
+									}
+								}
+							}
+						}else if(tz < dst_z && fabs(d) > 0.02 && dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){
+
+							occlusions[src_ind] ++;
+							if(debugg){
+								informations++;
+								src_cloud->points[src_ind].r = 255;
+								src_cloud->points[src_ind].g = 0;
+								src_cloud->points[src_ind].b = 255;
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+
+	if(debugg){printf("informations: %i\n",informations);}
+	if(debugg && informations > 0){
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(src_cloud), "scloud");
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dst_cloud), "dcloud");
+		viewer->spin();
+	}
+}
+
+std::vector< std::vector<float> > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){
+	cv::Mat src						= frame->rgb.clone();
+	unsigned short * depthdata		= (unsigned short	*)(frame->depth.data);
+	const float idepth				= frame->camera->idepth_scale;
+
+	cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT );
+
+	unsigned char * srcdata = (unsigned char *)src.data;
+	unsigned int width = src.cols;
+	unsigned int height = src.rows;
+
+	std::vector<float> dxc;
+	dxc.resize(width*height);
+	for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;}
+
+	std::vector<float> dyc;
+	dyc.resize(width*height);
+	for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;}
+
+	std::vector<double> src_dxdata;
+	src_dxdata.resize(width*height);
+	for(unsigned int i = 0; i < width*height;i++){src_dxdata[i] = 0;}
+
+	std::vector<double> src_dydata;
+	src_dydata.resize(width*height);
+	for(unsigned int i = 0; i < width*height;i++){src_dydata[i] = 0;}
+
+	std::vector<bool> maxima_dxdata;
+	maxima_dxdata.resize(width*height);
+	for(unsigned int i = 0; i < width*height;i++){maxima_dxdata[i] = 0;}
+
+	std::vector<bool> maxima_dydata;
+	maxima_dydata.resize(width*height);
+	for(unsigned int i = 0; i < width*height;i++){maxima_dydata[i] = 0;}
+
+
+	for(unsigned int w = 0; w < width;w++){
+		for(unsigned int h = 0; h < height;h++){
+			int ind = h*width+w;
+			src_dxdata[ind] = 0;
+			src_dydata[ind] = 0;
+		}
+	}
+
+	unsigned int chans = 3;
+	for(unsigned int c = 0; c < chans;c++){
+		for(unsigned int w = 1; w < width;w++){
+			for(unsigned int h = 1; h < height;h++){
+				int ind = h*width+w;
+				int dir		= 1;
+				src_dxdata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-1)+c]) / 255.0)/3.0;
+
+				dir		= width;
+				src_dydata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-width)+c]) / 255.0)/3.0;
+			}
+		}
+
+	}
+
+	for(unsigned int w = 1; w < width-1;w++){
+		for(unsigned int h = 1; h < height-1;h++){
+			int ind = h*width+w;
+			maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1]     && src_dxdata[ind] > src_dxdata[ind+1]);
+			maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]);
+		}
+	}
+
+	std::vector< std::vector<double> > probs;
+	for(unsigned int c = 0; c < chans;c++){
+		std::vector<double> Xvec;
+		int dir;
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				dir		= 1;
+				Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]));
+
+				dir		= width;
+				Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]));
+			}
+		}
+
+		Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size());
+		for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];}
+
+		double stdval = 0;
+		for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);}
+		stdval = sqrt(stdval/double(Xvec.size()));
+
+		DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2();
+		func->zeromean				= true;
+		func->maxp					= 0.99999;
+		func->startreg				= 0.5;
+		func->debugg_print			= false;
+		//func->bidir					= true;
+		func->maxd					= 256.0;
+		func->histogram_size		= 256;
+		func->fixed_histogram_size	= true;
+		func->startmaxd				= func->maxd;
+		func->starthistogram_size	= func->histogram_size;
+		func->blurval				= 0.005;
+		func->stdval2				= stdval;
+		func->maxnoise				= stdval;
+		func->reset();
+		func->computeModel(X);
+
+		std::vector<double> dx;  dx.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;}
+		std::vector<double> dy;	 dy.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;}
+		for(unsigned int w = 1; w < width;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+
+				dir		= 1;
+				dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]));
+
+				dir		= width;
+				dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]));
+			}
+		}
+		delete func;
+
+		probs.push_back(dx);
+		probs.push_back(dy);
+	}
+
+	{
+		std::vector<double> Xvec;
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+
+				if(w > 1){
+					int dir = -1;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					z2 = 2*z2-z3;
+
+					if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));}
+				}
+
+				if(h > 1){
+					int dir = -width;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					z2 = 2*z2-z3;
+
+					if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));}
+				}
+			}
+		}
+
+		Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size());
+		for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];}
+
+		double stdval = 0;
+		for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);}
+		stdval = sqrt(stdval/double(Xvec.size()));
+
+		DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2();
+		funcZ->zeromean				= true;
+		funcZ->startreg				= 0.002;
+		funcZ->debugg_print			= false;
+		//funcZ->bidir				= true;
+		funcZ->maxp					= 0.999999;
+		funcZ->maxd					= 0.1;
+		funcZ->histogram_size		= 100;
+		funcZ->fixed_histogram_size	= true;
+		funcZ->startmaxd			= funcZ->maxd;
+		funcZ->starthistogram_size	= funcZ->histogram_size;
+		funcZ->blurval				= 0.5;
+		funcZ->stdval2				= stdval;
+		funcZ->maxnoise				= stdval;
+		funcZ->reset();
+		funcZ->computeModel(X);
+
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+
+				if(w > 1){
+					int dir = -1;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					z2 = 2*z2-z3;
+
+					if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));}
+				}
+
+				if(h > 1){
+					int dir = -width;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+					z2 = 2*z2-z3;
+
+					if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));}
+				}
+			}
+		}
+
+		std::vector<double> dx;  dx.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;}
+		std::vector<double> dy;	 dy.resize(width*height);	for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;}
+
+		for(unsigned int w = 1; w < width-1;w++){
+			for(unsigned int h = 1; h < height-1;h++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+
+				if(w > 1){
+					int dir = -1;
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+
+					float dz = fabs(z-z2);
+					if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));}
+					if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));}
+				}
+
+				if(h > 1){
+					int dir = -width;
+
+					int other2 = ind+2*dir;
+					int other = ind+dir;
+
+					float z3 = idepth*float(depthdata[other2]);
+					float z2 = idepth*float(depthdata[other]);
+
+					float dz = fabs(z-z2);
+					if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));}
+					if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));}
+				}
+			}
+		}
+
+		delete funcZ;
+		probs.push_back(dx);
+		probs.push_back(dy);
+	}
+
+
+	for(unsigned int w = 0; w < width;w++){
+		for(unsigned int h = 0; h < height;h++){
+			int ind = h*width+w;
+
+			float probX = 0;
+			float probY = 0;
+
+			if(w > 0 && w < width-1){
+				float ax = 0.5;
+				float bx = 0.5;
+				for(unsigned int p = 0; p < probs.size()-2; p+=2){
+					float pr = probs[p][ind];
+					ax *= pr;
+					bx *= 1.0-pr;
+				}
+				float px = ax/(ax+bx);
+				float current = 0;
+				if(!frame->det_dilate.data[ind]){	current = (1-px) * float(maxima_dxdata[ind]);}
+				else{								current = std::max(float(1-probs[probs.size()-2][ind]),0.8f*(1-px) * float(maxima_dxdata[ind]));}
+				probX = 1-current;
+			}
+
+			if(h > 0 && h < height-1){
+				float ay = 0.5;
+				float by = 0.5;
+				for(unsigned int p = 1; p < probs.size()-2; p+=2){
+					float pr = probs[p][ind];
+					ay *= pr;
+					by *= 1.0-pr;
+				}
+				float py = ay/(ay+by);
+				float current = 0;
+				if(!frame->det_dilate.data[ind]){	current = (1-py) * float(maxima_dydata[ind]);}
+				else{								current = std::max(float(1-probs[probs.size()-1][ind]),0.8f*(1-py) * float(maxima_dydata[ind]));}
+				probY = 1-current;
+			}
+
+			dxc[ind] = std::min(probX,probY);
+			dyc[ind] = std::min(probX,probY);
+		}
+	}
+
+
+	std::vector< std::vector<float> > probs2;
+	probs2.push_back(dxc);
+	probs2.push_back(dyc);
+
+	cv::Mat edges;
+	edges.create(height,width,CV_32FC3);
+	float * edgesdata = (float *)edges.data;
+
+	for(unsigned int i = 0; i < width*height;i++){
+		edgesdata[3*i+0] = 0;
+		edgesdata[3*i+1] = dxc[i];
+		edgesdata[3*i+2] = dyc[i];
+	}
+
+	//		cv::namedWindow( "src", cv::WINDOW_AUTOSIZE );          cv::imshow( "src",	src);
+	//		cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE );          cv::imshow( "edges",	edges);
+	//		cv::waitKey(0);
+
+	return probs2;
+}
+
+std::vector<int> doInference(std::vector<double> & prior, std::vector< std::vector<int> > & connectionId, std::vector< std::vector<double> > & connectionStrength){
+	double start_inf = getTime();
+
+	unsigned int nr_data = prior.size();
+	unsigned int nr_edges = 0;
+	for(unsigned int j = 0; j < nr_data;j++){
+		nr_edges += connectionId[j].size();
+	}
+
+	gc::Graph<double,double,double> * g = new gc::Graph<double,double,double>(nr_data,nr_edges);
+	for(unsigned int i = 0; i < nr_data;i++){
+		g -> add_node();
+		double p_fg = prior[i];
+		if(p_fg < 0){
+			g -> add_tweights( i, 0, 0 );
+			continue;
+		}
+		double p_bg = 1-p_fg;
+		double weightFG = -log(p_fg);
+		double weightBG = -log(p_bg);
+		g -> add_tweights( i, weightFG, weightBG );
+	}
+
+	for(unsigned int i = 0; i < nr_data;i++){
+		for(unsigned int j = 0; j < connectionId[i].size();j++){
+			double weight = connectionStrength[i][j];
+			g -> add_edge( i, connectionId[i][j], weight, weight );
+		}
+	}
+
+	g -> maxflow();
+
+	std::vector<int> labels;
+	labels.resize(nr_data);
+	for(unsigned int i = 0; i < nr_data;i++){labels[i] = g->what_segment(i);}
+	delete g;
+
+	double end_inf = getTime();
+	printf("nr data: %i nr edges: %i inference time: %10.10fs\n",nr_data,nr_edges,end_inf-start_inf);
+
+	return labels;
+}
+/*
+void ModelUpdater::getDynamicWeights(bool store_distance, std::vector<double> & dvec, std::vector<double> & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2,  int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<float> > & interframe_connectionStrength, double debugg){
+	std::vector<superpoint> framesp1_test		= frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false);
+	std::vector<ReprojectionResult> rr_vec_test	= frame2->getReprojections(framesp1_test,p,0,false);
+
+	double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size());
+
+	if( inlierratio < 0.01 ){return ;}
+	std::vector<superpoint> framesp1		= frame1->getSuperPoints();
+	std::vector<superpoint> framesp2		= frame2->getSuperPoints();//p);
+
+	getDynamicWeights(store_distance,dvec,nvec,dfunc,nfunc,p,frame1,framesp1_test,framesp1,overlaps,occlusions,frame2,framesp2,offset1,offset2,interframe_connectionId, interframe_connectionStrength, debugg);
+}
+
+void ModelUpdater::getDynamicWeights(bool store_distance, std::vector<double> & dvec, std::vector<double> & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector<superpoint> & tframesp1_test, std::vector<superpoint> & tframesp1, double * overlaps, double * occlusions, RGBDFrame* frame2, std::vector<superpoint> & tframesp2,  int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<float> > & interframe_connectionStrength, double debugg){
+	double totalstartTime = getTime();
+
+	std::string currentString;
+	double startTime;
+	startTime = getTime();
+	unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data);
+	unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data);
+
+	std::vector<superpoint> & framesp1_test		= tframesp1_test;//frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false);
+	std::vector<ReprojectionResult> rr_vec_test	= frame2->getReprojections(framesp1_test,p,0,false);
+	double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size());
+
+	currentString = "part1";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - startTime;
+	startTime = getTime();
+
+	if( inlierratio < 0.01 ){return ;}
+
+	std::vector<superpoint> & framesp1		= tframesp1;//frame1->getSuperPoints();
+	std::vector<superpoint> & framesp2		= tframesp2;//frame2->getSuperPoints();
+
+	currentString = "part2";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - startTime;
+	startTime = getTime();
+
+	std::vector<ReprojectionResult> rr_vec	= frame2->getReprojections(framesp1,p,0,false);
+	inlierratio = double(rr_vec.size())/double(framesp1.size());
+	unsigned long nr_rr = rr_vec.size();
+
+	currentString = "part3";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - startTime;
+	startTime = getTime();
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr src_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dst_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	if(debugg){
+		src_cloud = getPointCloudFromVector(framesp1,3,255,0,255);
+		dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255);
+	}
+
+	double threshold = 0;
+	if(!store_distance){
+		threshold = pow(20.0*dfunc->getNoise(),2);
+	}
+
+	int totsum = 0;
+	for(unsigned long ind = 0; ind < nr_rr;ind++){
+		ReprojectionResult & rr = rr_vec[ind];
+		unsigned int src_ind = rr.src_ind;
+		superpoint & src_p = framesp1[src_ind];
+		if(src_p.point(2) < 0.0){continue;}
+
+		totsum++;
+		unsigned int dst_ind = rr.dst_ind;
+		superpoint & dst_p = framesp2[dst_ind];
+		double src_variance = 1.0/src_p.point_information;
+		double dst_variance = 1.0/dst_p.point_information;
+		double total_variance = src_variance+dst_variance;
+		double total_stdiv = sqrt(total_variance);
+		double d = rr.residualZ/total_stdiv;
+		double surface_angle = rr.angle;
+		if(store_distance){
+			dvec.push_back(d);
+			nvec.push_back(1-surface_angle);
+		}else{
+			double dE2 = rr.residualE2/total_stdiv;
+			double p_overlap_angle = nfunc->getProb(1-surface_angle);
+			double p_overlap = dfunc->getProb(d);
+			double p_occlusion = dfunc->getProbInfront(d);
+
+			p_overlap *= p_overlap_angle;
+
+			if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0 && dE2 < threshold){
+				interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind);
+				interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap));
+			}
+
+			if(debugg){
+				src_cloud->points[src_ind].r = 255.0 * p_overlap;
+				src_cloud->points[src_ind].g = 255.0 * p_occlusion;
+				src_cloud->points[src_ind].b = 0;
+			}
+
+			if(dst_detdata[dst_ind] != 0){continue;}
+			if(src_detdata[src_ind] != 0){continue;}
+
+			overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap));
+			occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion));
+		}
+	}
+	currentString = "part4";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - startTime;
+	startTime = getTime();
+
+	currentString = "total_getDyn";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - totalstartTime;
+	startTime = getTime();
+
+	if(debugg && totsum > 0){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr src_cloud = getPointCloudFromVector(framesp1,3,255,0,255);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255);
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(src_cloud), "scloud");
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dst_cloud), "dcloud");
+		viewer->spin();
+	}
+}
+*/
+
+void ModelUpdater::getDynamicWeights(bool store_distance, std::vector<double> & dvec, std::vector<double> & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, double * notocclusions, RGBDFrame* frame2,  int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<float> > & interframe_connectionStrength, double debugg){
+	std::vector<superpoint> framesp1_test		= frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false);
+	std::vector<ReprojectionResult> rr_vec_test	= frame2->getReprojections(framesp1_test,p,0,false);
+
+	double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size());
+
+	if( inlierratio < 0.01 ){return ;}
+	std::vector<superpoint> framesp1		= frame1->getSuperPoints();
+	std::vector<superpoint> framesp2		= frame2->getSuperPoints();//p);
+
+	getDynamicWeights(store_distance,dvec,nvec,dfunc,nfunc,p,frame1,framesp1_test,framesp1,overlaps,occlusions,notocclusions,frame2,framesp2,offset1,offset2,interframe_connectionId, interframe_connectionStrength, debugg);
+}
+
+void ModelUpdater::getDynamicWeights(bool store_distance, std::vector<double> & dvec, std::vector<double> & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector<superpoint> & tframesp1_test, std::vector<superpoint> & tframesp1, double * overlaps, double * occlusions, double * notocclusions, RGBDFrame* frame2, std::vector<superpoint> & tframesp2,  int offset1, int offset2, std::vector< std::vector<int> > & interframe_connectionId, std::vector< std::vector<float> > & interframe_connectionStrength, double debugg){
+	double totalstartTime = getTime();
+
+	std::string currentString;
+	double startTime;
+	startTime = getTime();
+	unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data);
+	unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data);
+
+	std::vector<superpoint> & framesp1_test		= tframesp1_test;//frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false);
+	std::vector<ReprojectionResult> rr_vec_test	= frame2->getReprojections(framesp1_test,p,0,false);
+	double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size());
+
+	currentString = "part1";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - startTime;
+	startTime = getTime();
+
+	if( inlierratio < 0.01 ){return ;}
+
+	std::vector<superpoint> & framesp1		= tframesp1;//frame1->getSuperPoints();
+	std::vector<superpoint> & framesp2		= tframesp2;//frame2->getSuperPoints();
+
+	currentString = "part2";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - startTime;
+	startTime = getTime();
+
+	std::vector<ReprojectionResult> rr_vec	= frame2->getReprojections(framesp1,p,0,false);
+	inlierratio = double(rr_vec.size())/double(framesp1.size());
+	unsigned long nr_rr = rr_vec.size();
+
+	currentString = "part3";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - startTime;
+	startTime = getTime();
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr src_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dst_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	if(debugg){
+		src_cloud = getPointCloudFromVector(framesp1,3,255,0,255);
+		dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255);
+	}
+
+	double threshold = 0;
+	if(!store_distance){
+		threshold = pow(20.0*dfunc->getNoise(),2);
+	}
+
+	int totsum = 0;
+	for(unsigned long ind = 0; ind < nr_rr;ind++){
+		ReprojectionResult & rr = rr_vec[ind];
+		unsigned int src_ind = rr.src_ind;
+		superpoint & src_p = framesp1[src_ind];
+		if(src_p.point(2) < 0.0){continue;}
+
+		totsum++;
+		unsigned int dst_ind = rr.dst_ind;
+		superpoint & dst_p = framesp2[dst_ind];
+		double src_variance = 1.0/src_p.point_information;
+		double dst_variance = 1.0/dst_p.point_information;
+		double total_variance = src_variance+dst_variance;
+		double total_stdiv = sqrt(total_variance);
+		double d = rr.residualZ/total_stdiv;
+		double surface_angle = rr.angle;
+		if(store_distance){
+			dvec.push_back(d);
+			nvec.push_back(1-surface_angle);
+		}else{
+			double dE2 = rr.residualE2/total_stdiv;
+			double p_overlap_angle = nfunc->getProb(1-surface_angle);
+			double p_overlap = dfunc->getProb(d);
+			double p_occlusion = dfunc->getProbInfront(d);//std::max(1 - 1e-6,std::max(1e-6,dfunc->getProbInfront(d)));
+			double p_behind = 1-p_overlap-p_occlusion;
+
+			p_overlap *= p_overlap_angle;
+
+			if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0 && dE2 < threshold){
+				interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind);
+				interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap));
+			}
+
+			if(debugg){
+				src_cloud->points[src_ind].r = 255.0 * p_overlap;
+				src_cloud->points[src_ind].g = 255.0 * p_occlusion;
+				src_cloud->points[src_ind].b = 0;
+			}
+
+			if(dst_detdata[dst_ind] != 0){continue;}
+			if(src_detdata[src_ind] != 0){continue;}
+
+			double olp = overlaps[src_ind];
+			double nolp = 1-olp;
+			nolp *= (1-p_overlap);
+			//overlaps[src_ind] = std::min(0.9999,std::max(olp,p_overlap));
+			overlaps[src_ind] = std::min(0.999999999,1-nolp);
+			occlusions[src_ind] += p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion));
+			notocclusions[src_ind]++;
+
+			//overlaps[src_ind] = std::min(0.9999,std::max(overlaps[src_ind],p_overlap));
+			//double prev = 1.0-occlusions[src_ind];
+			//notocclusions[src_ind] *= 1.0-p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion));
+
+
+			//occlusions[src_ind] *= p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion));
+			//notocclusions[src_ind] *= 1.0-p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion));
+		}
+	}
+	currentString = "part4";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - startTime;
+	startTime = getTime();
+
+	currentString = "total_getDyn";
+	if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;}
+	benchtime[currentString] += getTime() - totalstartTime;
+	startTime = getTime();
+
+	if(false && !store_distance){
+		cv::Mat overlapsimg;
+		overlapsimg.create(frame1->camera->height,frame1->camera->width,CV_32FC3);
+		float * overlapsimgdata = (float*)overlapsimg.data;
+		cv::Mat occlusionsimg;
+		occlusionsimg.create(frame1->camera->height,frame1->camera->width,CV_32FC3);
+		float * occlusionsimgdata = (float*)occlusionsimg.data;
+		for(unsigned long ind = 0; ind < frame1->camera->height*frame1->camera->width;ind++){
+			overlapsimgdata[3*ind+0] = overlaps[ind];
+			overlapsimgdata[3*ind+1] = overlaps[ind];
+			overlapsimgdata[3*ind+2] = overlaps[ind];
+
+			occlusionsimgdata[3*ind+0] = occlusions[ind]/std::max(1.0,notocclusions[ind]);
+			occlusionsimgdata[3*ind+1] = occlusionsimgdata[3*ind+0];
+			occlusionsimgdata[3*ind+2] = occlusionsimgdata[3*ind+0];
+		}
+		cv::namedWindow( "rgb",				cv::WINDOW_AUTOSIZE );	cv::imshow( "rgb",			frame1->rgb );
+		cv::namedWindow( "occlusionsimg",	cv::WINDOW_AUTOSIZE );	cv::imshow( "occlusionsimg",occlusionsimg );
+		cv::namedWindow( "overlapsimg",		cv::WINDOW_AUTOSIZE );	cv::imshow( "overlapsimg",	overlapsimg );
+		cv::waitKey(0);
+	}
+
+	if(debugg && totsum > 0){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr src_cloud = getPointCloudFromVector(framesp1,3,255,0,255);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255);
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(src_cloud), "scloud");
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dst_cloud), "dcloud");
+		viewer->spin();
+	}
+}
+
+int setupPriors(int method,float current_occlusions, float current_overlaps, float bg_occlusions, float bg_overlaps, bool valid,
+				float & prior_moving, float & prior_dynamic, float & prior_static, float & foreground_weight, float & background_weight){
+	float bias = 0.1;
+	float maxdiff = 0.001;
+
+	foreground_weight = 0;
+	background_weight = 0;
+	prior_moving    = 0.0;
+	prior_dynamic   = 0.0;
+	prior_static    = 0.0;
+
+	if(method == 0){
+		if(valid){
+			float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f);
+			float p_moving  = 0.5f*p_moving_or_dynamic + current_occlusions;
+			float p_dynamic = 0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions)		*   current_overlaps,0.0f);
+			float p_static  = std::max(bg_overlaps-p_moving-p_dynamic,0.0f);
+
+			float leftover = 1.0f-p_moving-p_dynamic-p_static;
+			float notMoving = current_overlaps;
+
+			float p_moving_leftover   =							 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0;
+			float p_dynamic_leftover  = 0.5*leftover*notMoving + 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0;
+			float p_static_leftover   = 0.5*leftover*notMoving;
+
+			prior_moving    = p_moving+p_moving_leftover;
+			prior_dynamic   = p_dynamic+p_dynamic_leftover;
+			prior_static    = p_static+p_static_leftover;
+
+			double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , prior_moving+prior_dynamic))-bias;
+			double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , prior_static))+bias;
+			double norm = p_fg + p_bg;
+			p_fg /= norm;
+			p_bg /= norm;
+
+
+			if(norm > 0){
+				foreground_weight = -log(p_fg);
+				background_weight = -log(p_bg);
+			}
+		}
+	}
+
+	if(method == 1){
+		if(valid){
+
+			float minprob = 0.01;
+			float bias = 0.001;
+			float overlap_same_prob = 0.95;
+
+			//Prob behind all bg
+			float prob_behind = 1-(bg_overlaps+bg_occlusions);
+			float leftover = 1-prob_behind;
+
+			float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f);
+			float p_moving  = 0.5f*p_moving_or_dynamic + current_occlusions;
+			float p_dynamic = 0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions)		*   current_overlaps,0.0f);
+
+			float p_static  = std::max(overlap_same_prob*bg_overlaps-p_moving-p_dynamic,0.0f);
+
+			leftover = 1.0f-p_moving-p_dynamic-p_static;
+			float notMoving = current_overlaps;
+
+			float p_moving_leftover   =	0.5*leftover*(1-notMoving);
+			float p_dynamic_leftover  = 0.5*leftover;
+			float p_static_leftover   = 0.5*leftover*notMoving;
+
+			prior_moving    = (1.0-4.0*minprob-bias)*((1.0-prob_behind)*(p_moving+p_moving_leftover)	+0.25*prob_behind)+		minprob;
+			prior_dynamic   = (1.0-4.0*minprob-bias)*((1.0-prob_behind)*(p_dynamic+p_dynamic_leftover)	+0.25*prob_behind)+		minprob;
+			prior_static    = (1.0-4.0*minprob-bias)*((1.0-prob_behind)*(p_static+p_static_leftover)	+0.50*prob_behind)+ 2.0*minprob + bias;
+
+			foreground_weight = -log(prior_moving+prior_dynamic);
+			background_weight = -log(prior_static);
+		}
+	}
+
+	if(method == 2){
+		if(valid){
+
+			float minprob = 0.1;
+			float bias = 0.00;
+			float overlap_same_prob = 0.7;
+
+			//Prob behind all bg
+			float prob_behind = 1-(bg_overlaps+bg_occlusions);
+			float leftover = 1-prob_behind;
+
+			float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f);
+			float p_moving  = 0.5f*p_moving_or_dynamic + current_occlusions;
+			float p_dynamic = 0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions)		*   current_overlaps,0.0f);
+
+			float p_static  = std::max(overlap_same_prob*bg_overlaps-p_moving-p_dynamic,0.0f);
+
+			leftover = 1.0f-p_moving-p_dynamic-p_static;
+			float notMoving = current_overlaps;
+
+			float p_moving_leftover   =	0.5*leftover*(1-notMoving);
+			float p_dynamic_leftover  = 0.5*leftover;
+			float p_static_leftover   = 0.5*leftover*notMoving;
+
+			prior_moving    = p_moving+p_moving_leftover;
+			prior_dynamic   = (1.0-prob_behind)*(p_dynamic+p_dynamic_leftover);
+			prior_static    = (1.0-prob_behind)*(p_static+p_static_leftover);
+
+			double norm = prior_moving+prior_dynamic+prior_static;
+			prior_dynamic   += (1.0-norm)*0.5;
+			prior_static    += (1.0-norm)*0.5;
+
+			//			prior_moving    = (1.0-3.0*minprob-bias)*prior_moving  + minprob;
+			//			prior_dynamic   = (1.0-3.0*minprob-bias)*prior_dynamic + minprob;
+			//			prior_static    = (1.0-3.0*minprob-bias)*prior_static  + minprob + bias;
+
+			//			prior_moving    = (1.0-3.0*minprob-bias)*prior_moving  + minprob;
+			//			prior_dynamic   = (1.0-3.0*minprob-bias)*prior_dynamic + minprob;
+			//			prior_static    = (1.0-3.0*minprob-bias)*prior_static  + minprob + bias;
+
+			double prob_fg = prior_dynamic;
+			double prob_bg = prior_moving+prior_static;
+			//double norm = prob_fg+prob_bg;
+
+			foreground_weight = -log((1.0-2.0*minprob)*prob_fg+minprob);//+norm*0.5);
+			background_weight = -log((1.0-2.0*minprob)*prob_bg+minprob);//+norm*0.5);
+
+			//			prior_moving    += norm/3.0;
+			//			prior_dynamic   += norm/3.0;
+			//			prior_static    += norm/3.0;
+		}
+	}
+
+	if(method == 3){
+		if(valid){
+			float minprob = 0.01;
+			float bias = 0.01;
+			float overlap_same_prob = 0.75;
+
+			//Prob behind all bg
+			float prob_behind = 1-(bg_overlaps+bg_occlusions);
+
+			float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f);
+			float p_moving  = 0.5f*p_moving_or_dynamic + current_occlusions;
+			float p_dynamic = 0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions)		*   current_overlaps,0.0f);
+
+			float p_static  = std::max(overlap_same_prob*bg_overlaps-p_moving-p_dynamic,0.0f);
+
+			float leftover				= 1.0f-p_moving-p_dynamic-p_static;
+			float notMoving				= 1.0f-current_occlusions;//current_overlaps;
+
+			float p_moving_leftover   =	0.5*leftover*(1-notMoving);
+			float p_dynamic_leftover  = 0.5*leftover;
+			float p_static_leftover   = 0.5*leftover*notMoving;
+
+			prior_moving  = p_moving+p_moving_leftover;
+			prior_dynamic = (1.0-prob_behind)*(p_dynamic+p_dynamic_leftover);
+			prior_static  = (1.0-prob_behind)*(p_static+p_static_leftover);
+
+			float p_dynamic_or_static = 1.0f-p_moving-p_dynamic-p_static;
+			prior_dynamic += 0.5*p_dynamic_or_static;
+			prior_static  += 0.5*p_dynamic_or_static;
+
+			foreground_weight = -log((1.0-2.0*minprob-bias)*(prior_moving+prior_dynamic) + minprob		 );
+			background_weight = -log((1.0-2.0*minprob-bias)* prior_static				 + minprob + bias);
+
+			prior_moving  = (1.0-3.0*minprob-bias)* prior_moving	+ minprob;
+			prior_dynamic = (1.0-3.0*minprob-bias)* prior_dynamic	+ minprob;
+			prior_static  = (1.0-3.0*minprob-bias)* prior_static	+ minprob + bias;
+		}else{
+			prior_static = prior_dynamic = prior_moving = 1.0/3.0;
+			foreground_weight = -log(0.5);
+			background_weight = -log(0.5);
+		}
+	}
+	return 0;
+}
+
+void ModelUpdater::computeMovingDynamicStatic(std::vector<cv::Mat> & movemask, std::vector<cv::Mat> & dynmask, vector<Matrix4d> bgcp, vector<RGBDFrame*> bgcf, vector<Matrix4d> poses, vector<RGBDFrame*> frames, bool debugg, std::string savePath){
+	static int segment_run_counter = -1;
+	segment_run_counter++;
+
+	double computeMovingDynamicStatic_startTime = getTime();
+
+	SegmentationResults sr;
+
+	int tot_nr_pixels = 0;
+	std::vector<int> offsets;
+
+	for(unsigned int i = 0; i < frames.size(); i++){
+		offsets.push_back(tot_nr_pixels);
+		unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height;
+		tot_nr_pixels += nr_pixels;
+	}
+
+	std::vector<unsigned char> labels;
+	labels.resize(tot_nr_pixels);
+
+	//Graph...
+	std::vector< std::vector<int> > interframe_connectionId;
+	std::vector< std::vector<float> > interframe_connectionStrength;
+	interframe_connectionId.resize(tot_nr_pixels);
+	interframe_connectionStrength.resize(tot_nr_pixels);
+
+	int current_point         = 0;
+	float * priors            = new float[3*tot_nr_pixels];
+	float * prior_weights     = new float[2*tot_nr_pixels];
+	bool * valids             = new bool[tot_nr_pixels];
+
+	std::vector<double> dvec;
+	std::vector<double> nvec;
+	DistanceWeightFunction2 * dfunc;
+	DistanceWeightFunction2 * nfunc;
+
+	double startTime = getTime();
+
+	std::vector< std::vector<superpoint> > framesp_test;
+	std::vector< std::vector<superpoint> > framesp;
+	for(unsigned int i = 0; i < frames.size(); i++){
+		framesp_test.push_back(frames[i]->getSuperPoints(Eigen::Matrix4d::Identity(),10,false));
+		framesp.push_back(frames[i]->getSuperPoints());
+	}
+	printf("frames init time: %5.5fs\n",getTime()-startTime);
+
+	startTime = getTime();
+	std::vector< std::vector<superpoint> > bgsp;
+	for(unsigned int i = 0; i < bgcf.size(); i++){
+		bgsp.push_back(bgcf[i]->getSuperPoints());
+	}
+	printf("bg init time:     %5.5fs\n",getTime()-startTime);
+
+	startTime = getTime();
+	for(unsigned int i = 0; i < frames.size(); i++){
+		std::vector<superpoint> & framesp1_test = framesp_test[i];
+		std::vector<superpoint> & framesp1		= framesp[i];
+		for(unsigned int j = 0; j < frames.size(); j++){
+			if(i == j){continue;}
+			Eigen::Matrix4d p = poses[i].inverse() * poses[j];
+			std::vector<superpoint> & framesp2 = framesp[j];
+			getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp1_test,framesp1, 0, 0, 0, frames[j],framesp2,offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false);
+		}
+	}
+
+	double dstdval = 0;
+	for(unsigned int i = 0; i < dvec.size(); i++){dstdval += dvec[i]*dvec[i];}
+	dstdval = sqrt(dstdval/double(dvec.size()-1));
+
+	GeneralizedGaussianDistribution * dggdnfunc	= new GeneralizedGaussianDistribution(true,true,false,true,true);
+	dggdnfunc->nr_refineiters					= 4;
+	DistanceWeightFunction2PPR3 * dfuncTMP		= new DistanceWeightFunction2PPR3(dggdnfunc);
+	dfunc = dfuncTMP;
+	dfuncTMP->startreg				= 0.00;
+	dfuncTMP->max_under_mean		= false;
+	dfuncTMP->debugg_print			= false;
+	dfuncTMP->bidir					= true;
+	dfuncTMP->zeromean				= false;
+	dfuncTMP->maxp					= 0.9999;
+	dfuncTMP->maxd					= 0.5;
+	dfuncTMP->histogram_size		= 1000;
+	dfuncTMP->fixed_histogram_size	= false;
+	dfuncTMP->startmaxd				= dfuncTMP->maxd;
+	dfuncTMP->starthistogram_size	= dfuncTMP->histogram_size;
+	dfuncTMP->blurval				= 0.5;
+	dfuncTMP->maxnoise				= dstdval;
+	dfuncTMP->compute_infront		= true;
+	dfuncTMP->ggd					= true;
+	dfuncTMP->reset();
+
+	if(savePath.size() != 0){
+		dfuncTMP->savePath = std::string(savePath)+"/segment_"+std::to_string(segment_run_counter)+"_dfunc.m";
+	}
+
+	dfunc->computeModel(dvec);
+
+	GeneralizedGaussianDistribution * ggdnfunc	= new GeneralizedGaussianDistribution(true,true);
+	ggdnfunc->nr_refineiters					= 4;
+	DistanceWeightFunction2PPR3 * nfuncTMP		= new DistanceWeightFunction2PPR3(ggdnfunc);
+	nfunc = nfuncTMP;
+	nfuncTMP->startreg				= 0.00;
+	nfuncTMP->debugg_print			= false;
+	nfuncTMP->bidir					= false;
+	nfuncTMP->zeromean				= true;
+	nfuncTMP->maxp					= 0.9999;
+	nfuncTMP->maxd					= 1.0;
+	nfuncTMP->histogram_size		= 1000;
+	nfuncTMP->fixed_histogram_size	= true;
+	nfuncTMP->startmaxd				= nfuncTMP->maxd;
+	nfuncTMP->starthistogram_size	= nfuncTMP->histogram_size;
+	nfuncTMP->blurval				= 0.5;
+	nfuncTMP->stdval2				= 1;
+	nfuncTMP->maxnoise				= 1;
+	nfuncTMP->ggd					= true;
+	nfuncTMP->reset();
+	nfunc->computeModel(nvec);
+
+	if(savePath.size() != 0){
+		nfuncTMP->savePath = std::string(savePath)+"/segment_"+std::to_string(segment_run_counter)+"_nfunc.m";
+	}
+
+
+
+	printf("training time:     %5.5fs\n",getTime()-startTime);
+
+	long frameConnections = 0;
+	std::vector< std::vector< std::vector<float> > > pixel_weights;
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud  (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+	double total_priortime = 0;
+	double total_connectiontime = 0;
+	double total_alloctime = 0;
+	double total_dealloctime = 0;
+	double total_Dynw = 0;
+
+	double maxprob_same = 0.999999999999999999;
+
+	for(unsigned int i = 0; i < frames.size(); i++){
+		if(debugg != 0){printf("currently workin on frame %i\n",i);}
+		int offset = offsets[i];
+		RGBDFrame * frame = frames[i];
+		float		   * normalsdata	= (float			*)(frame->normals.data);
+		startTime = getTime();
+		std::vector< std::vector<float> > probs = frame->getImageProbs();
+
+		total_connectiontime += getTime()-startTime;
+		pixel_weights.push_back(probs);
+		unsigned short * depthdata	= (unsigned short	*)(frame->depth.data);
+
+		Camera * camera				= frame->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
+		const float idepth			= camera->idepth_scale;
+		const float cx				= camera->cx;
+		const float cy				= camera->cy;
+		const float ifx				= 1.0/camera->fx;
+		const float ify				= 1.0/camera->fy;
+
+		startTime = getTime();
+		unsigned int nr_pixels = width * height;
+		double * current_overlaps	= new double[nr_pixels];
+		double * current_occlusions		= new double[nr_pixels];
+		double * current_notocclusions		= new double[nr_pixels];
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			current_overlaps[j] = 0;
+			current_occlusions[j] = 0.0;
+			current_notocclusions[j] = 0.0;
+		}
+
+		double * bg_overlaps			= new double[nr_pixels];
+		double * bg_occlusions			= new double[nr_pixels];
+		double * bg_notocclusions		= new double[nr_pixels];
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			bg_overlaps[j]	= 0;
+			bg_occlusions[j] = 0.0;
+			bg_notocclusions[j] = 0.0;
+		}
+		total_alloctime += getTime()-startTime;
+
+		startTime = getTime();
+		for(unsigned int j = 0; j < frames.size(); j++){
+			if(i == j){continue;}
+			Eigen::Matrix4d p = poses[i].inverse() * poses[j];
+			getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], current_overlaps, current_occlusions, current_notocclusions, frames[j],framesp[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false);
+		}
+
+		for(unsigned int j = 0; j < bgcf.size(); j++){
+			Eigen::Matrix4d p = poses[i].inverse() * bgcp[j];
+			getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], bg_overlaps, bg_occlusions, bg_notocclusions, bgcf[j],bgsp[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false);
+		}
+
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			bg_occlusions[j]		= std::min(0.99999,bg_occlusions[j]/std::max(1.0,bg_notocclusions[j]));
+			current_occlusions[j]	= std::min(0.99999,current_occlusions[j]/std::max(1.0,current_notocclusions[j]));
+		}
+
+		total_Dynw += getTime()-startTime;
+
+
+
+		startTime = getTime();
+		unsigned char * detdata = (unsigned char*)(frame->det_dilate.data);
+		for(unsigned int h = 0; h < height;h++){
+			for(unsigned int w = 0; w < width;w++){
+				int ind = h*width+w;
+
+				valids[offset+ind] = detdata[ind] == 0 && normalsdata[3*ind] != 2;
+				setupPriors(3,
+						current_occlusions[ind],current_overlaps[ind],bg_occlusions[ind],bg_overlaps[ind],valids[offset+ind],
+						priors[3*(offset+ind)+0], priors[3*(offset+ind)+1],priors[3*(offset+ind)+2],
+						prior_weights[2*(offset+ind)+0], prior_weights[2*(offset+ind)+1]);
+
+				if(probs[0][ind] > 0.00000001){frameConnections++;}
+				if(probs[1][ind] > 0.00000001){frameConnections++;}
+
+				current_point++;
+			}
+		}
+
+		double start_inf = getTime();
+		gc::Graph<double,double,double> * g = new gc::Graph<double,double,double>(nr_pixels,2*nr_pixels);
+		for(unsigned long ind = 0; ind < nr_pixels;ind++){
+			g -> add_node();
+			double weightFG = prior_weights[2*(offset+ind)+0];
+			double weightBG = prior_weights[2*(offset+ind)+1];
+			g -> add_tweights( ind, weightFG, weightBG );
+		}
+
+		for(unsigned int w = 0; w < width;w++){
+			for(unsigned int h = 0; h < height;h++){
+				int ind = h*width+w;
+				if(w > 0 && probs[0][ind] > 0.00000001){
+					int other = ind-1;
+					double p_same = std::min(double(probs[0][ind]),maxprob_same);
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					g -> add_edge( ind, other, weight, weight );
+				}
+
+				if(h > 0 && probs[1][ind] > 0.00000001){
+					int other = ind-width;
+					double p_same = std::min(double(probs[1][ind]),maxprob_same);
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					g -> add_edge( ind, other, weight, weight );
+				}
+			}
+		}
+
+		g -> maxflow();
+		for(unsigned long ind = 0; ind < nr_pixels;ind++){labels[offset+ind] = g->what_segment(ind);}
+
+		if(debugg != 0){printf("local inference time: %10.10fs\n\n",getTime()-start_inf);}
+
+
+
+
+		//		cv::Mat detrgb;
+		//		detrgb.create(height,width,CV_8UC3);
+
+		//		for(unsigned long ind = 0; ind < nr_pixels;ind++){
+		//			int dd = detdata[ind] == 0;
+		//			detrgb.data[3*ind+0] = dd * frame->rgb.data[3*ind+0];
+		//			detrgb.data[3*ind+1] = dd * frame->rgb.data[3*ind+1];
+		//			detrgb.data[3*ind+2] = dd * frame->rgb.data[3*ind+2];
+		//		}
+
+		//		cv::namedWindow( "edgeimg"		, cv::WINDOW_AUTOSIZE );		cv::imshow( "edgeimg",		edgeimg );
+		//		cv::namedWindow( "det_dilate"	, cv::WINDOW_AUTOSIZE );		cv::imshow( "det_dilate",	frame->det_dilate);
+		//		cv::namedWindow( "det_dilate2"	, cv::WINDOW_AUTOSIZE );		cv::imshow( "det_dilate2",	detrgb);
+		//		cv::namedWindow( "rgb"			, cv::WINDOW_AUTOSIZE );		cv::imshow( "rgb",			frame->rgb );
+		//		cv::namedWindow( "depth"		, cv::WINDOW_AUTOSIZE );		cv::imshow( "depth",		frame->depth );
+		//		cv::namedWindow( "priors"		, cv::WINDOW_AUTOSIZE );		cv::imshow( "priors",		priorsimg );
+		//		cv::namedWindow( "labelimg"		, cv::WINDOW_AUTOSIZE );		cv::imshow( "labelimg",		labelimg );
+		//		cv::waitKey(0);
+
+
+		if(savePath.size() != 0){
+			cv::Mat edgeimg;
+			edgeimg.create(height,width,CV_8UC3);
+			unsigned char * edgedata = (unsigned char*)edgeimg.data;
+
+			for(int j = 0; j < width*height; j++){
+				edgedata[3*j+0] = 0;
+				edgedata[3*j+1] = 255.0*(1-probs[0][j]);
+				edgedata[3*j+2] = 255.0*(1-probs[1][j]);
+			}
+
+			cv::Mat labelimg;
+			labelimg.create(height,width,CV_8UC3);
+			unsigned char * labelimgdata = (unsigned char*)labelimg.data;
+			for(unsigned long ind = 0; ind < nr_pixels;ind++){
+				double label = g->what_segment(ind);
+				labelimgdata[3*ind+0] = 255*label;
+				labelimgdata[3*ind+1] = 255*label;
+				labelimgdata[3*ind+2] = 255*label;
+			}
+
+			cv::Mat priorsimg;
+			priorsimg.create(height,width,CV_8UC3);
+			unsigned char * priorsdata = (unsigned char*)priorsimg.data;
+			for(unsigned long ind = 0; ind < nr_pixels;ind++){
+				priorsdata[3*ind+0]			= 255.0*priors[3*(offset+ind)+2];
+				priorsdata[3*ind+1]			= 255.0*priors[3*(offset+ind)+1];
+				priorsdata[3*ind+2]			= 255.0*priors[3*(offset+ind)+0];
+			}
+
+			cv::Mat current_overlapsimg;
+			current_overlapsimg.create(height,width,CV_8UC3);
+			unsigned char * current_overlapsdata = (unsigned char*)current_overlapsimg.data;
+			for(unsigned long ind = 0; ind < nr_pixels;ind++){
+				current_overlapsdata[3*ind+0]			= 255.0*current_overlaps[ind];
+				current_overlapsdata[3*ind+1]			= 255.0*current_overlaps[ind];
+				current_overlapsdata[3*ind+2]			= 255.0*current_overlaps[ind];
+			}
+
+			cv::Mat bg_overlapsimg;
+			bg_overlapsimg.create(height,width,CV_8UC3);
+			unsigned char * bg_overlapsdata = (unsigned char*)bg_overlapsimg.data;
+			for(unsigned long ind = 0; ind < nr_pixels;ind++){
+				bg_overlapsdata[3*ind+0]			= 255.0*bg_overlaps[ind];
+				bg_overlapsdata[3*ind+1]			= 255.0*bg_overlaps[ind];
+				bg_overlapsdata[3*ind+2]			= 255.0*bg_overlaps[ind];
+			}
+
+			cv::Mat current_occlusionsimg;
+			current_occlusionsimg.create(height,width,CV_8UC3);
+			unsigned char * current_occlusionsdata = (unsigned char*)current_occlusionsimg.data;
+			for(unsigned long ind = 0; ind < nr_pixels;ind++){
+				current_occlusionsdata[3*ind+0]			= 255.0*current_occlusions[ind];
+				current_occlusionsdata[3*ind+1]			= 255.0*current_occlusions[ind];
+				current_occlusionsdata[3*ind+2]			= 255.0*current_occlusions[ind];
+			}
+
+			cv::Mat bg_occlusionsimg;
+			bg_occlusionsimg.create(height,width,CV_8UC3);
+			unsigned char * bg_occlusionsdata = (unsigned char*)bg_occlusionsimg.data;
+			for(unsigned long ind = 0; ind < nr_pixels;ind++){
+				bg_occlusionsdata[3*ind+0]			= 255.0*bg_occlusions[ind];
+				bg_occlusionsdata[3*ind+1]			= 255.0*bg_occlusions[ind];
+				bg_occlusionsdata[3*ind+2]			= 255.0*bg_occlusions[ind];
+			}
+
+			cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_current_overlapsimg.png", current_overlapsimg );
+			cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_bg_overlapsimg.png", bg_overlapsimg );
+			cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_current_occlusionsimg.png", current_occlusionsimg );
+			cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_bg_occlusionsimg.png", bg_occlusionsimg );
+			cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_edgeimg.png", edgeimg );
+			cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_rgb.png", frame->rgb  );
+			cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_priors.png", priorsimg );
+			cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_labelimg.png", labelimg );
+		}
+		delete g;
+
+		Eigen::Matrix4d p = poses[i];
+		float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+		float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+		float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+		for(unsigned int h = 0; h < height;h++){
+			for(unsigned int w = 0; w < width;w++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+				float x = (float(w) - cx) * z * ifx;
+				float y = (float(h) - cy) * z * ify;
+				pcl::PointXYZRGBNormal point;
+				point.x = m00*x + m01*y + m02*z + m03;
+				point.y = m10*x + m11*y + m12*z + m13;
+				point.z = m20*x + m21*y + m22*z + m23;
+				point.r = frame->rgb.data[3*ind+2];//priors[3*(offset+ind)+0]*255.0;
+				point.g = frame->rgb.data[3*ind+1];//priors[3*(offset+ind)+1]*255.0;
+				point.b = frame->rgb.data[3*ind+0];//priors[3*(offset+ind)+2]*255.0;
+				cloud->points.push_back(point);
+			}
+		}
+		total_priortime += getTime()-startTime;
+
+		startTime = getTime();
+		delete[] current_occlusions;
+		delete[] current_notocclusions;
+		delete[] current_overlaps;
+		delete[] bg_occlusions;
+		delete[] bg_notocclusions;
+		delete[] bg_overlaps;
+		total_dealloctime += getTime()-startTime;
+	}
+
+	delete dfuncTMP;
+	delete nfuncTMP;
+
+	printf("total_priortime        = %5.5fs\n",		total_priortime);
+	printf("total_connectiontime   = %5.5fs\n",		total_connectiontime);
+	printf("total_alloctime        = %5.5fs\n",		total_alloctime);
+	printf("total_dealloctime      = %5.5fs\n",		total_dealloctime);
+	printf("total_Dynw             = %5.5fs\n",		total_Dynw);
+
+	long interframeConnections = 0;
+	for(unsigned int i = 0; i < interframe_connectionId.size();i++){interframeConnections += interframe_connectionId[i].size();}
+
+	double start_inf = getTime();
+	gc::Graph<double,double,double> * g = new gc::Graph<double,double,double>(current_point,frameConnections+interframeConnections);
+	for(unsigned long i = 0; i < current_point;i++){
+		g -> add_node();
+		double weightFG = prior_weights[2*i+0];
+		double weightBG = prior_weights[2*i+1];
+
+		g -> add_tweights( i, weightFG, weightBG );
+	}
+
+	//double maxprob_same = 0.99999999999;
+	for(unsigned int i = 0; i < frames.size(); i++){
+		int offset = offsets[i];
+		Camera * camera				= frames[i]->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
+		std::vector< std::vector<float> > & probs = pixel_weights[i];
+		for(unsigned int w = 0; w < width;w++){
+			for(unsigned int h = 0; h < height;h++){
+				int ind = h*width+w;
+				if(w > 0 && probs[0][ind] > 0.00000001 && w < width-1){
+					int other = ind-1;
+					double p_same = std::min(double(probs[0][ind]),maxprob_same);
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					g -> add_edge( ind+offset, other+offset, weight, weight );
+				}
+
+				if(h > 0 && probs[1][ind] > 0.00000001 && h < height-1) {
+					int other = ind-width;
+					double p_same = std::min(double(probs[1][ind]),maxprob_same);
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					g -> add_edge( ind+offset, other+offset, weight, weight );
+				}
+			}
+		}
+	}
+
+	std::vector<std::vector<unsigned long> > interframe_connectionId_added;
+	interframe_connectionId_added.resize(interframe_connectionId.size());
+
+	std::vector<std::vector<double> > interframe_connectionStrength_added;
+	interframe_connectionStrength_added.resize(interframe_connectionStrength.size());
+
+	double initAdded = 0;
+	for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+		for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+			double weight = interframe_connectionStrength[i][j];
+			unsigned long other = interframe_connectionId[i][j];
+			if(weight > 0.01 && labels[i] != labels[other]){
+				g -> add_edge( i, other, weight, weight );
+
+				interframe_connectionId_added[i].push_back(other);
+				interframe_connectionStrength_added[i].push_back(weight);
+
+				interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back();
+				interframe_connectionStrength[i].pop_back();
+
+				interframe_connectionId[i][j] = interframe_connectionId[i].back();
+				interframe_connectionId[i].pop_back();
+				j--;
+
+				initAdded++;
+			}
+		}
+	}
+
+	g -> maxflow();
+	for(unsigned long ind = 0; ind < current_point;ind++){labels[ind] = g->what_segment(ind);}
+
+	double tot_inf = 0;
+	for(unsigned int it = 0; it < 40; it++){
+		double start_inf1 = getTime();
+
+		double diffs = 0;
+		for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+			for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+				double weight = interframe_connectionStrength[i][j];
+				unsigned long other = interframe_connectionId[i][j];
+				if(weight > 0.01 && labels[i] != labels[other]){diffs++;}
+			}
+		}
+
+
+		double adds = 100000;
+		double prob = std::min(adds / diffs,1.0);
+		printf("diffs: %f adds: %f prob: %f ",diffs,adds,prob);
+		printf("ratio of total diffs: %f\n",diffs/double(frameConnections+interframeConnections));
+
+		if(diffs == 0){break;}
+
+		double trueadds = 0;
+		for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+			for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+				double weight = interframe_connectionStrength[i][j];
+				unsigned long other = interframe_connectionId[i][j];
+				if(weight > 0.01 && labels[i] != labels[other]){
+					if(rand() <= prob*RAND_MAX){
+						trueadds++;
+						g -> add_edge( i, other, weight, weight );
+
+						interframe_connectionId_added[i].push_back(other);
+						interframe_connectionStrength_added[i].push_back(weight);
+
+						interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back();
+						interframe_connectionStrength[i].pop_back();
+
+						interframe_connectionId[i][j] = interframe_connectionId[i].back();
+						interframe_connectionId[i].pop_back();
+						j--;
+					}
+				}
+			}
+		}
+
+		g -> maxflow();
+		for(unsigned long ind = 0; ind < current_point;ind++){labels[ind] = g->what_segment(ind);}
+
+		tot_inf += getTime()-start_inf1;
+		if(debugg != 0){printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf);}
+
+		if(tot_inf > 90){break;}
+	}
+
+	double interfrace_constraints_added = 0;
+	for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+		interfrace_constraints_added += interframe_connectionId_added[i].size();
+		for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){
+			interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]);
+			interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]);
+		}
+	}
+
+	delete g;
+	double end_inf = getTime();
+	printf("static inference time: %10.10fs interfrace_constraints added ratio: %f\n",end_inf-start_inf,interfrace_constraints_added/double(interframeConnections));
+
+	const unsigned int nr_frames		= frames.size();
+	const unsigned int width			= frames[0]->camera->width;
+	const unsigned int height			= frames[0]->camera->height;
+	const unsigned int pixels_per_image	= width*height;
+	const unsigned int nr_pixels		= nr_frames*pixels_per_image;
+
+	double probthresh = 0.5;
+	double str_probthresh = -log(probthresh);
+	unsigned int number_of_dynamics = 0;
+	unsigned int nr_obj_dyn = 0;
+	unsigned int nr_obj_mov = 0;
+	std::vector<unsigned int> objectlabel;
+	std::vector<int> labelID;
+	labelID.push_back(0);
+	objectlabel.resize(nr_pixels);
+
+	for(unsigned long i = 0; i < nr_pixels; i++){objectlabel[i] = 0;}
+	for(unsigned long ind = 0; ind < nr_pixels; ind++){
+		if(valids[ind] && objectlabel[ind] == 0 && labels[ind] != 0){
+			unsigned int current_label = labels[ind];
+			number_of_dynamics++;
+			objectlabel[ind] = number_of_dynamics;
+			unsigned long todocounter = 0;
+			std::vector< unsigned long > todo;
+			todo.push_back(ind);
+
+			double score0 = 0;
+			double score1 = 0;
+
+			double pscore0 = 0;
+			double pscore1 = 0;
+
+			double nscore0 = 0;
+			double nscore1 = 0;
+
+			double totsum = 0;
+			while(todocounter < todo.size()){
+				unsigned long cind = todo[todocounter++];
+				unsigned long frameind = cind / pixels_per_image;
+
+
+				unsigned long iind = cind % pixels_per_image;
+				unsigned long w = iind % width;
+				unsigned long h = iind / width;
+
+				double p0 = priors[3*cind+0];
+				double p1 = priors[3*cind+1];
+				double p2 = priors[3*cind+2];
+
+				if(valids[cind]){
+					double s0 = 0;
+					if(p1 > p2){s0 += p0 - p1;}
+					else{       s0 += p0 - p2;}
+					score0 += s0;
+
+					if(s0 > 0){	pscore0 += s0;}
+					else{		nscore0 += s0;}
+
+					double s1 = 0;
+					if(p0 > p2){s1 += p1 - p0;}
+					else{       s1 += p1 - p2;}
+					score1 += s1;
+
+					if(s1 > 0){	pscore1 += s1;}
+					else{		nscore1 += s1;}
+					totsum++;
+				}
+
+				float * dedata = (float*)(frames[frameind]->de.data);
+				unsigned short * depthdata = (unsigned short *)(frames[frameind]->depth.data);
+				if(depthdata[iind] == 0){printf("big giant WTF... file %i line %i\n",__FILE__,__LINE__);continue;}
+
+				int dir;
+				dir = -1;
+				if( w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && depthdata[iind+dir] != 0 && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){
+					objectlabel[cind+dir] = number_of_dynamics;
+					todo.push_back(cind+dir);
+				}
+
+				dir = 1;
+				if( w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && depthdata[iind+dir] != 0 && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){
+					objectlabel[cind+dir] = number_of_dynamics;
+					todo.push_back(cind+dir);
+				}
+
+				dir = -width;
+				if( h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && depthdata[iind+dir] != 0 && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){
+					objectlabel[cind+dir] = number_of_dynamics;
+					todo.push_back(cind+dir);
+				}
+
+				dir = width;
+				if( h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && depthdata[iind+dir] != 0 && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){
+					objectlabel[cind+dir] = number_of_dynamics;
+					todo.push_back(cind+dir);
+				}
+
+				for(unsigned long j = 0; j < interframe_connectionId[cind].size();j++){
+					unsigned long other = interframe_connectionId[cind][j];
+					if(interframe_connectionStrength[cind][j] > str_probthresh && objectlabel[other] == 0 && labels[other] == current_label){
+						objectlabel[other] = number_of_dynamics;
+						todo.push_back(other);
+					}
+				}
+			}
+
+			score0 = 100.0*pscore0+nscore0;
+			score1 = pscore1+nscore1;
+
+
+			labelID.push_back(0);
+			if(debugg != 0){
+				if(totsum > 100){
+					printf("---------------------------\n");
+					printf("score0: %10.10f score1: %10.10f ",score0,score1);
+					printf("totsum: %10.10f\n",totsum);
+
+					printf("pscore0: %10.10f nscore0: %10.10f ",pscore0,nscore0);
+					printf("pscore1: %10.10f nscore1: %10.10f\n",pscore1,nscore1);
+				}
+			}
+
+			if(std::max(score0,score1) < 100){continue;}
+
+			if(score1 > score0){
+				labelID.back() = ++nr_obj_dyn;
+				if(debugg != 0){printf("Dynamic: %f -> %f\n",score1,totsum);}
+				sr.component_dynamic.push_back(todo);
+				sr.scores_dynamic.push_back(score1);
+				sr.total_dynamic.push_back(totsum);
+			}else{
+				if(score0 > 10000){
+					labelID.back() = --nr_obj_mov;
+					if(debugg != 0){printf("Moving: %f -> %f\n",score0,totsum);}
+					sr.component_moving.push_back(todo);
+					sr.scores_moving.push_back(score0);
+					sr.total_moving.push_back(totsum);
+				}
+			}
+		}
+	}
+
+	for(unsigned long ind = 0; ind < nr_pixels; ind++){
+		unsigned int ol = objectlabel[ind];
+		if(ol != 0 && labelID[ol] == 0){
+			objectlabel[ind] = 0;
+			labels[ind] = 0;
+		}
+	}
+
+	for(unsigned int i = 0; i < interframe_connectionId.size();i++){interframe_connectionId[i].clear();}
+	interframe_connectionId.clear();
+
+	for(unsigned int i = 0; i < interframe_connectionStrength.size();i++){interframe_connectionStrength[i].clear();}
+	interframe_connectionStrength.clear();
+
+	printf("connectedComponent: %5.5fs\n",getTime()-start_inf);
+
+	int current = 0;
+	for(unsigned long i = 0; i < frames.size(); i++){
+		Camera * camera				= frames[i]->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
+
+		cv::Mat m;
+		m.create(height,width,CV_8UC1);
+		unsigned char * mdata = (unsigned char*)m.data;
+
+		cv::Mat d;
+		d.create(height,width,CV_8UC1);
+		unsigned char * ddata = (unsigned char*)d.data;
+
+		cv::Mat d2;
+		d2.create(height,width,CV_8UC1);
+		unsigned char * ddata2 = (unsigned char*)d2.data;
+
+
+		for(int j = 0; j < width*height; j++){
+			mdata[j] = 0;
+			ddata[j] = 0;
+			ddata2[j] = labels[current];
+			unsigned int label = objectlabel[current];
+			int lid = labelID[label];
+			if(lid >  0){
+				ddata[j] = lid;
+			}else if(lid < 0){
+				mdata[j] = -lid;
+			}
+			current++;
+		}
+		movemask.push_back(m);
+		dynmask.push_back(d);
+	}
+
+	if(savePath.size() != 0){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_sample (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		cloud_sample->points.resize(current_point);
+		cloud_sample->width = current_point;
+		cloud_sample->height = 1;
+		for(unsigned int i = 0; i < current_point; i++){
+			cloud_sample->points[i]	= cloud->points[i];
+			cloud_sample->points[i].r = priors[3*i+0]*255.0;
+			cloud_sample->points[i].g = priors[3*i+1]*255.0;
+			cloud_sample->points[i].b = priors[3*i+2]*255.0;
+		}
+		pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_priors.pcd", *cloud_sample);
+
+		for(unsigned int i = 0; i < current_point; i++){
+			cloud_sample->points[i].r = 0;
+			cloud_sample->points[i].g = 0;
+			cloud_sample->points[i].b = 255;
+		}
+
+		for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){
+			for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){
+				cloud_sample->points[sr.component_dynamic[c][i]].r = 0;
+				cloud_sample->points[sr.component_dynamic[c][i]].g = 255;
+				cloud_sample->points[sr.component_dynamic[c][i]].b = 0;
+			}
+		}
+
+		for(unsigned int c = 0; c < sr.component_moving.size(); c++){
+			for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){
+				cloud_sample->points[sr.component_moving[c][i]].r = 255;
+				cloud_sample->points[sr.component_moving[c][i]].g = 0;
+				cloud_sample->points[sr.component_moving[c][i]].b = 0;
+			}
+		}
+		pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_classes.pcd", *cloud_sample);
+
+		for(unsigned int i = 0; i < current_point; i++){
+			cloud_sample->points[i].r = 0;
+			cloud_sample->points[i].g = 0;
+			cloud_sample->points[i].b = 255;
+		}
+
+		for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){
+			int randr = rand()%256;
+			int randg = rand()%256;
+			int randb = rand()%256;
+
+			for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){
+				cloud_sample->points[sr.component_dynamic[c][i]].r = randr;
+				cloud_sample->points[sr.component_dynamic[c][i]].g = randg;
+				cloud_sample->points[sr.component_dynamic[c][i]].b = randb;
+			}
+		}
+
+		for(unsigned int c = 0; c < sr.component_moving.size(); c++){
+			int randr = rand()%256;
+			int randg = rand()%256;
+			int randb = rand()%256;
+
+			for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){
+				cloud_sample->points[sr.component_moving[c][i]].r = randr;
+				cloud_sample->points[sr.component_moving[c][i]].g = randg;
+				cloud_sample->points[sr.component_moving[c][i]].b = randb;
+			}
+		}
+		pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_clusters.pcd", *cloud_sample);
+
+		cloud->width = cloud->points.size();
+		cloud->height = 1;
+		pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_full.pcd", *cloud);
+
+		cloud_sample->points.resize(0);
+		for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){
+			for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){
+				cloud_sample->points.push_back(cloud->points[sr.component_dynamic[c][i]]);
+			}
+		}
+		cloud_sample->width = cloud_sample->points.size();
+		cloud_sample->height = 1;
+		pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_dynamicobjects.pcd", *cloud_sample);
+	}
+
+
+	printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime);
+	if(debugg){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_sample (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+		for(unsigned int i = 0; i < current_point; i++){
+			cloud->points[i].r = priors[3*i+0]*255.0;
+			cloud->points[i].g = priors[3*i+1]*255.0;
+			cloud->points[i].b = priors[3*i+2]*255.0;
+		}
+
+		cloud_sample->points.clear();
+		for(unsigned int i = 0; i < current_point; i++){
+			if(rand() % 4 == 0){
+				cloud_sample->points.push_back(cloud->points[i]);
+			}
+		}
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_sample), "cloud");
+		viewer->spin();
+
+		cloud_sample->points.clear();
+		for(unsigned int i = 0; i < current_point; i++){
+			if(rand() % 4 == 0){
+
+				double p_fg = exp(-prior_weights[2*i+0]);
+
+				cloud_sample->points.push_back(cloud->points[i]);
+				cloud_sample->points.back().r = p_fg*255.0;//(priors[3*i+0]+priors[3*i+2])*255.0;
+				cloud_sample->points.back().g = p_fg*255.0;
+				cloud_sample->points.back().b = p_fg*255.0;//;
+			}
+		}
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_sample), "cloud");
+		viewer->spin();
+
+
+		cloud_sample->points.clear();
+		for(unsigned int i = 0; i < current_point; i++){
+			if(rand() % 4 == 0 && labels[i] == 0){
+				cloud_sample->points.push_back(cloud->points[i]);
+				cloud_sample->points.back().r = 0;
+				cloud_sample->points.back().g = 0;
+				cloud_sample->points.back().b = 255;
+			}
+		}
+
+		for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){
+			int randr = rand()%256;
+			int randg = rand()%256;
+			int randb = rand()%256;
+			for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){
+				cloud_sample->points.push_back(cloud->points[sr.component_dynamic[c][i]]);
+				cloud_sample->points.back().r = 0;//randr;
+				cloud_sample->points.back().g = 255;//randg;
+				cloud_sample->points.back().b = 0;//randb;
+			}
+		}
+
+		for(unsigned int c = 0; c < sr.component_moving.size(); c++){
+			int randr = rand()%256;
+			int randg = rand()%256;
+			int randb = rand()%256;
+			for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){
+				cloud_sample->points.push_back(cloud->points[sr.component_moving[c][i]]);
+				cloud_sample->points.back().r = 255;//randr;
+				cloud_sample->points.back().g = 0;//randg;
+				cloud_sample->points.back().b = 0;//randb;
+			}
+		}
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_sample), "cloud");
+		viewer->spin();
+
+		cloud_sample->points.clear();
+		for(unsigned int i = 0; i < current_point; i++){
+			if(rand() % 1 == 0){
+				cloud_sample->points.push_back(cloud->points[i]);
+			}
+		}
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_sample), "cloud");
+		viewer->spin();
+	}
+
+	delete[] valids;
+	delete[] priors;
+	delete[] prior_weights;
+	//printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime);
+}
+
+/*
+void ModelUpdater::computeMovingDynamicStatic(std::vector<cv::Mat> & movemask, std::vector<cv::Mat> & dynmask, vector<Matrix4d> bgcp, vector<RGBDFrame*> bgcf, vector<Matrix4d> poses, vector<RGBDFrame*> frames, bool debugg){
+	double computeMovingDynamicStatic_startTime = getTime();
+
+	SegmentationResults sr;
+
+	int tot_nr_pixels = 0;
+	std::vector<int> offsets;
+
+	for(unsigned int i = 0; i < frames.size(); i++){
+		offsets.push_back(tot_nr_pixels);
+		unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height;
+		tot_nr_pixels += nr_pixels;
+	}
+
+	//Graph...
+	std::vector< std::vector<int> > interframe_connectionId;
+	std::vector< std::vector<float> > interframe_connectionStrength;
+	interframe_connectionId.resize(tot_nr_pixels);
+	interframe_connectionStrength.resize(tot_nr_pixels);
+
+
+	int current_point           = 0;
+	float * priors             = new float[3*tot_nr_pixels];
+	bool * valids             = new bool[tot_nr_pixels];
+
+	std::vector<double> dvec;
+	std::vector<double> nvec;
+	DistanceWeightFunction2 * dfunc;
+	DistanceWeightFunction2 * nfunc;
+
+	double startTime = getTime();
+
+	std::vector< std::vector<superpoint> > framesp_test;
+	std::vector< std::vector<superpoint> > framesp;
+	for(unsigned int i = 0; i < frames.size(); i++){
+		framesp_test.push_back(frames[i]->getSuperPoints(Eigen::Matrix4d::Identity(),10,false));
+		framesp.push_back(frames[i]->getSuperPoints());
+	}
+	printf("frames init time: %5.5fs\n",getTime()-startTime);
+
+	startTime = getTime();
+	std::vector< std::vector<superpoint> > bgsp;
+	for(unsigned int i = 0; i < bgcf.size(); i++){
+		bgsp.push_back(bgcf[i]->getSuperPoints());
+	}
+	printf("bg init time:     %5.5fs\n",getTime()-startTime);
+
+	startTime = getTime();
+	//	printf("computing all residuals\n");
+	for(unsigned int i = 0; i < frames.size(); i++){
+		std::vector<superpoint> & framesp1_test = framesp_test[i];
+		std::vector<superpoint> & framesp1		= framesp[i];
+		for(unsigned int j = 0; j < frames.size(); j++){
+			if(i == j){continue;}
+			Eigen::Matrix4d p = poses[i].inverse() * poses[j];
+			std::vector<superpoint> & framesp2 = framesp[j];
+			getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp1_test,framesp1, 0, 0, frames[j],framesp2,offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false);
+		}
+	}
+
+
+	//	printf("training ppr\n");
+	double dstdval = 0;
+	for(unsigned int i = 0; i < dvec.size(); i++){dstdval += dvec[i]*dvec[i];}
+	dstdval = sqrt(dstdval/double(dvec.size()-1));
+
+	DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2();
+	dfunc = dfuncTMP;
+	dfuncTMP->refine_mean			= true;
+	dfuncTMP->zeromean				= false;
+	dfuncTMP->startreg				= 0.0;
+	//dfuncTMP->startreg				= 0.001;
+	//dfuncTMP->startreg				= 0.0000001;
+	dfuncTMP->debugg_print			= false;
+	dfuncTMP->bidir					= true;
+	//dfuncTMP->bidir					= false;
+	//dfuncTMP->maxp					= 0.9;
+	dfuncTMP->maxp					= 0.9;
+	dfuncTMP->maxd					= 0.5;
+	dfuncTMP->histogram_size		= 1000;
+	dfuncTMP->fixed_histogram_size	= false;//true;
+	dfuncTMP->max_under_mean		= false;
+	dfuncTMP->startmaxd				= dfuncTMP->maxd;
+	dfuncTMP->starthistogram_size	= dfuncTMP->histogram_size;
+	dfuncTMP->blurval				= 0.5;
+	dfuncTMP->stdval2				= dstdval;
+	dfuncTMP->maxnoise				= dstdval;
+	dfuncTMP->compute_infront		= true;
+	dfuncTMP->reset();
+	dfunc->computeModel(dvec);
+	double dthreshold = dfunc->getNoise();
+
+	GeneralizedGaussianDistribution * ggdnfunc	= new GeneralizedGaussianDistribution(true,true);
+	ggdnfunc->nr_refineiters					= 4;
+	DistanceWeightFunction2PPR3 * nfuncTMP		= new DistanceWeightFunction2PPR3(ggdnfunc);
+	nfunc = nfuncTMP;
+	nfuncTMP->startreg				= 0.00;
+	nfuncTMP->debugg_print			= false;
+	nfuncTMP->bidir					= false;
+	nfuncTMP->zeromean				= true;
+	nfuncTMP->maxp					= 0.999;
+	nfuncTMP->maxd					= 1.0;
+	nfuncTMP->histogram_size		= 1000;
+	nfuncTMP->fixed_histogram_size	= true;
+	nfuncTMP->startmaxd				= nfuncTMP->maxd;
+	nfuncTMP->starthistogram_size	= nfuncTMP->histogram_size;
+	nfuncTMP->blurval				= 0.5;
+	nfuncTMP->stdval2				= 1;
+	nfuncTMP->maxnoise				= 1;
+	nfuncTMP->ggd					= true;
+	nfuncTMP->reset();
+	nfunc->computeModel(nvec);
+
+	printf("training time:     %5.5fs\n",getTime()-startTime);
+
+	long frameConnections = 0;
+	std::vector< std::vector< std::vector<float> > > pixel_weights;
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud  (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+	double total_priortime = 0;
+	double total_connectiontime = 0;
+	double total_alloctime = 0;
+	double total_dealloctime = 0;
+	double total_Dynw = 0;
+
+	for(unsigned int i = 0; i < frames.size(); i++){
+		printf("currently workin on frame %i\n",i);
+		int offset = offsets[i];
+		RGBDFrame * frame = frames[i];
+		startTime = getTime();
+		std::vector< std::vector<float> > probs = getImageProbs(frame,9);
+		total_connectiontime += getTime()-startTime;
+		pixel_weights.push_back(probs);
+		unsigned short * depthdata	= (unsigned short	*)(frame->depth.data);
+
+		Camera * camera				= frame->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
+		const float idepth			= camera->idepth_scale;
+		const float cx				= camera->cx;
+		const float cy				= camera->cy;
+		const float ifx				= 1.0/camera->fx;
+		const float ify				= 1.0/camera->fy;
+
+		startTime = getTime();
+		unsigned int nr_pixels = width * height;
+		double * current_overlaps	= new double[nr_pixels];
+		double * current_occlusions		= new double[nr_pixels];
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			current_overlaps[j] = 0;
+			current_occlusions[j] = 0;
+		}
+
+		double * bg_overlaps	= new double[nr_pixels];
+		double * bg_occlusions		= new double[nr_pixels];
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			bg_overlaps[j] = 0;
+			bg_occlusions[j] = 0;
+		}
+		total_alloctime += getTime()-startTime;
+
+		startTime = getTime();
+		for(unsigned int j = 0; j < frames.size(); j++){
+			if(i == j){continue;}
+			Eigen::Matrix4d p = poses[i].inverse() * poses[j];
+			getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], current_overlaps, current_occlusions, frames[j],framesp[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false);
+		}
+
+		for(unsigned int j = 0; j < bgcf.size(); j++){
+			Eigen::Matrix4d p = poses[i].inverse() * bgcp[j];
+			getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], bg_overlaps, bg_occlusions, bgcf[j],bgsp[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false);
+		}
+		total_Dynw += getTime()-startTime;
+
+
+		startTime = getTime();
+		unsigned char * detdata = (unsigned char*)(frame->det_dilate.data);
+		float minprob = 0.01;
+		for(unsigned int h = 0; h < height;h++){
+			for(unsigned int w = 0; w < width;w++){
+				int ind = h*width+w;
+
+				valids[offset+ind] = detdata[ind] == 0;
+
+				float p_moving  = current_occlusions[ind];
+				float p_dynamic  =     std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0);
+				p_dynamic       += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0);
+				p_moving        += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0);
+
+				float p_static  = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0);
+
+				float leftover = 1-p_moving-p_dynamic-p_static;
+				float notMoving = current_overlaps[ind];
+
+				float bias = 0.0001;
+
+				float p_moving_leftover   = -bias+(1-notMoving)*leftover/4.0;
+				float p_dynamic_leftover  = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0;
+				float p_static_leftover   = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0;
+
+				float p_moving_tot	= (1.0-4.0*minprob)*(p_moving+p_moving_leftover)	+ minprob;
+				float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover)	+ minprob;
+				float p_static_tot	= (1.0-4.0*minprob)*(p_static+p_static_leftover)	+ 2.0*minprob;
+
+				priors[3*(offset+ind)+0]       = p_moving_tot;
+				priors[3*(offset+ind)+1]       = p_dynamic_tot;
+				priors[3*(offset+ind)+2]       = p_static_tot;
+
+				if(probs[0][ind] > 0.00000001){frameConnections++;}
+				if(probs[1][ind] > 0.00000001){frameConnections++;}
+
+				current_point++;
+			}
+		}
+
+		Eigen::Matrix4d p = poses[i];
+		float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+		float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+		float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+		for(unsigned int h = 0; h < height;h++){
+			for(unsigned int w = 0; w < width;w++){
+				int ind = h*width+w;
+				float z = idepth*float(depthdata[ind]);
+				float x = (float(w) - cx) * z * ifx;
+				float y = (float(h) - cy) * z * ify;
+				pcl::PointXYZRGBNormal point;
+				point.x = m00*x + m01*y + m02*z + m03;
+				point.y = m10*x + m11*y + m12*z + m13;
+				point.z = m20*x + m21*y + m22*z + m23;
+				point.r = priors[3*(offset+ind)+0]*255.0;
+				point.g = priors[3*(offset+ind)+1]*255.0;
+				point.b = priors[3*(offset+ind)+2]*255.0;
+				cloud->points.push_back(point);
+			}
+		}
+		total_priortime += getTime()-startTime;
+
+		startTime = getTime();
+		delete[] current_occlusions;
+		delete[] current_overlaps;
+		delete[] bg_occlusions;
+		delete[] bg_overlaps;
+		total_dealloctime += getTime()-startTime;
+	}
+
+	delete dfuncTMP;
+	delete nfuncTMP;
+
+	printf("total_priortime        = %5.5fs\n",		total_priortime);
+	printf("total_connectiontime   = %5.5fs\n",		total_connectiontime);
+	printf("total_alloctime        = %5.5fs\n",		total_alloctime);
+	printf("total_dealloctime      = %5.5fs\n",		total_dealloctime);
+	printf("total_Dynw             = %5.5fs\n",		total_Dynw);
+
+	long interframeConnections = 0;
+	for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+		for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+			interframeConnections++;
+		}
+	}
+
+	double start_inf = getTime();
+	gc::Graph<double,double,double> * g = new gc::Graph<double,double,double>(current_point,frameConnections+interframeConnections);
+	for(unsigned long i = 0; i < current_point;i++){
+		g -> add_node();
+		double p_fg = priors[3*i+0]+priors[3*i+1];
+		double p_bg = priors[3*i+2];
+		double norm = p_fg + p_bg;
+		p_fg /= norm;
+		p_bg /= norm;
+		if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){
+			g -> add_tweights( i, 0, 0 );
+			continue;
+		}
+
+		double weightFG = -log(p_fg);
+		double weightBG = -log(p_bg);
+		g -> add_tweights( i, weightFG, weightBG );
+	}
+
+	double maxprob_same = 0.99999999999;
+	for(unsigned int i = 0; i < frames.size(); i++){
+		int offset = offsets[i];
+		Camera * camera				= frames[i]->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
+		std::vector< std::vector<float> > & probs = pixel_weights[i];
+		for(unsigned int w = 0; w < width;w++){
+			for(unsigned int h = 0; h < height;h++){
+				int ind = h*width+w;
+				if(w > 0 && probs[0][ind] > 0.00000001){
+					int other = ind-1;
+					double p_same = std::min(double(probs[0][ind]),maxprob_same);
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					g -> add_edge( ind+offset, other+offset, weight, weight );
+				}
+
+				if(h > 0 && probs[1][ind] > 0.00000001){
+					int other = ind-width;
+					double p_same = std::min(double(probs[1][ind]),maxprob_same);
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					g -> add_edge( ind+offset, other+offset, weight, weight );
+				}
+			}
+		}
+	}
+	printf("run inference\n");
+
+	std::vector<std::vector<unsigned long> > interframe_connectionId_added;
+	interframe_connectionId_added.resize(interframe_connectionId.size());
+
+	std::vector<std::vector<double> > interframe_connectionStrength_added;
+	interframe_connectionStrength_added.resize(interframe_connectionStrength.size());
+
+	double tot_inf = 0;
+	for(unsigned int it = 0; it < 10; it++){
+		double start_inf1 = getTime();
+
+		g -> maxflow();
+
+		double diffs = 0;
+		for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+			for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+				double weight = interframe_connectionStrength[i][j];
+				unsigned long other = interframe_connectionId[i][j];
+				if(weight > 1 && g->what_segment(i) != g->what_segment(other)){
+					diffs++;
+				}
+			}
+		}
+
+		if(diffs < 1000){break;}
+
+		double adds = 100000;
+		double prob = std::min(adds / diffs,1.0);
+		printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob);
+
+		double trueadds = 0;
+
+		for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+			for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+				double weight = interframe_connectionStrength[i][j];
+				unsigned long other = interframe_connectionId[i][j];
+				if(weight > 0.1 && g->what_segment(i) != g->what_segment(other)){
+					if(rand() <= prob*RAND_MAX){
+						trueadds++;
+						g -> add_edge( i, other, weight, weight );
+
+						interframe_connectionId_added[i].push_back(other);
+						interframe_connectionStrength_added[i].push_back(weight);
+
+						interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back();
+						interframe_connectionStrength[i].pop_back();
+
+						interframe_connectionId[i][j] = interframe_connectionId[i].back();
+						interframe_connectionId[i].pop_back();
+						j--;
+					}
+				}
+			}
+		}
+		printf("trueadds: %f\n",trueadds);
+
+		tot_inf += getTime()-start_inf1;
+		printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf);
+
+		if(tot_inf > 90){break;}
+	}
+
+	for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+		for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){
+			interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]);
+			interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]);
+		}
+	}
+
+	int dynamic_label = 1;
+	std::vector<unsigned char> labels;
+	std::vector<int> dyn_ind;
+	std::vector<int> stat_ind;
+	labels.resize(current_point);
+	stat_ind.resize(current_point);
+	long nr_dynamic = 0;
+	for(unsigned long i = 0; i < current_point;i++){
+		labels[i] = g->what_segment(i);
+		nr_dynamic += labels[i]==dynamic_label;
+		if(labels[i]==dynamic_label){
+			stat_ind[i] = dyn_ind.size();
+			dyn_ind.push_back(i);
+		}
+	}
+	delete g;
+
+	double end_inf = getTime();
+	printf("static inference time: %10.10fs\n",end_inf-start_inf);
+
+	long dynamic_frameConnections = 0;
+
+	for(unsigned long i = 0; i < frames.size(); i++){
+		int offset = offsets[i];
+		Camera * camera				= frames[i]->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
+		std::vector< std::vector<float> > & probs = pixel_weights[i];
+		for(unsigned int w = 0; w < width;w++){
+			for(unsigned int h = 0; h < height;h++){
+				int ind = h*width+w;
+				if(labels[ind+offset] == dynamic_label){
+					if(w > 0 && probs[0][ind] > 0.00000001){
+						int other = ind-1;
+						if(labels[ind+offset] == labels[other+offset]){dynamic_frameConnections++;}
+					}
+
+					if(h > 0 && probs[1][ind] > 0.00000001){
+						int other = ind-width;
+						if(labels[ind+offset] == labels[other+offset]){dynamic_frameConnections++;}
+					}
+				}
+			}
+		}
+	}
+
+	long dynamic_interframeConnections = 0;
+	for(unsigned long i = 0; i < current_point;i++){
+		if(labels[i] == dynamic_label){
+			for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+				if(labels[interframe_connectionId[i][j]] == labels[i]){dynamic_interframeConnections++;}
+			}
+		}
+	}
+
+	start_inf = getTime();
+	gc::Graph<double,double,double> * dynamic_g = new gc::Graph<double,double,double>(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections);
+	for(unsigned long i = 0; i < dyn_ind.size();i++){
+		dynamic_g -> add_node();
+		double p_fg = priors[3*dyn_ind[i]+0];
+		double p_bg = priors[3*dyn_ind[i]+1];
+		double norm = p_fg+p_bg;
+		p_fg /= norm;
+		p_bg /= norm;
+		if(norm <= 0){
+			dynamic_g -> add_tweights( i, 0, 0 );
+			continue;
+		}
+
+		double weightFG = -log(p_fg);
+		double weightBG = -log(p_bg);
+		dynamic_g -> add_tweights( i, weightFG, weightBG );
+	}
+
+	for(unsigned long i = 0; i < frames.size(); i++){
+		int offset = offsets[i];
+		Camera * camera				= frames[i]->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
+		std::vector< std::vector<float> > & probs = pixel_weights[i];
+		for(unsigned int w = 0; w < width;w++){
+			for(unsigned int h = 0; h < height;h++){
+				int ind = h*width+w;
+				if(labels[ind+offset] == dynamic_label){
+					if(w > 0 && probs[0][ind] > 0.00000001){
+						int other = ind-1;
+						if(labels[ind+offset] == labels[other+offset]){
+							//dynamic_frameConnections++;
+							double p_same = std::min(double(probs[0][ind]),maxprob_same);
+							double not_p_same = 1-p_same;
+							double weight = -log(not_p_same);
+							dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight );
+						}
+					}
+
+					if(h > 0 && probs[1][ind] > 0.00000001){
+						int other = ind-width;
+						if(labels[ind+offset] == labels[other+offset]){
+							//dynamic_frameConnections++;
+							double p_same = std::min(double(probs[0][ind]),maxprob_same);
+							double not_p_same = 1-p_same;
+							double weight = -log(not_p_same);
+							dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight );
+						}
+					}
+				}
+			}
+		}
+	}
+
+	for(unsigned long i = 0; i < current_point;i++){
+		if(labels[i] == dynamic_label){
+			for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+				if(labels[interframe_connectionId[i][j]] == labels[i]){
+					double weight = interframe_connectionStrength[i][j];
+					dynamic_g -> add_edge( stat_ind[i], stat_ind[ interframe_connectionId[i][j] ], weight, weight );
+				}
+			}
+		}
+	}
+	dynamic_g -> maxflow();
+
+
+	tot_inf = 0;
+	for(unsigned int it = 0; false && it < 100; it++){
+		double start_inf1 = getTime();
+		dynamic_g -> maxflow();
+
+		double diffs = 0;
+		for(unsigned long i = 0; i < current_point;i++){
+			if(labels[i] == dynamic_label){
+				for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+					if(labels[interframe_connectionId[i][j]] == labels[i]){
+						double weight = interframe_connectionStrength[i][j];
+
+						unsigned long i2 = stat_ind[i];
+						unsigned long j2 = stat_ind[ interframe_connectionId[i][j] ];
+						if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){
+							diffs++;
+						}
+	dfuncTMP->compute_infront		= true;
+					}
+				}
+			}
+		}
+
+
+		double adds = 100000;
+		double prob = std::min(adds / diffs,1.0);
+		printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob);
+
+		if(diffs < 1000){break;}
+
+
+		double trueadds = 0;
+
+		for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+			if(labels[i] == dynamic_label){
+				for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){
+					if(labels[interframe_connectionId[i][j]] == labels[i]){
+
+						double weight = interframe_connectionStrength[i][j];
+						unsigned long other = interframe_connectionId[i][j];
+
+						unsigned long i2 = stat_ind[i];
+						unsigned long j2 = stat_ind[ interframe_connectionId[i][j] ];
+						if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){
+							if(rand() <= prob*RAND_MAX){
+								trueadds++;
+								g -> add_edge( i2, j2, weight, weight );
+
+								interframe_connectionId_added[i].push_back(other);
+								interframe_connectionStrength_added[i].push_back(weight);
+
+								interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back();
+								interframe_connectionStrength[i].pop_back();
+
+								interframe_connectionId[i][j] = interframe_connectionId[i].back();
+								interframe_connectionId[i].pop_back();
+								j--;
+							}
+						}
+					}
+				}
+			}
+		}
+		printf("trueadds: %f\n",trueadds);
+
+		tot_inf += getTime()-start_inf1;
+		printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf);
+		if(tot_inf > 90){break;}
+	}
+
+	end_inf = getTime();
+	printf("dynamic inference time: %10.10fs\n",end_inf-start_inf);
+
+	long nr_moving = 0;
+	for(unsigned long i = 0; i < dyn_ind.size();i++){
+		int res = dynamic_g->what_segment(i);
+		if(res == 1){
+			labels[dyn_ind[i]] = 2;
+			nr_moving++;
+		}
+	}
+	delete dynamic_g;
+
+	for(unsigned int i = 0; i < interframe_connectionId.size();i++){
+		for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){
+			interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]);
+			interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]);
+		}
+	}
+
+	const unsigned int nr_frames		= frames.size();
+	const unsigned int width			= frames[0]->camera->width;
+	const unsigned int height			= frames[0]->camera->height;
+	const unsigned int pixels_per_image	= width*height;
+	const unsigned int nr_pixels		= nr_frames*pixels_per_image;
+
+	double probthresh = 0.5;
+	double str_probthresh = -log(probthresh);
+	unsigned int number_of_dynamics = 0;
+	unsigned int nr_obj_dyn = 0;
+	unsigned int nr_obj_mov = 0;
+	std::vector<unsigned int> objectlabel;
+	std::vector<int> labelID;
+	labelID.push_back(0);
+	objectlabel.resize(nr_pixels);
+	for(unsigned long i = 0; i < nr_pixels; i++){objectlabel[i] = 0;}
+	for(unsigned long ind = 0; ind < nr_pixels; ind++){
+		if(valids[ind] && objectlabel[ind] == 0 && labels[ind] != 0){
+			unsigned int current_label = labels[ind];
+			number_of_dynamics++;
+			objectlabel[ind] = number_of_dynamics;
+			unsigned long todocounter = 0;
+			std::vector< unsigned long > todo;
+			todo.push_back(ind);
+			double score = 0;
+			double totsum = 0;
+			while(todocounter < todo.size()){
+				unsigned long cind = todo[todocounter++];
+				unsigned long frameind = cind / pixels_per_image;
+
+
+				unsigned long iind = cind % pixels_per_image;
+				unsigned long w = iind % width;
+				unsigned long h = iind / width;
+
+				double p0 = priors[3*cind+0];
+				double p1 = priors[3*cind+1];
+				double p2 = priors[3*cind+2];
+
+				if(valids[cind]){
+					double s = 0;
+					if(p0 > p2){s += p1 - p0;}
+					else{       s += p1 - p2;}
+					score += s;
+					totsum++;
+				}
+
+				float * dedata = (float*)(frames[frameind]->de.data);
+
+				int dir;
+				dir = -1;
+				if( w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){
+					objectlabel[cind+dir] = number_of_dynamics;
+					todo.push_back(cind+dir);
+				}
+
+				dir = 1;
+				if( w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){
+					objectlabel[cind+dir] = number_of_dynamics;
+					todo.push_back(cind+dir);
+				}
+
+				dir = -width;
+				if( h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){
+					objectlabel[cind+dir] = number_of_dynamics;
+					todo.push_back(cind+dir);
+				}
+
+				dir = width;
+				if( h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){
+					objectlabel[cind+dir] = number_of_dynamics;
+					todo.push_back(cind+dir);
+				}
+
+				for(unsigned long j = 0; j < interframe_connectionId[cind].size();j++){
+					unsigned long other = interframe_connectionId[cind][j];
+					if(interframe_connectionStrength[cind][j] > str_probthresh && objectlabel[other] == 0 && labels[other] == current_label){
+						objectlabel[other] = number_of_dynamics;
+						todo.push_back(other);
+
+						double d = sqrt(pow(cloud->points[cind].x-cloud->points[other].x,2)+pow(cloud->points[cind].y-cloud->points[other].y,2)+pow(cloud->points[cind].z-cloud->points[other].z,2));
+						if(d > 0.15){
+							printf("d: %f -> %f\n",d,exp(-interframe_connectionStrength[cind][j]));
+						}
+					}
+				}
+			}
+
+			printf("score: %f\n",score);
+
+			if(debugg && todo.size() > 10000){
+
+
+				printf("3-class\n");
+				for(unsigned int i = 0; i < current_point; i++){
+					cloud->points[i].r = priors[3*i+0]*255.0;
+					cloud->points[i].g = priors[3*i+1]*255.0;
+					cloud->points[i].b = priors[3*i+2]*255.0;
+				}
+
+				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_sample (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+				cloud_sample->points.clear();
+				for(unsigned int i = 0; i < current_point; i++){
+					//if(rand() % 4 == 0){
+						cloud_sample->points.push_back(cloud->points[i]);
+					//}
+				}
+				viewer->removeAllPointClouds();
+				viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_sample), "cloud");
+				viewer->spin();
+
+				for(unsigned int i = 0; i < current_point; i++){
+					cloud_sample->points[i].r = 255;
+					cloud_sample->points[i].g = 0;
+					cloud_sample->points[i].b = 0;
+				}
+
+				for(unsigned int j = 0; j < todo.size(); j++){
+					unsigned int i = todo[j];
+					cloud_sample->points[i].r = 0;
+					cloud_sample->points[i].g = 255;
+					cloud_sample->points[i].b = 0;
+				}
+				viewer->removeAllPointClouds();
+				viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_sample), "cloud");
+				viewer->spin();
+			}
+
+			labelID.push_back(0);
+			//if(score < 200){continue;}
+
+			if(score < 0){continue;}
+			if(current_label == 1){
+				labelID.back() = ++nr_obj_dyn;
+				printf("Dynamic: %f -> %f\n",score,totsum);
+				sr.component_dynamic.push_back(todo);
+				sr.scores_dynamic.push_back(score);
+				sr.total_dynamic.push_back(totsum);
+			}
+			if(current_label == 2){
+				labelID.back() = --nr_obj_mov;
+				printf("Moving: %f -> %f\n",score,totsum);
+				sr.component_moving.push_back(todo);
+				sr.scores_moving.push_back(score);
+				sr.total_moving.push_back(totsum);
+			}
+		}
+	}
+	interframe_connectionId.clear();
+	interframe_connectionStrength.clear();
+
+	printf("connectedComponent: %5.5fs\n",getTime()-start_inf);
+
+	int current = 0;
+	for(unsigned long i = 0; i < frames.size(); i++){
+		Camera * camera				= frames[i]->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
+
+		cv::Mat m;
+		m.create(height,width,CV_8UC1);
+		unsigned char * mdata = (unsigned char*)m.data;
+
+		cv::Mat d;
+		d.create(height,width,CV_8UC1);
+		unsigned char * ddata = (unsigned char*)d.data;
+
+		cv::Mat d2;
+		d2.create(height,width,CV_8UC1);
+		unsigned char * ddata2 = (unsigned char*)d2.data;
+
+		for(int j = 0; j < width*height; j++){
+			mdata[j] = 0;
+			ddata[j] = 0;
+			ddata2[j] = labels[current];
+			unsigned int label = objectlabel[current];
+			int lid = labelID[label];
+			if(lid >  0){
+				ddata[j] = lid;
+			}else if(lid < 0){
+				mdata[j] = -lid;
+			}
+			current++;
+		}
+		movemask.push_back(m);
+		dynmask.push_back(d);
+	}
+
+	if(debugg){
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_sample (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+		printf("3-class\n");
+		for(unsigned int i = 0; i < current_point; i++){
+			cloud->points[i].r = priors[3*i+0]*255.0;
+			cloud->points[i].g = priors[3*i+1]*255.0;
+			cloud->points[i].b = priors[3*i+2]*255.0;
+		}
+
+		cloud_sample->points.clear();
+		for(unsigned int i = 0; i < current_point; i++){
+			if(rand() % 4 == 0){
+				cloud_sample->points.push_back(cloud->points[i]);
+			}
+		}
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_sample), "cloud");
+		viewer->spin();
+
+
+		cloud_sample->points.clear();
+		for(unsigned int i = 0; i < current_point; i++){
+			if(rand() % 4 == 0 && labels[i] == 0){
+				cloud_sample->points.push_back(cloud->points[i]);
+				cloud_sample->points.back().r = 0;
+				cloud_sample->points.back().g = 0;
+				cloud_sample->points.back().b = 255;
+			}
+		}
+
+		for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){
+			int randr = rand()%256;
+			int randg = rand()%256;
+			int randb = rand()%256;
+			for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){
+				cloud_sample->points.push_back(cloud->points[sr.component_dynamic[c][i]]);
+				cloud_sample->points.back().r = randr;
+				cloud_sample->points.back().g = randg;
+				cloud_sample->points.back().b = randb;
+			}
+		}
+
+		for(unsigned int c = 0; c < sr.component_moving.size(); c++){
+			int randr = rand()%256;
+			int randg = rand()%256;
+			int randb = rand()%256;
+			for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){
+				cloud_sample->points.push_back(cloud->points[sr.component_moving[c][i]]);
+				cloud_sample->points.back().r = randr;
+				cloud_sample->points.back().g = randg;
+				cloud_sample->points.back().b = randb;
+			}
+		}
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud_sample), "cloud");
+		viewer->spin();
+	}
+
+	delete[] priors;
+	printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime);
+}
+*/
+
+vector<Mat> ModelUpdater::computeDynamicObject(vector<Matrix4d> bgcp, vector<RGBDFrame*> bgcf, vector<Mat> bgmm, vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<Mat> mm, vector<Matrix4d> poses, vector<RGBDFrame*> frames, vector<Mat> masks, bool debugg){
+	//
+	std::vector<cv::Mat> newmasks;
+	//for(double test = 0.99; test > 0.00000001; test *= 0.1)
+	{
+		//	double maxprob_same = 1-test;//0.99;
+
+		double maxprob_same = 0.999999;
+
+		double maxprob = 0.7;
+
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud  (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr bgcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+		int tot_nr_pixels = 0;
+		std::vector<int> offsets;
+
+		for(unsigned int i = 0; i < frames.size(); i++){
+			offsets.push_back(tot_nr_pixels);
+			unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height;
+			tot_nr_pixels += nr_pixels;
+		}
+
+		//Graph...
+		std::vector<double> prior;
+		std::vector< std::vector<int> > connectionId;
+		std::vector< std::vector<double> > connectionStrength;
+		std::vector< std::vector<int> > interframe_connectionId;
+		std::vector< std::vector<double> > interframe_connectionStrength;
+
+		prior.resize(tot_nr_pixels);
+		connectionId.resize(tot_nr_pixels);
+		connectionStrength.resize(tot_nr_pixels);
+		interframe_connectionId.resize(tot_nr_pixels);
+		interframe_connectionStrength.resize(tot_nr_pixels);
+
+
+
+		for(unsigned int i = 0; i < frames.size(); i++){
+			printf("frame: %i\n",i);
+			int offset = offsets[i];
+			RGBDFrame * frame = frames[i];
+			unsigned short * depthdata	= (unsigned short	*)(frame->depth.data);
+			unsigned char * rgbdata		= (unsigned char	*)(frame->rgb.data);
+			unsigned char * detdata		= (unsigned char	*)(frame->det_dilate.data);
+			Camera * camera				= frame->camera;
+			const unsigned int width	= camera->width;
+			const unsigned int height	= camera->height;
+
+
+			unsigned int nr_pixels = frame->camera->width * frame->camera->height;
+			double * overlaps	= new double[nr_pixels];
+			double * occlusions		= new double[nr_pixels];
+			for(unsigned int j = 0; j < nr_pixels; j++){
+				overlaps[j] = 0;
+				occlusions[j] = 0;
+			}
+
+			std::vector< std::vector<int> > interframe_connectionId_tmp;
+			std::vector< std::vector<double> > interframe_connectionStrength_tmp;
+
+			for(unsigned int j = 0; j < bgcp.size(); j++){
+				if(frame == bgcf[j]){continue;}
+				Eigen::Matrix4d p = poses[i].inverse() * bgcp[j];
+				getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false);
+			}
+
+			for(unsigned int j = 0; j < nr_pixels; j++){
+				occlusions[j] = 0;
+			}
+			printf("CHECK AGAINST BACKGROUND\n");
+			for(unsigned int j = 0; j < cp.size(); j++){
+				if(frame == cf[j]){continue;}
+				Eigen::Matrix4d p = poses[i].inverse() * cp[j];
+				getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false);
+			}
+			std::vector< std::vector<float> > probs = getImageProbs(frames[i],9);
+
+			std::vector<double> frame_prior;
+			std::vector< std::vector<int> > frame_connectionId;
+			std::vector< std::vector<double> > frame_connectionStrength;
+
+			frame_prior.resize(nr_pixels);
+			frame_connectionId.resize(nr_pixels);
+			frame_connectionStrength.resize(nr_pixels);
+
+
+			printf("starting partition\n");
+			double start_part = getTime();
+			unsigned int nr_data = width*height;
+			for(unsigned int j = 0; j < nr_data;j++){
+				if(depthdata[j] == 0){
+					frame_prior[j] = -1;
+					prior[offset+j] = -1;
+					continue;
+				}
+
+				double p_fg = 0.499999999;
+
+				if(occlusions[j] >= 1){	p_fg = maxprob;}
+				else if(overlaps[j] >= 1){	p_fg = 0.4;}
+
+				p_fg = std::max(1-maxprob,std::min(maxprob,p_fg));
+
+				frame_prior[j] = p_fg;
+				prior[offset+j] = p_fg;
+			}
+
+
+			for(unsigned int w = 0; w < width;w++){
+				for(unsigned int h = 0; h < height;h++){
+					int ind = h*width+w;
+					if(w > 0 && w < width-1){
+						int other = ind-1;
+						double p_same = std::min(double(probs[0][ind]),maxprob_same);
+						double not_p_same = 1-p_same;
+						double weight = -log(not_p_same);
+						if(!std::isnan(weight) && weight > 0){
+							frame_connectionId[ind].push_back(other);
+							frame_connectionStrength[ind].push_back(weight);
+
+							connectionId[ind+offset].push_back(other+offset);
+							connectionStrength[ind+offset].push_back(weight);
+						}
+					}
+
+					if(h > 0 && h < height-1){
+						int other = ind-width;
+						double p_same = std::min(double(probs[1][ind]),maxprob_same);
+						double not_p_same = 1-p_same;
+						double weight = -log(not_p_same);
+
+						if(!std::isnan(weight) && weight > 0){
+							frame_connectionId[ind].push_back(other);
+							frame_connectionStrength[ind].push_back(weight);
+
+							connectionId[ind+offset].push_back(other+offset);
+							connectionStrength[ind+offset].push_back(weight);
+						}
+					}
+
+					/*
+				if(w > 0 && w < width-1){
+					double ax = 0.5;
+					double bx = 0.5;
+					for(unsigned int p = 0; p < probs.size(); p+=2){
+						double pr = probs[p][ind];
+						ax *= pr;
+						bx *= 1.0-pr;
+					}
+					double px = ax/(ax+bx);
+
+					int other = ind-1;
+					//double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same));
+					//double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same));
+					//double p_same = std::min(probs[probs.size()-2][ind],maxprob_same);
+					//double p_same = std::max(1-maxprob_same,std::min(std::min(px,probs[probs.size()-2][ind]),maxprob_same));
+					//double p_same = std::min(std::min(px,probs[probs.size()-2][ind]),maxprob_same);
+					//double p_same = std::min(px,maxprob_same);
+
+//					double p_same = std::min(probs[probs.size()-2][ind],maxprob_same);
+//					if(detdata[ind] == 0){
+//						p_same = std::min(px,maxprob_same);
+//					}
+
+					float p_same = std::min(px,maxprob_same);
+
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					if(!std::isnan(weight) && weight > 0){
+						frame_connectionId[ind].push_back(other);
+						frame_connectionStrength[ind].push_back(weight);
+
+						connectionId[ind+offset].push_back(other+offset);
+						connectionStrength[ind+offset].push_back(weight);
+					}
+				}
+
+				if(h > 0 && h < height-1){
+
+					double ay = 0.5;
+					double by = 0.5;
+					for(unsigned int p = 1; p < probs.size(); p+=2){
+						double pr = probs[p][ind];
+						ay *= pr;
+						by *= 1.0-pr;
+					}
+					double py = ay/(ay+by);
+
+					int other = ind-width;
+					//double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same));
+
+					//double p_same = std::min(py,maxprob_same);
+					//double p_same = std::max(1-maxprob_same,std::min(std::min(py,probs[probs.size()-1][ind]),maxprob_same));
+					//double p_same = std::min(probs[probs.size()-1][ind],maxprob_same);
+
+					//double p_same = std::min(std::min(py,probs[probs.size()-1][ind]),maxprob_same);
+					double p_same = std::min(py,maxprob_same);
+
+					//double p_same = std::min(probs[probs.size()-1][ind],maxprob_same);
+					//if(detdata[ind] == 0){
+					//	p_same = std::min(py,maxprob_same);
+					//}
 
-double getTime(){
-	struct timeval start1;
-	gettimeofday(&start1, NULL);
-	return double(start1.tv_sec+(start1.tv_usec/1000000.0));
-}
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+
+					if(!std::isnan(weight) && weight > 0){
+						frame_connectionId[ind].push_back(other);
+						frame_connectionStrength[ind].push_back(weight);
+
+						connectionId[ind+offset].push_back(other+offset);
+						connectionStrength[ind+offset].push_back(weight);
+					}
+				}
+				*/
+				}
+			}
+
+
+
+			double end_part = getTime();
+			printf("part time: %10.10fs\n",end_part-start_part);
 
-float graph_cut(vector<Graph*>& graphs_out,vector<vector<int>>& second_graphinds, Graph& graph_in, std::vector<int> graph_inds){
-	using adjacency_iterator = boost::graph_traits<Graph>::adjacency_iterator;
-	typename boost::property_map<Graph, boost::vertex_index_t>::type vertex_id		= boost::get(boost::vertex_index, graph_in);
-	typename boost::property_map<Graph, boost::edge_weight_t>::type  edge_id		= boost::get(boost::edge_weight, graph_in);
-	typename boost::property_map<Graph, boost::vertex_name_t>::type  vertex_name	= boost::get(boost::vertex_name, graph_in);
+			//std::vector<int> labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength);
 
-	BOOST_AUTO(parities, boost::make_one_bit_color_map(boost::num_vertices(graph_in), boost::get(boost::vertex_index, graph_in)));
+			Eigen::Matrix4d p = poses[i];
+			float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+			float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+			float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
 
-	float w = boost::stoer_wagner_min_cut(graph_in, boost::get(boost::edge_weight, graph_in), boost::parity_map(parities));
 
-	unordered_map<VertexIndex, VertexIndex> mappings;
-	VertexIndex counters[2] = {0, 0};
+			const float idepth			= camera->idepth_scale;
+			const float cx				= camera->cx;
+			const float cy				= camera->cy;
+			const float ifx				= 1.0/camera->fx;
+			const float ify				= 1.0/camera->fy;
 
-	graphs_out.push_back(new Graph(1));
-	graphs_out.push_back(new Graph(1));
-	second_graphinds.push_back(vector<int>());
-	second_graphinds.push_back(vector<int>());
-	//std::cout << "One set of vertices consists of:" << std::endl;
-	bool flag;
-	Edge edge;
-	for (size_t i = 0; i < boost::num_vertices(graph_in); ++i) {
-		int first = boost::get(parities, i);
-		second_graphinds[first].push_back(graph_inds[i]);
-		// iterate adjacent edges
-		adjacency_iterator ai, ai_end;
-		for (tie(ai, ai_end) = boost::adjacent_vertices(i, graph_in);  ai != ai_end; ++ai) {
-			VertexIndex neighbor_index = boost::get(vertex_id, *ai);
-			int second = boost::get(parities, neighbor_index);
-			if (first == second && neighbor_index < i) {
-				tie(edge, flag) = boost::edge(i, neighbor_index, graph_in);
-				edge_weight_property weight = boost::get(edge_id, edge);
-				if (mappings.count(i) == 0) {
-					mappings[i] = counters[first]++;
+			for(unsigned int w = 0; w < width;w++){
+				for(unsigned int h = 0; h < height;h++){
+					int ind = h*width+w;
+					float z = idepth*float(depthdata[ind]);
+					if(z > 0){
+						float x = (float(w) - cx) * z * ifx;
+						float y = (float(h) - cy) * z * ify;
+
+						pcl::PointXYZRGBNormal point;
+						point.x = m00*x + m01*y + m02*z + m03;
+						point.y = m10*x + m11*y + m12*z + m13;
+						point.z = m20*x + m21*y + m22*z + m23;
+						point.b = rgbdata[3*ind+0];
+						point.g = rgbdata[3*ind+1];
+						point.r = rgbdata[3*ind+2];
+
+						cloud->points.push_back(point);
+					}
 				}
-				if (mappings.count(neighbor_index) == 0) {
-					mappings[neighbor_index] = counters[first]++;
+			}
+
+			bool debugg2 = debugg;
+			if(debugg2){
+				for(unsigned int w = 0; w < width;w++){
+					for(unsigned int h = 0; h < height;h++){
+						int ind = h*width+w;
+						float z = idepth*float(depthdata[ind]);
+						if(z > 0){
+							float x = (float(w) - cx) * z * ifx;
+							float y = (float(h) - cy) * z * ify;
+
+							float tx	= m00*x + m01*y + m02*z + m03;
+							float ty	= m10*x + m11*y + m12*z + m13;
+							float tz	= m20*x + m21*y + m22*z + m23;
+
+							pcl::PointXYZRGBNormal point;
+							point.x = tx;
+							point.y = ty;
+							point.z = tz;
+
+							point.r = 0;
+							point.g = 255;
+							point.b = 255;
+
+							if(overlaps[ind] >= 1){
+								point.r = 0;
+								point.g = 255;
+								point.b = 0;
+							}
+							if(occlusions[ind] >= 1){
+								point.r = 255;
+								point.g = 0;
+								point.b = 0;
+							}
+							//cloud->points[ind] = point;
+							cloud2->points.push_back(point);
+						}
+					}
 				}
-				tie(edge, flag) = boost::add_edge(mappings[neighbor_index], mappings[i], weight, *graphs_out[first]);
+			}
 
-				typename boost::property_map<Graph, boost::vertex_name_t>::type vertex_name_first = boost::get(boost::vertex_name, *graphs_out[first]);
-				boost::get(vertex_name_first, mappings[i]) = boost::get(vertex_name, i);
-				boost::get(vertex_name_first, mappings[neighbor_index]) = boost::get(vertex_name, *ai);
+			for(unsigned int j = 0; j < nr_pixels; j++){
+				occlusions[j] = 0;
 			}
+
+			for(unsigned int j = 0; j < frames.size(); j++){
+				if(frame == frames[j]){continue;}
+				Eigen::Matrix4d p = poses[i].inverse() * poses[j];
+				getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, frames[j],masks[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false);
+			}
+
+			//Time to compute external masks...
+			delete[] overlaps;
+			delete[] occlusions;
 		}
-	}
-	return w;
-}
 
-float recursive_split(std::vector<Graph*> * graphs_out,std::vector<std::vector<int>> * graphinds_out, Graph * graph, std::vector<int> graph_inds){
-	if(boost::num_vertices(*graph) == 1){
-		graphs_out->push_back(graph);
-		graphinds_out->push_back(graph_inds);
-		return 0;
-	}
+		//std::vector<int> global_labels = doInference(prior,connectionId,connectionStrength);
 
-	vector<Graph*> second_graphs;
-	vector<vector<int>> second_graphinds;
-	float w = graph_cut(second_graphs,second_graphinds,*graph,graph_inds);
-	if(w <= 0){
-		delete graph;
-		return 2*w + recursive_split(graphs_out, graphinds_out,second_graphs.front(),second_graphinds.front()) + recursive_split(graphs_out, graphinds_out, second_graphs.back(),second_graphinds.back());
-	}else{
-		graphs_out->push_back(graph);
-		graphinds_out->push_back(graph_inds);
-		delete second_graphs.front();
-		delete second_graphs.back();
-		return 0;
-	}
-}
+		for(unsigned int i = 0; i < interframe_connectionId.size(); i++){
+			for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){
+				connectionId[i].push_back(interframe_connectionId[i][j]);
+				connectionStrength[i].push_back(interframe_connectionStrength[i][j]);
+			}
+		}
 
-std::vector<int> partition_graph(std::vector< std::vector< float > > & scores){
-	int nr_data = scores.size();
-	Graph* graph = new Graph(nr_data);
-	std::vector<int> graph_inds;
-	graph_inds.resize(nr_data);
+		std::vector<int> improvedglobal_labels = doInference(prior,connectionId,connectionStrength);
+
+		for(unsigned int f = 0; f < frames.size(); f++){
+			int offset = offsets[f];
+			RGBDFrame * frame = frames[f];
+			unsigned short * depthdata	= (unsigned short	*)(frame->depth.data);
+			//		unsigned char * rgbdata		= (unsigned char	*)(frame->rgb.data);
+
+			Camera * camera				= frame->camera;
+			const unsigned int width	= camera->width;
+			const unsigned int height	= camera->height;
+
+			unsigned int nr_pixels = frame->camera->width * frame->camera->height;
+
+			//		cv::Mat originalmask;
+			//		originalmask.create(height,width,CV_8UC1);
+			//		unsigned char * originaldata = (unsigned char *)(originalmask.data);
+
+			cv::Mat improvedmask;
+			improvedmask.create(height,width,CV_8UC1);
+			unsigned char * improveddata = (unsigned char *)(improvedmask.data);
+
+			//		cv::Mat diffmask;
+			//		diffmask.create(height,width,CV_8UC3);
+			//		unsigned char * diffdata = (unsigned char *)(diffmask.data);
+
+			for(unsigned int i = 0; i < nr_pixels;i++){
+				//internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph<double,double,double>::SOURCE));
+				//originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph<double,double,double>::SOURCE));
+				improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph<double,double,double>::SOURCE));
+				//			if(improveddata[i] < originaldata[i]){
+				//				diffdata[3*i+0] = 0;
+				//				diffdata[3*i+1] = 255;
+				//				diffdata[3*i+2] = 0;
+				//			}else if(improveddata[i] > originaldata[i]){
+				//				diffdata[3*i+0] = 0;
+				//				diffdata[3*i+1] = 0;
+				//				diffdata[3*i+2] = 255;
+				//			}else{
+				//				diffdata[3*i+0] = 0;
+				//				diffdata[3*i+1] = 0;
+				//				diffdata[3*i+2] = 0;
+				//			}
 
-	typename boost::property_map<Graph, boost::vertex_name_t>::type vertex_name = boost::get(boost::vertex_name, *graph);
 
-	float sum = 0;
-	for(int i = 0; i < nr_data; i++){
-		graph_inds[i] = i;
-		for(int j = i+1; j < nr_data; j++){
-			float weight = scores[i][j];
-			if(weight != 0){
-				sum += 2*weight;
-				edge_weight_property e = weight;
-				boost::add_edge(i, j, e, *graph);
 			}
+
+			//				frame->show(false);
+			//				//cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE );
+			//				//cv::imshow( "Original", originalmask );
+			//				cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE );
+			//				cv::imshow( "Improved", improvedmask );
+			//				//cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE );
+			//				//cv::imshow( "Difference", diffmask );
+			//				cv::waitKey(0);
+			newmasks.push_back(improvedmask);
 		}
-	}
 
-	std::vector<Graph*> * graphs_out = new std::vector<Graph*>();
-	std::vector<std::vector<int>> * graphinds_out = new std::vector<std::vector<int>>();
-	float best = sum-recursive_split(graphs_out,graphinds_out, graph,graph_inds );
 
-	std::vector<int> part;
-	part.resize(nr_data);
-	//int * part = new int[nr_data];
-	for(unsigned int i = 0; i < graphinds_out->size(); i++){
-		for(unsigned int j = 0; j < graphinds_out->at(i).size(); j++){
-			part[graphinds_out->at(i).at(j)] = i;
-		}
-	}
-	return part;
-}
+		if(debugg){
 
-std::vector<int> ModelUpdater::getPartition(std::vector< std::vector< float > > & scores, int dims, int nr_todo, double timelimit){
-	return partition_graph(scores);
-}
 
-ModelUpdater::ModelUpdater(){
-	occlusion_penalty = 10;
-    massreg_timeout = 60;
-    model = 0;
-}
+			for(unsigned int i = 0; i < bgcf.size(); i++){
+				RGBDFrame * frame = bgcf[i];
+				unsigned short * depthdata	= (unsigned short	*)(frame->depth.data);
+				unsigned char * rgbdata		= (unsigned char	*)(frame->rgb.data);
 
-ModelUpdater::ModelUpdater(Model * model_){	model = model_;}
-ModelUpdater::~ModelUpdater(){}
+				Camera * camera				= frame->camera;
+				const unsigned int width	= camera->width;
+				const unsigned int height	= camera->height;
 
-FusionResults ModelUpdater::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){return FusionResults();}
+				Eigen::Matrix4d p = bgcp[i];
+				float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+				float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+				float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
 
-void ModelUpdater::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanity){
-	for(unsigned int i = 0; i < model2->frames.size();i++){
-		model->frames.push_back(model2->frames[i]);
-		//model->masks.push_back(model2->masks[i]);
-		model->modelmasks.push_back(model2->modelmasks[i]);
-		model->relativeposes.push_back(guess*model2->relativeposes[i]);
-	}
-}
 
-UpdatedModels ModelUpdater::fuseData(FusionResults * f, Model * model1,Model * model2){return UpdatedModels();}
+				const float idepth			= camera->idepth_scale;
+				const float cx				= camera->cx;
+				const float cy				= camera->cy;
+				const float ifx				= 1.0/camera->fx;
+				const float ify				= 1.0/camera->fy;
 
-void ModelUpdater::refine(double reg,bool useFullMask){
-	//return ;
-	printf("void ModelUpdater::refine()\n");
+				for(unsigned int w = 0; w < width;w++){
+					for(unsigned int h = 0; h < height;h++){
+						int ind = h*width+w;
+						float z = idepth*float(depthdata[ind]);
+						if(z > 0){
+							float x = (float(w) - cx) * z * ifx;
+							float y = (float(h) - cy) * z * ify;
 
-	std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(model->relativeposes, model->frames,model->modelmasks,false);
-	std::vector<std::vector < float > > scores = getScores(ocs);
+							float tx	= m00*x + m01*y + m02*z + m03;
+							float ty	= m10*x + m11*y + m12*z + m13;
+							float tz	= m20*x + m21*y + m22*z + m23;
+
+							pcl::PointXYZRGBNormal point;
+							point.x = tx;
+							point.y = ty;
+							point.z = tz;
+
+							point.b = rgbdata[3*ind+0];
+							point.g = rgbdata[3*ind+1];
+							point.r = rgbdata[3*ind+2];
+
+							bgcloud->points.push_back(point);
+						}
+					}
+				}
+			}
+			/*
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud1, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud1), "scloud");
+		viewer->spin();
+
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud), "scloud");
+		viewer->spin();
+		*/
+			viewer->removeAllPointClouds();
+			viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud2, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud2), "scloud");
+			//viewer->addPointCloud<pcl::PointXYZRGBNormal> (bgcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(bgcloud), "bgcloud");
+			viewer->spin();
+
+			viewer->removeAllPointClouds();
 
-	double sumscore_bef = 0;
-    for(unsigned int i = 0; i < scores.size(); i++){
-        for(unsigned int j = 0; j < scores.size(); j++){
-			sumscore_bef += scores[i][j];
 		}
 	}
+	return newmasks;
+}
 
-	MassFusionResults mfr;
-	MassRegistrationPPR * massreg = new MassRegistrationPPR(reg);
-    massreg->timeout = massreg_timeout;
-	massreg->viewer = viewer;
-	massreg->visualizationLvl = 0;
-	massreg->maskstep = 4;//std::max(1,int(0.5+0.02*double(model->frames.size())));
-	massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size())));
+std::vector<cv::Mat> ModelUpdater::computeDynamicObject(reglib::Model * bg,  Eigen::Matrix4d bgpose, vector<Matrix4d> cp, vector<RGBDFrame*> cf, vector<cv::Mat> oldmasks){
+	std::vector<cv::Mat> newmasks;
+	for(unsigned int i = 0; i < cf.size(); i++){
+		unsigned short * depthdata		= (unsigned short	*)(cf[i]->depth.data);
 
-	printf("maskstep: %i nomaskstep: %i\n",massreg->maskstep,massreg->nomaskstep);
+		Camera * camera				= cf[i]->camera;
+		const unsigned int width	= camera->width;
+		const unsigned int height	= camera->height;
 
-	//massreg->type = PointToPoint;
-	if(useFullMask){
-//        massreg->visualizationLvl = 0;
-//		MassRegistrationPPRColor * massregC = new MassRegistrationPPRColor(reg);
-//		massregC->viewer = viewer;
-//		massregC->visualizationLvl = 1;
-//		massregC->steps = 8;
-//		massregC->stopval = 0.001;
-//		massregC->setData(model->frames,model->modelmasks);
-//		mfr = massregC->getTransforms(model->relativeposes);
-//		model->relativeposes = mfr.poses;
-//	exit(0);
 
-		massreg->setData(model->frames,model->modelmasks);
-		massreg->nomask = false;
-//		massreg->visualizationLvl = 0;
-		massreg->stopval = 0.001;
-		massreg->steps = 10;
+		unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height;
+		double * overlaps	= new double[nr_pixels];
+		double * occlusions		= new double[nr_pixels];
+		for(unsigned int j = 0; j < nr_pixels; j++){
+			overlaps[j] = 0;
+			occlusions[j] = 0;
+		}
 
-		mfr = massreg->getTransforms(model->relativeposes);
-		//model->relativeposes = mfr.poses;
-		//massreg->steps = 8;
-		//mfr = massreg->getTransforms(model->relativeposes);
-		//model->relativeposes = mfr.poses;
-//		exit(0);
 
-//		massreg->visualizationLvl = 2;
-//		massreg->stopval = 0.002;
-//        massreg->steps = 4;
-//		mfr = massreg->getTransforms(model->relativeposes);
-//		model->relativeposes = mfr.poses;
+		std::vector< std::vector<int> > interframe_connectionId_tmp;
+		std::vector< std::vector<double> > interframe_connectionStrength_tmp;
 
-	}else{
-printf("%s::%i\n",__FILE__,__LINE__);
-		exit(0);
+		//        for(unsigned int j = 0; j < bgcp.size(); j++){
+		//			if(frame == bgcf[j]){continue;}
+		//            Eigen::Matrix4d p = poses[i].inverse() * bgcp[j];
+		//			getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false);
+		//        }
 
-		massreg->nomask = true;
-		massreg->visualizationLvl = 3;
-		massreg->steps = 10;
-		massreg->stopval = 0.001;
-		//massreg->setData(model->frames,model->masks);
-		massreg->setData(model->frames,model->modelmasks);
-		mfr = massreg->getTransforms(model->relativeposes);
-		//model->relativeposes = mfr.poses;
+		for(unsigned int j = 0; j < cf.size(); j++){
+			if(i == j){continue;}
+			Eigen::Matrix4d p = cp[i].inverse() * cp[j];
+			getDynamicWeights(false,p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp);
+		}
+		std::vector< std::vector<float> > probs = getImageProbs(cf[i],5);
+
+		printf("starting partition\n");
+		double start_part = getTime();
+		unsigned int nr_data = width*height;
+		gc::Graph<double,double,double> *g = new gc::Graph<double,double,double>( nr_data, width*(height-1) + (width-1)*height);
+
+		double maxprob = 0.7;
+		for(unsigned int j = 0; j < nr_data;j++){
+			g -> add_node();
+			if(depthdata[j] == 0){
+				g -> add_tweights(j,0,0);
+				continue;
+			}
 
-	}
+			double p_fg = 0.499;
 
-	std::vector<std::vector < OcclusionScore > > ocs2 = getOcclusionScores(mfr.poses, model->frames,model->modelmasks,false);
-	std::vector<std::vector < float > > scores2 = getScores(ocs2);
+			if(occlusions[j] >= 1){	p_fg = maxprob;}
+			else if(overlaps[j] >= 1){	p_fg = 0.4;}
 
-	double sumscore_aft = 0;
-    for(unsigned int i = 0; i < scores2.size(); i++){
-        for(unsigned int j = 0; j < scores2.size(); j++){
-			sumscore_aft += scores2[i][j];
+			p_fg = std::max(1-maxprob,std::min(maxprob,p_fg));
+
+			double p_bg = 1-p_fg;
+			double weightFG = -log(p_fg);
+			double weightBG = -log(p_bg);
+			g -> add_tweights( j, weightFG, weightBG );
 		}
-	}
-	if(sumscore_aft >= sumscore_bef){
-		model->relativeposes = mfr.poses;
-		model->scores = scores2;
-		model->total_scores = sumscore_aft;
-	}
-}
 
-void ModelUpdater::show(bool stop){
-	//printf("void ModelUpdater::show()\n");
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = model->getPCLcloud(1,false);
-	viewer->removeAllPointClouds();
-	viewer->addPointCloud<pcl::PointXYZRGB>(cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(cloud), "cloud");
-	if(stop){	viewer->spin();}
-	else{		viewer->spinOnce();}
-	viewer->removeAllPointClouds();
-}
+		float maxprob_same = 0.999999999;
+		for(unsigned int w = 0; w < width;w++){
+			for(unsigned int h = 0; h < height;h++){
+				int ind = h*width+w;
+
+				double ax = 0.5;
+				double bx = 0.5;
+				for(unsigned int p = 0; p < probs.size(); p+=2){
+					double pr = probs[p][ind];
+					ax *= pr;
+					bx *= 1.0-pr;
+				}
+				float px = ax/(ax+bx);
+
+				double ay = 0.5;
+				double by = 0.5;
+				for(unsigned int p = 1; p < probs.size(); p+=2){
+					double pr = probs[p][ind];
+					ay *= pr;
+					by *= 1.0-pr;
+				}
+				float py = ay/(ay+by);
+
+				if(w > 0){
+					int other = ind-1;
+					double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same));
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					g -> add_edge( ind, other, weight, weight );
+				}
 
-//Backproject and prune occlusions
-void ModelUpdater::pruneOcclusions(){}
+				if(h > 0){
+					int other = ind-width;
+					double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same));
+					double not_p_same = 1-p_same;
+					double weight = -log(not_p_same);
+					g -> add_edge( ind, other, weight, weight );
+				}
+			}
+		}
 
-void ModelUpdater::computeResiduals(std::vector<float> & residuals, std::vector<float> & weights,
-                                    RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask,
-                                    RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask,
-                                    Eigen::Matrix4d p, bool debugg){
-
-    unsigned char  * src_maskdata		= (unsigned char	*)(src_modelmask->mask.data);
-    unsigned char  * src_rgbdata		= (unsigned char	*)(src->rgb.data);
-    unsigned short * src_depthdata		= (unsigned short	*)(src->depth.data);
-    float		   * src_normalsdata	= (float			*)(src->normals.data);
-
-    unsigned char  * dst_maskdata		= (unsigned char	*)(dst_modelmask->mask.data);
-    unsigned char  * dst_rgbdata		= (unsigned char	*)(dst->rgb.data);
-    unsigned short * dst_depthdata		= (unsigned short	*)(dst->depth.data);
-    float		   * dst_normalsdata	= (float			*)(dst->normals.data);
-
-    float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
-    float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
-    float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
-
-    Camera * src_camera				= src->camera;
-    const unsigned int src_width	= src_camera->width;
-    const unsigned int src_height	= src_camera->height;
-    const float src_idepth			= src_camera->idepth_scale;
-    const float src_cx				= src_camera->cx;
-    const float src_cy				= src_camera->cy;
-    const float src_ifx				= 1.0/src_camera->fx;
-    const float src_ify				= 1.0/src_camera->fy;
-
-    Camera * dst_camera				= dst->camera;
-    const unsigned int dst_width	= dst_camera->width;
-    const unsigned int dst_height	= dst_camera->height;
-    const float dst_idepth			= dst_camera->idepth_scale;
-    const float dst_cx				= dst_camera->cx;
-    const float dst_cy				= dst_camera->cy;
-    const float dst_fx				= dst_camera->fx;
-    const float dst_fy				= dst_camera->fy;
-
-    const unsigned int dst_width2	= dst_camera->width  - 2;
-    const unsigned int dst_height2	= dst_camera->height - 2;
-
-    std::vector<int> & testw = src_modelmask->testw;
-    std::vector<int> & testh = src_modelmask->testh;
-
-    unsigned int test_nrdata = testw.size();
-    for(unsigned int ind = 0; ind < test_nrdata;ind++){
-        unsigned int src_w = testw[ind];
-        unsigned int src_h = testh[ind];
-
-        int src_ind = src_h*src_width+src_w;
-        if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){
-            float z = src_idepth*float(src_depthdata[src_ind]);
-            float nx = src_normalsdata[3*src_ind+0];
-
-            if(z > 0 && nx != 2){
-                float ny = src_normalsdata[3*src_ind+1];
-                float nz = src_normalsdata[3*src_ind+2];
-
-                float x = (float(src_w) - src_cx) * z * src_ifx;
-                float y = (float(src_h) - src_cy) * z * src_ify;
-
-                float tx	= m00*x + m01*y + m02*z + m03;
-                float ty	= m10*x + m11*y + m12*z + m13;
-                float tz	= m20*x + m21*y + m22*z + m23;
-                //float tnx	= m00*nx + m01*ny + m02*nz;
-                //float tny	= m10*nx + m11*ny + m12*nz;
-                float tnz	= m20*nx + m21*ny + m22*nz;
-
-                float itz	= 1.0/tz;
-                float dst_w	= dst_fx*tx*itz + dst_cx;
-                float dst_h	= dst_fy*ty*itz + dst_cy;
-
-                if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
-                    unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
-
-                    float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
-                    if(dst_z > 0){
-                        float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0
-                        residuals.push_back(diff_z);
-                        weights.push_back(1.0);
-                    }
-                }
-            }
-        }
-    }
-}
+		int flow = g -> maxflow();
+		printf("flow: %i\n",flow);
 
-void ModelUpdater::recomputeScores(){
-//	printf("recomputeScores\n");
-	std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(model->relativeposes,model->frames,model->modelmasks,false);
-	model->scores = getScores(ocs);
 
-	model->total_scores = 0;
-    for(unsigned int i = 0; i < model->scores.size(); i++){
-        for(unsigned int j = 0; j < model->scores.size(); j++){
-			model->total_scores += model->scores[i][j];
-		}
+		double end_part = getTime();
+		printf("part time: %10.10fs\n",end_part-start_part);
+
+		cv::Mat internalmask;
+
+		internalmask.create(height,width,CV_8UC1);
+		unsigned char * internaldata = (unsigned char *)(internalmask.data);
+		for(unsigned int i = 0; i < width*height;i++){internaldata[i] = 255.0*(g->what_segment(i) == gc::Graph<double,double,double>::SOURCE);}
+
+		//		cv::imshow( "rgb", cf[i]->rgb );
+		//		cv::imshow( "internalmask", internalmask );
+		//		cv::waitKey(30);
+
+		newmasks.push_back(internalmask);
+		//Time to compute external masks...
+		delete[] overlaps;
+		delete[] occlusions;
 	}
+
+	return newmasks;
 }
 
 OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step, bool debugg){
@@ -410,14 +4754,22 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask *
 	}
 
 	std::vector<float> residuals;
+	std::vector<float> angleweights;
 	std::vector<float> weights;
 	std::vector<int> ws;
 	std::vector<int> hs;
+
+	std::vector<int> dst_ws;
+	std::vector<int> dst_hs;
+
 	residuals.reserve(src_width*src_height);
+	angleweights.reserve(src_width*src_height);
 	weights.reserve(src_width*src_height);
 	if(debugg){
 		ws.reserve(src_width*src_height);
 		hs.reserve(src_width*src_height);
+		dst_ws.reserve(dst_width*dst_height);
+		dst_hs.reserve(dst_width*dst_height);
 	}
 
 	double sum = 0;
@@ -428,125 +4780,154 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask *
 
 	unsigned int test_nrdata = testw.size();
 	unsigned int indstep = step;
-//	int indstep = std::max(1.0,double(test_nrdata)/double(step));
-//	printf("indstep: %i\n",indstep);
-//	for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){
-//	//for(unsigned int src_w = 0; src_w < src_width-1; src_w++){
-//	//    for(unsigned int src_h = 0; src_h < src_height;src_h++){
-//		unsigned int src_w = testw[ind];
-//		unsigned int src_h = testh[ind];
-
-//		int src_ind0 = src_h*src_width+src_w-1;
-//		int src_ind1 = src_h*src_width+src_w;
-//		int src_ind2 = src_h*src_width+src_w+1;
-//		if(src_maskdata[src_ind0] == 255 && src_maskdata[src_ind1] == 255 && src_maskdata[src_ind2] == 255){// && p.z > 0 && !isnan(p.normal_x)){
-//			float z0 = src_idepth*float(src_depthdata[src_ind0]);
-//			float z1 = src_idepth*float(src_depthdata[src_ind1]);
-//			float z2 = src_idepth*float(src_depthdata[src_ind2]);
-
-//			double diff0 = (z0-z1)/(z0*z0+z1*z1);
-//			double diff1 = (z2-z1)/(z2*z2+z1*z1);
-//			if( fabs(diff0) < fabs(diff1)){
-//				if(diff0 != 0){
-//					sum += diff0*diff0;
-//					count++;
-//				}
-//			}else{
-//				if(diff1 != 0){
-//					sum += diff1*diff1;
-//					count++;
-//				}
-//			}
-//		}
-//	}
-//    double me = sqrt(sum/(count+1));
-//    double pred = 1.3*sqrt(2)*me;
-/*
-    for(unsigned int ind = 0; ind < test_nrdata;ind++){
-    //for(unsigned int src_w = 0; src_w < src_width-1; src_w++){
-    //    for(unsigned int src_h = 0; src_h < src_height;src_h++){
-        unsigned int src_w = testw[ind];
-        unsigned int src_h = testh[ind];
-
-        int src_ind = src_h*src_width+src_w;
-        if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){
-            float z = src_idepth*float(src_depthdata[src_ind]);
-            float nx = src_normalsdata[3*src_ind+0];
-            if(z > 0 && nx != 2){
-            //if(z > 0){
-                float x = (float(src_w) - src_cx) * z * src_ifx;
-                float y = (float(src_h) - src_cy) * z * src_ify;
-
-                float tx	= m00*x + m01*y + m02*z + m03;
-                float ty	= m10*x + m11*y + m12*z + m13;
-                float tz	= m20*x + m21*y + m22*z + m23;
-
-                //float tnx	= m00*nx + m01*ny + m02*nz;
-                //float tny	= m10*nx + m11*ny + m12*nz;
-                float tnz	= m20*nx + m21*ny + m22*nz;
-
-                float itz	= 1.0/tz;
-                float dst_w	= dst_fx*tx*itz + dst_cx;
-                float dst_h	= dst_fy*ty*itz + dst_cy;
-
-                if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
-                    unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
-                    float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
-                    if(dst_z > 0){
-                        float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0
-                        residuals.push_back(diff_z);
-                        if(debugg){
-                            ws.push_back(src_w);
-                            hs.push_back(src_h);
-                        }
-                    }
-                }
-            }
-        }
-    }
-*/
-	for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){
+	//	int indstep = std::max(1.0,double(test_nrdata)/double(step));
+	//	printf("indstep: %i\n",indstep);
+	//	for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){
+	//	//for(unsigned int src_w = 0; src_w < src_width-1; src_w++){
+	//	//    for(unsigned int src_h = 0; src_h < src_height;src_h++){
+	//		unsigned int src_w = testw[ind];
+	//		unsigned int src_h = testh[ind];
+
+	//		int src_ind0 = src_h*src_width+src_w-1;
+	//		int src_ind1 = src_h*src_width+src_w;
+	//		int src_ind2 = src_h*src_width+src_w+1;
+	//		if(src_maskdata[src_ind0] == 255 && src_maskdata[src_ind1] == 255 && src_maskdata[src_ind2] == 255){// && p.z > 0 && !isnan(p.normal_x)){
+	//			float z0 = src_idepth*float(src_depthdata[src_ind0]);
+	//			float z1 = src_idepth*float(src_depthdata[src_ind1]);
+	//			float z2 = src_idepth*float(src_depthdata[src_ind2]);
+
+	//			double diff0 = (z0-z1)/(z0*z0+z1*z1);
+	//			double diff1 = (z2-z1)/(z2*z2+z1*z1);
+	//			if( fabs(diff0) < fabs(diff1)){
+	//				if(diff0 != 0){
+	//					sum += diff0*diff0;
+	//					count++;
+	//				}
+	//			}else{
+	//				if(diff1 != 0){
+	//					sum += diff1*diff1;
+	//					count++;
+	//				}
+	//			}
+	//		}
+	//	}
+	//    double me = sqrt(sum/(count+1));
+	//    double pred = 1.3*sqrt(2)*me;
+	/*
+	for(unsigned int ind = 0; ind < test_nrdata;ind++){
+	//for(unsigned int src_w = 0; src_w < src_width-1; src_w++){
+	//    for(unsigned int src_h = 0; src_h < src_height;src_h++){
 		unsigned int src_w = testw[ind];
 		unsigned int src_h = testh[ind];
 
 		int src_ind = src_h*src_width+src_w;
-        if(src_maskdata[src_ind] == 255){
+		if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){
 			float z = src_idepth*float(src_depthdata[src_ind]);
 			float nx = src_normalsdata[3*src_ind+0];
-
 			if(z > 0 && nx != 2){
-				float ny = src_normalsdata[3*src_ind+1];
-				float nz = src_normalsdata[3*src_ind+2];
-
+			//if(z > 0){
 				float x = (float(src_w) - src_cx) * z * src_ifx;
 				float y = (float(src_h) - src_cy) * z * src_ify;
 
 				float tx	= m00*x + m01*y + m02*z + m03;
 				float ty	= m10*x + m11*y + m12*z + m13;
 				float tz	= m20*x + m21*y + m22*z + m23;
+
+				//float tnx	= m00*nx + m01*ny + m02*nz;
+				//float tny	= m10*nx + m11*ny + m12*nz;
+				float tnz	= m20*nx + m21*ny + m22*nz;
+
 				float itz	= 1.0/tz;
 				float dst_w	= dst_fx*tx*itz + dst_cx;
 				float dst_h	= dst_fy*ty*itz + dst_cy;
 
 				if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
 					unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
-
 					float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
 					if(dst_z > 0){
 						float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0
 						residuals.push_back(diff_z);
+						if(debugg){
+							ws.push_back(src_w);
+							hs.push_back(src_h);
+						}
+					}
+				}
+			}
+		}
+	}
+*/
+	for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){
+		unsigned int src_w = testw[ind];
+		unsigned int src_h = testh[ind];
+
+		int src_ind = src_h*src_width+src_w;
+		if(src_maskdata[src_ind] == 255){
+			float src_z = src_idepth*float(src_depthdata[src_ind]);
+			float src_nx = src_normalsdata[3*src_ind+0];
+			if(src_z > 0 && src_nx != 2){
+				float src_ny = src_normalsdata[3*src_ind+1];
+				float src_nz = src_normalsdata[3*src_ind+2];
+				//if(src_z > 0){
+				float src_x = (float(src_w) - src_cx) * src_z * src_ifx;
+				float src_y = (float(src_h) - src_cy) * src_z * src_ify;
+
+				float tx	= m00*src_x + m01*src_y + m02*src_z + m03;
+				float ty	= m10*src_x + m11*src_y + m12*src_z + m13;
+				float tz	= m20*src_x + m21*src_y + m22*src_z + m23;
+				float itz	= 1.0/tz;
+				float dst_w	= dst_fx*tx*itz + dst_cx;
+				float dst_h	= dst_fy*ty*itz + dst_cy;
+
+				if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){
+					unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5);
 
-						float tnx	= m00*nx + m01*ny + m02*nz;
-						float tny	= m10*nx + m11*ny + m12*nz;
-						float tnz	= m20*nx + m21*ny + m22*nz;
+					float dst_z = dst_idepth*float(dst_depthdata[dst_ind]);
+					float dst_nx = dst_normalsdata[3*dst_ind+0];
+					if(dst_z > 0 && dst_nx != 2){
+						float dst_ny = dst_normalsdata[3*dst_ind+1];
+						float dst_nz = dst_normalsdata[3*dst_ind+2];
 
 						float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
 						float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
-						float angle = (tnx*dst_x+tny*dst_y+tnz*dst_z)/sqrt(dst_x*dst_x + dst_y*dst_y + dst_z*dst_z);
-						weights.push_back(1-angle);
+
+						float tnx	= m00*src_nx + m01*src_ny + m02*src_nz;
+						float tny	= m10*src_nx + m11*src_ny + m12*src_nz;
+						float tnz	= m20*src_nx + m21*src_ny + m22*src_nz;
+
+						double info = (src_z*src_z+dst_z*dst_z);
+
+						//double d = mysign(dst_z-tz)*fabs(dst_nx*(dst_x-tx) + dst_ny*(dst_y-ty) + dst_nz*(dst_z-tz));
+						double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz));
+						residuals.push_back(d/info);
+
+						//float diff_z = (dst_z-tz)/(src_z*src_z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0
+						//residuals.push_back(diff_z);
+
+
+
+
+
+						//						float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx;
+						//						float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify;
+						//float angle = (tnx*dst_x+tny*dst_y+tnz*dst_z)/sqrt(dst_x*dst_x + dst_y*dst_y + dst_z*dst_z);
+						//weights.push_back(1-angle);
+						//weights.push_back(ny*dst_ny);
+
+
+						double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z);
+						double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst);
+
+						double dist_src = sqrt(src_x*src_x+src_y*src_y+src_z*src_z);
+						double angle_src = fabs((src_x*src_nx+src_y*src_ny+src_z*src_nz)/dist_src);
+						weights.push_back(angle_src * angle_dst);
+
 						if(debugg){
 							ws.push_back(src_w);
 							hs.push_back(src_h);
+
+							dst_ws.push_back(dst_w);
+							dst_hs.push_back(dst_h);
 						}
 					}
 				}
@@ -557,22 +4938,22 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask *
 
 
 
-//	DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2();
-//	func->maxp			= 1.0;
-//	func->update_size	= true;
-//	func->zeromean      = true;
-//	func->startreg		= 0.0001;
-//	func->debugg_print	= debugg;
-//	func->bidir			= true;
-//	func->maxnoise      = pred;
-//	func->reset();
+	//	DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2();
+	//	func->maxp			= 1.0;
+	//	func->update_size	= true;
+	//	func->zeromean      = true;
+	//	func->startreg		= 0.0001;
+	//	func->debugg_print	= debugg;
+	//	func->bidir			= true;
+	//	func->maxnoise      = pred;
+	//	func->reset();
 
 	DistanceWeightFunction2 * func = new DistanceWeightFunction2();
 	func->f = THRESHOLD;
-	func->p = 0.005;
+	func->p = 0.02;
 
 	Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size());
-    for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];}
+	for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];}
 	func->computeModel(X);
 
 	Eigen::VectorXd  W = func->getProbs(X);
@@ -594,6 +4975,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask *
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		scloud->points.resize(src_width*src_height);
 		dcloud->points.resize(dst_width*dst_height);
+		/*
 		for(unsigned int src_w = 0; src_w < src_width-1; src_w++){
 			for(unsigned int src_h = 0; src_h < src_height;src_h++){
 				int src_ind = src_h*src_width+src_w;
@@ -616,7 +4998,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask *
 		}
 		for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){
 			for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){
-                unsigned int dst_ind = dst_h*dst_width+dst_w;
+				unsigned int dst_ind = dst_h*dst_width+dst_w;
 				if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){
 					float z = dst_idepth*float(dst_depthdata[dst_ind]);
 					if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){
@@ -643,7 +5025,8 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask *
 		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
 		viewer->spin();
 		viewer->removeAllPointClouds();
-
+		*/
+		/*
 		for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){
 			for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){
 				int dst_ind = dst_h*dst_width+dst_w;
@@ -665,8 +5048,8 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask *
 				unsigned int src_ind = h * src_width + w;
 				if(ocl > 0.01 || weight > 0.01){
 					scloud->points[src_ind].r = 255.0*ocl;
-					scloud->points[src_ind].g = 255.0*weight;
-					scloud->points[src_ind].b = 255.0*0;
+					scloud->points[src_ind].g = 0;
+					scloud->points[src_ind].b = 0;
 				}else{
 					scloud->points[src_ind].x = 0;
 					scloud->points[src_ind].y = 0;
@@ -753,8 +5136,130 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask *
 		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
 		viewer->spin();
 		viewer->removeAllPointClouds();
+*/
+		/*
+		for(unsigned int src_w = 0; src_w < src_width-1; src_w++){
+			for(unsigned int src_h = 0; src_h < src_height;src_h++){
+				int src_ind = src_h*src_width+src_w;
+				if(src_maskdata[src_ind] != 255){continue;}
+				float z = src_idepth*float(src_depthdata[src_ind]);
+				if(z > 0){
+					float x = (float(src_w) - src_cx) * z * src_ifx;
+					float y = (float(src_h) - src_cy) * z * src_ify;
+					scloud->points[src_ind].x = x;
+					scloud->points[src_ind].y = y;
+					scloud->points[src_ind].z = z;
+					scloud->points[src_ind].r = 0;
+					scloud->points[src_ind].g = 0;
+					scloud->points[src_ind].b = 0;
+				}
+			}
+		}
+*/
+
+		for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){
+			for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){
+				unsigned int dst_ind = dst_h*dst_width+dst_w;
+				float z = dst_idepth*float(dst_depthdata[dst_ind]);
+				if(z > 0){
+					float x = (float(dst_w) - dst_cx) * z * dst_ifx;
+					float y = (float(dst_h) - dst_cy) * z * dst_ify;
+					dcloud->points[dst_ind].x = x;
+					dcloud->points[dst_ind].y = y;
+					dcloud->points[dst_ind].z = z+2;
+					dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2];
+					dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1];
+					dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0];
+					if(false && dst_maskdata[dst_ind] == 255){
+						dcloud->points[dst_ind].r = 255;
+						dcloud->points[dst_ind].g = 000;
+						dcloud->points[dst_ind].b = 255;
+					}
+				}
+			}
+		}
+		//		viewer->removeAllPointClouds();
+		//		//printf("%i showing results\n",__LINE__);
+		//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+		//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+
+		viewer->removeAllShapes();
+		for(unsigned int i = 0; i < residuals.size(); i++){
+			float r = residuals[i];
+			float weight = W(i);
+			float ocl = 0;
+			if(r > 0){ocl += 1-weight;}
+			if(debugg){
+				int w = ws[i];
+				int h = hs[i];
+				unsigned int src_ind = h * src_width + w;
+
+				int dst_w = dst_ws[i];
+				int dst_h = dst_hs[i];
+				unsigned int dst_ind = dst_h * src_width + dst_w;
+				if(ocl > 0.01 || weight > 0.01){
+					float z = src_idepth*float(src_depthdata[src_ind]);
+					float x = (float(w) - src_cx) * z * src_ifx;
+					float y = (float(h) - src_cy) * z * src_ify;
+					float tx	= m00*x + m01*y + m02*z + m03;
+					float ty	= m10*x + m11*y + m12*z + m13;
+					float tz	= m20*x + m21*y + m22*z + m23;
+					scloud->points[src_ind].x = tx;
+					scloud->points[src_ind].y = ty;
+					scloud->points[src_ind].z = tz+2;
+
+					scloud->points[src_ind].r = 255.0*ocl*weights.at(i);
+					scloud->points[src_ind].g = 255.0*weight*weights.at(i);
+					scloud->points[src_ind].b = 0;
+
+					char buf [1024];
+					sprintf(buf,"line%i",i);
+					viewer->addLine<pcl::PointXYZRGBNormal> (scloud->points[src_ind], dcloud->points[dst_ind],buf);
+				}else{
+					scloud->points[src_ind].x = 0;
+					scloud->points[src_ind].y = 0;
+					scloud->points[src_ind].z = 0;
+				}
+			}
+		}
+
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud");
+
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+		viewer->spin();
+		viewer->removeAllPointClouds();
+
+		/*
+		for(unsigned int i = 0; i < residuals.size(); i++){
+			float r = residuals[i];
+			float weight = W(i);
+			float ocl = 0;
+			if(r > 0){ocl += 1-weight;}
+			if(debugg){
+				int w = ws[i];
+				int h = hs[i];
+				unsigned int src_ind = h * src_width + w;
+				if(ocl > 0.01 || weight > 0.01){
+					scloud->points[src_ind].r = 255.0*(1-weights.at(i));
+					scloud->points[src_ind].g = 255.0*weights.at(i);
+					scloud->points[src_ind].b = 0;
+				}else{
+					scloud->points[src_ind].x = 0;
+					scloud->points[src_ind].y = 0;
+					scloud->points[src_ind].z = 0;
+				}
+			}
+		}
+
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+		viewer->spin();
+		viewer->removeAllPointClouds();
+*/
 	}
-/*
+	/*
 	if(debugg){
 		viewer->removeAllPointClouds();
 		viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
@@ -912,26 +5417,25 @@ vector<vector < OcclusionScore > > ModelUpdater::getOcclusionScores(std::vector<
 	for(unsigned int i = 0; i < current_frames.size(); i++){total_points+=current_modelmasks[i]->testw.size();}
 	int step = std::max(long(1),long(speedup*total_points*long(current_frames.size()))/long(50000000));
 
-//	printf("total_points: %i\n",total_points);
-//	printf("current_frames.size(): %i\n",current_frames.size());
-//	printf("ratio: %f\n",double(total_points*long(current_frames.size()))/double(50000000));
+	//	printf("total_points: %i\n",total_points);
+	//	printf("current_frames.size(): %i\n",current_frames.size());
+	//	printf("ratio: %f\n",double(total_points*long(current_frames.size()))/double(50000000));
 	printf("step: %i\n",step);
 
 	vector<vector < OcclusionScore > > occlusionScores;
 	occlusionScores.resize(current_frames.size());
-    for(unsigned int i = 0; i < current_frames.size(); i++){occlusionScores[i].resize(current_frames.size());}
+	for(unsigned int i = 0; i < current_frames.size(); i++){occlusionScores[i].resize(current_frames.size());}
 
 	int max_points = step;//100000.0/double(current_frames.size()*(current_frames.size()-1));
-    //float occlusion_penalty = 10.0f;
+	//float occlusion_penalty = 10.0f;
 	std::vector<std::vector < float > > scores;
 	scores.resize(occlusionScores.size());
-    for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());}
+	for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());}
 
-	bool lock = true;
-    for(unsigned int i = 0; i < current_frames.size(); i++){
+	bool lock = false;
+	for(unsigned int i = 0; i < current_frames.size(); i++){
 		scores[i][i] = 0;
-        for(unsigned int j = i+1; j < current_frames.size(); j++){
-			//printf("scores %i %i\n",i,j);
+		for(unsigned int j = i+1; j < current_frames.size(); j++){
 			if(lock && current_modelmasks[j]->sweepid == current_modelmasks[i]->sweepid && current_modelmasks[j]->sweepid != -1){
 				occlusionScores[i][j].score = 999999;
 				occlusionScores[i][j].occlusions = 0;
@@ -957,10 +5461,10 @@ void ModelUpdater::computeMassRegistration(std::vector<Eigen::Matrix4d> current_
 std::vector<std::vector < float > > ModelUpdater::getScores(std::vector<std::vector < OcclusionScore > > occlusionScores){//, float occlusion_penalty){
 	std::vector<std::vector < float > > scores;
 	scores.resize(occlusionScores.size());
-    for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());}
-    for(unsigned int i = 0; i < scores.size(); i++){
+	for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());}
+	for(unsigned int i = 0; i < scores.size(); i++){
 		scores[i][i] = 0;
-        for(unsigned int j = i+1; j < scores.size(); j++){
+		for(unsigned int j = i+1; j < scores.size(); j++){
 			scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions);
 			scores[j][i] = scores[i][j];
 		}
diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp
index c543c30e..d4e24940 100644
--- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp
+++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp
@@ -14,98 +14,116 @@ namespace reglib
 ModelUpdaterBasicFuse::ModelUpdaterBasicFuse(Registration * registration_){
 	registration = registration_;
 	model = new reglib::Model();
+
+    show_init_lvl = 0;//init show
+    show_refine_lvl = 0;//refine show
+    show_scoring = false;//fuse scoring show
 }
 
 ModelUpdaterBasicFuse::ModelUpdaterBasicFuse(Model * model_, Registration * registration_){
 	registration = registration_;
 	model = model_;
+
+    show_init_lvl = 0;//init show
+    show_refine_lvl = 0;//refine show
+    show_scoring = false;//fuse scoring show
 }
 
-ModelUpdaterBasicFuse::~ModelUpdaterBasicFuse(){}
+ModelUpdaterBasicFuse::~ModelUpdaterBasicFuse(){
+	//printf("deleting ModelUpdaterBasicFuse\n");
+}
 
 FusionResults ModelUpdaterBasicFuse::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){
-	if(model->frames.size() > 0){
-        printf("registerModel(%i %i)\n",int(model->id),int(model2->id));
+	if(model->points.size() > 0 && model2->points.size() > 0){
 		registration->viewer	= viewer;
 		int step1 = std::max(int(model->frames.size())/1,1);
 		int step2 = std::max(int(model2->frames.size())/1,1);//int step2 = 5;//std::min(int(model2->frames.size()),5);
-
+double register_setup_start = getTime();
 		CloudData * cd1 = model ->getCD(model->points.size()/step1);
 		registration->setDst(cd1);
 
 		CloudData * cd2	= model2->getCD(model2->points.size()/step2);
 		registration->setSrc(cd2);
+//printf("register_setup_start:          %5.5f\n",getTime()-register_setup_start);
+double register_compute_start = getTime();
 		FusionResults fr = registration->getTransform(guess);
-
-		printf("%i registerModel(%i %i)\n",__LINE__,int(model->id),int(model2->id));
+//printf("register_compute_start:          %5.5f\n",getTime()-register_compute_start);
         double best = -99999999999999;
         int best_id = -1;
-		for(unsigned int ca = 0; ca < fr.candidates.size() && ca < 150; ca++){
-            Eigen::Matrix4d pose = fr.candidates[ca];
-            std::vector<Eigen::Matrix4d>	current_poses;
-            std::vector<RGBDFrame*>			current_frames;
-			//std::vector<cv::Mat>			current_masks;
-			std::vector<ModelMask * >		current_modelmasks;
-
-            for(unsigned int i = 0; i < model->frames.size(); i++){
-				current_poses.push_back(				model->relativeposes[i]);
-				current_frames.push_back(				model->frames[i]);
-				//current_masks.push_back(				model->masks[i]);
-				current_modelmasks.push_back(			model->modelmasks[i]);
-            }
-
-            for(unsigned int i = 0; i < model2->frames.size(); i++){
-                current_poses.push_back(	pose	*	model2->relativeposes[i]);
-                current_frames.push_back(				model2->frames[i]);
-				//current_masks.push_back(				model2->masks[i]);
-				current_modelmasks.push_back(			model2->modelmasks[i]);
-            }
-
-			//std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(current_poses, current_frames,current_masks,current_modelmasks);
-			std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(current_poses, current_frames,current_modelmasks,false,10.0);
-            std::vector<std::vector < float > > scores = getScores(ocs);
-            std::vector<int> partition = getPartition(scores,2,5,2);
-
-            double sumscore1 = 0;
-            for(unsigned int i = 0; i < scores.size(); i++){
-                for(unsigned int j = 0; j < scores.size(); j++){
-                    if(i < model->frames.size() && j < model->frames.size()){sumscore1 += scores[i][j];}
-                    if(i >= model->frames.size() && j >= model->frames.size()){sumscore1 += scores[i][j];}
-                }
-            }
-
-            double sumscore2 = 0;
-            for(unsigned int i = 0; i < scores.size(); i++){
-                for(unsigned int j = 0; j < scores.size(); j++){
-                    if(partition[i] == partition[j]){sumscore2 += scores[i][j];}
-                }
-            }
-
-            double improvement = sumscore2-sumscore1;
-
-            if(improvement > best){
-                best = improvement;
-                best_id = ca;
-            }
-
-			printf("tested %i with score: %f\n",ca,sumscore2);
-
-            //printf("sumscore before: %f\n",sumscore1);
-            //printf("sumscore after: %f\n",sumscore2);
-            //printf("%i improvement: %f\n",ca,improvement);
-
-            if(improvement > 0){
-				//printf("%i improvement: %f\n",ca,improvement);
-				//getOcclusionScores(current_poses, current_frames,current_masks,true);
-            }
-        }
+double register_evaluate_start = getTime();
+
+vector<Model *> testmodels;
+vector<Matrix4d> testrps;
+addModelsToVector(testmodels,testrps,model,Eigen::Matrix4d::Identity());
+addModelsToVector(testmodels,testrps,model2,Eigen::Matrix4d::Identity());
+
+int todo = fr.candidates.size();
+double expectedCost = double(todo)*computeOcclusionScoreCosts(testmodels);
+//printf("expectedCost: %f\n",expectedCost);
+
+
+		int step = 0.5 + expectedCost/11509168.5;// ~1 sec predicted
+		step = std::max(1,step);
+
+
+
+//		printf("step: %i\n",step);
+		for(unsigned int ca = 0; ca < todo; ca++){
+			Eigen::Matrix4d pose = fr.candidates[ca];
+
+			vector<Model *> models;
+			vector<Matrix4d> rps;
+
+			addModelsToVector(models,rps,model,Eigen::Matrix4d::Identity());
+			unsigned int nr_models1 = models.size();
+			addModelsToVector(models,rps,model2,pose);
+
+			vector<vector < OcclusionScore > > ocs = computeOcclusionScore(models,rps,step,false);
+			std::vector<std::vector < float > > scores = getScores(ocs);
+			std::vector<int> partition = getPartition(scores,2,5,2);
+
+//			for(unsigned int i = 0; i < scores.size(); i++){
+//				for(unsigned int j = 0; j < scores.size(); j++){
+//					if(scores[i][j] >= 0){printf(" ");}
+//					printf("%5.5f ",0.00001*scores[i][j]);
+//				}
+//				printf("\n");
+//			}
+//printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n");
+
+			double sumscore1 = 0;
+			for(unsigned int i = 0; i < models.size(); i++){
+				for(unsigned int j = 0; j < models.size(); j++){
+					if(i < nr_models1 && j < nr_models1){sumscore1 += scores[i][j];}
+					if(i >= nr_models1 && j >= nr_models1){sumscore1 += scores[i][j];}
+				}
+			}
+
+			double sumscore2 = 0;
+			for(unsigned int i = 0; i < scores.size(); i++){
+				for(unsigned int j = 0; j < scores.size(); j++){
+					if(partition[i] == partition[j]){sumscore2 += scores[i][j];}
+				}
+			}
+
+			double improvement = sumscore2-sumscore1;
+			//printf("improvement: %f\n",improvement);
+
+			if(improvement > best){
+				computeOcclusionScore(models,rps,step,false);
+
+				best = improvement;
+				best_id = ca;
+			}
+		}
+
+//printf("register_evaluate_start:          %5.5f\n",getTime()-register_evaluate_start);
+
         if(best_id != -1){
             fr.score = 9999999;
             fr.guess = fr.candidates[best_id];
         }
 
-		
-		//printf("score: %6.6f\n",fr.score);
 		delete cd1;
 		delete cd2;
 
@@ -116,250 +134,138 @@ FusionResults ModelUpdaterBasicFuse::registerModel(Model * model2, Eigen::Matrix
 
 void ModelUpdaterBasicFuse::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanity){}
 
-UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, Model * model2){
-	printf("MODEL1 ");
-	model1->print();
-
-	printf("MODEL2 ");
-	model2->print();
 
+UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, Model * model2){
 	UpdatedModels retval = UpdatedModels();
-    Eigen::Matrix4d pose = f->guess;
-
-	std::vector<Eigen::Matrix4d>	current_poses;
-    std::vector<RGBDFrame*>			current_frames;
-	std::vector<ModelMask*>			current_modelmasks;
-
-    for(unsigned int i = 0; i < model1->frames.size(); i++){
-		current_poses.push_back(				model1->relativeposes[i]);
-		current_frames.push_back(				model1->frames[i]);
-		current_modelmasks.push_back(			model1->modelmasks[i]);
-	}
+	Eigen::Matrix4d pose = f->guess;
+
+//	printf("MODEL1 ");
+//	model1->print();
+//	printf("MODEL2 ");
+//	model2->print();
+//	printf("input pose\n");
+//	std::cout << pose << std::endl << std::endl;
+
+//	std::vector<Eigen::Matrix4d>	current_poses;
+//	std::vector<RGBDFrame*>			current_frames;
+//	std::vector<ModelMask*>			current_modelmasks;
+
+//	for(unsigned int i = 0; i < model1->frames.size(); i++){
+//		current_poses.push_back(				model1->relativeposes[i]);
+//		current_frames.push_back(				model1->frames[i]);
+//		current_modelmasks.push_back(			model1->modelmasks[i]);
+//	}
+
+//	for(unsigned int i = 0; i < model2->frames.size(); i++){
+//		current_poses.push_back(	pose	*	model2->relativeposes[i]);
+//		current_frames.push_back(				model2->frames[i]);
+//		current_modelmasks.push_back(			model2->modelmasks[i]);
+//	}
+
+	vector<Model *> models;
+	vector<Matrix4d> rps;
+	addModelsToVector(models,rps,model,Eigen::Matrix4d::Identity());
+	unsigned int nr_models1 = models.size();
+	addModelsToVector(models,rps,model2,pose);
+
+	double expectedCost = computeOcclusionScoreCosts(models);
+//	printf("expectedCost: %f\n",expectedCost);
+	int step = 0.5 + expectedCost/(10.0*11509168.5);// ~10 sec predicted max time
+	step = std::max(1,step);
+
+    vector<vector < OcclusionScore > > ocs = computeOcclusionScore(models,rps,step,show_scoring);
+	std::vector<std::vector < float > > scores = getScores(ocs);
+	std::vector<int> partition = getPartition(scores,2,5,2);
 
-    for(unsigned int i = 0; i < model2->frames.size(); i++){
-		current_poses.push_back(	pose	*	model2->relativeposes[i]);
-        current_frames.push_back(				model2->frames[i]);
-		current_modelmasks.push_back(			model2->modelmasks[i]);
-	}
+//	for(unsigned int i = 0; i < scores.size(); i++){
+//		for(unsigned int j = 0; j < scores.size(); j++){
+//			if(scores[i][j] >= 0){printf(" ");}
+//			printf("%5.5f ",0.00001*scores[i][j]);
+//		}
+//		printf("\n");
+//	}
+//	printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n");
 
-	double beforescore = model1->total_scores+model2->total_scores;
-//	printf("beforescore: %f\n",beforescore);
-	//std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(current_poses, current_frames,current_masks,current_modelmasks,false);
-	std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(current_poses, current_frames,current_modelmasks,false);
-	//std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(current_poses, current_frames,current_modelmasks,true);
-    std::vector<std::vector < float > > scores = getScores(ocs);
-	std::vector<int> partition = getPartition(scores,2,5,2);
 
-//	printf("model1\n");
-	unsigned int frames1 = model1->scores.size();
-	unsigned int frames2 = model2->scores.size();
 	double sumscore1 = 0;
-	for(unsigned int i = 0; i < frames1; i++){
-		for(unsigned int j = 0; j < frames1; j++){
-			sumscore1 += scores[i][j];
+	for(unsigned int i = 0; i < models.size(); i++){
+		for(unsigned int j = 0; j < models.size(); j++){
+			if(i < nr_models1 && j < nr_models1){sumscore1 += scores[i][j];}
+			if(i >= nr_models1 && j >= nr_models1){sumscore1 += scores[i][j];}
 		}
 	}
 
-
-//	printf("model2\n");
 	double sumscore2 = 0;
-	for(unsigned int i = 0; i < frames2; i++){
-		for(unsigned int j = 0; j < frames2; j++){
-			sumscore2 += scores[i+frames1][j+frames1];
-		}
-	}
-
-	double sumscore = 0;
-    for(unsigned int i = 0; i < scores.size(); i++){
-        for(unsigned int j = 0; j < scores.size(); j++){
-			if(partition[i] == partition[j]){sumscore += scores[i][j];}
+	for(unsigned int i = 0; i < scores.size(); i++){
+		for(unsigned int j = 0; j < scores.size(); j++){
+			if(partition[i] == partition[j]){sumscore2 += scores[i][j];}
 		}
 	}
 
-	double improvement = sumscore-sumscore1-sumscore2;
-
-	printf("sumscore before part: %f\n",sumscore1+sumscore2);
-	printf("sumscore after  part: %f\n",sumscore);
-	printf("improvement:          %f\n",improvement);
+	double improvement = sumscore2-sumscore1;
 
+//	for(unsigned int i = 0; i < scores.size(); i++){
+//		for(unsigned int j = 0; j < scores.size(); j++){
+//			if(scores[i][j] > 0){printf(" ");}
+//			printf("%5.5f ",0.0001*scores[i][j]);
+//		}
+//		printf("\n");
+//	}
+//	printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n");
+//	printf("sumscore before part: %f\n",sumscore1);
+//	printf("sumscore after  part: %f\n",sumscore2);
+//	printf("improvement:          %f\n",improvement);
+//exit(0);
 	std::vector<int> count;
 	for(unsigned int i = 0; i < partition.size(); i++){
-        if(int(count.size()) <= partition[i]){count.resize(partition[i]+1);}
+		if(int(count.size()) <= partition[i]){count.resize(partition[i]+1);}
 		count[partition[i]]++;
 	}
 
 	int minpart = count[0];
 	for(unsigned int i = 1; i < count.size(); i++){minpart = std::min(minpart,count[i]);}
 
-    printf("partition: ");for(unsigned int i = 0; i < partition.size(); i++){printf("%i ",int(partition[i]));}printf("\n");
-
 	if(count.size() == 1){
 		model1->merge(model2,pose);
-		model1->recomputeModelPoints();
-		model1->scores = scores;
-		model1->total_scores = sumscore;
 		retval.updated_models.push_back(model1);
 		retval.deleted_models.push_back(model2);
-	}
-	else if(improvement > 1){//Cannot fully fuse... separating...
-//		printf("Cannot fully fuse...\n");
-//exit(0);
-//		retval.unchanged_models.push_back(model1);
-//		retval.unchanged_models.push_back(model2);
-//		return retval;
+	}else if(improvement > 1){//Cannot fully fuse... separating...
 
 		int c = 0;
 
 		int model1_ind = partition.front();
 		bool model1_same = true;
-        for(unsigned int i = 0; i < model1->frames.size(); i++){
+		for(unsigned int i = 0; i < model1->submodels.size(); i++){
 			if(partition[c] != model1_ind){model1_same = false;}
 			c++;
 		}
 
 		int model2_ind = partition.back();
 		bool model2_same = true;
-        for(unsigned int i = 0; i < model2->frames.size(); i++){
+		for(unsigned int i = 0; i < model2->submodels.size(); i++){
 			if(partition[c] != model2_ind){model2_same = false;}
 			c++;
 		}
 
 		if(!model1_same || !model2_same){//If something changed, update models
-//			printf("SOMETHING CHANGED\n");
-//exit(0);
-            for(unsigned int i = 0; i < count.size(); i++){retval.new_models.push_back(new Model());}
+			for(unsigned int i = 0; i < count.size(); i++){retval.new_models.push_back(new Model());}
+
 			for(unsigned int i = 0; i < partition.size(); i++){
-				retval.new_models[partition[i]]->frames.push_back(current_frames[i]);
-				retval.new_models[partition[i]]->relativeposes.push_back(current_poses[i]);
-				retval.new_models[partition[i]]->modelmasks.push_back(current_modelmasks[i]);
-				//retval.new_models[partition[i]]->masks.push_back(current_masks[i].clone());
-
-//				cv::namedWindow("mask",	cv::WINDOW_AUTOSIZE);
-//				cv::imshow(		"mask",	retval.new_models[partition[i]]->masks.back());
-//				cv::waitKey(0);
+				retval.new_models[partition[i]]->submodels.push_back(models[i]);
+				retval.new_models[partition[i]]->submodels_relativeposes.push_back(rps[i]);
 			}
 
-            for(unsigned int part = 0; part < retval.new_models.size(); part++){
-				Model * m = retval.new_models[part];
-				m->recomputeModelPoints();
-				m->scores.resize(m->frames.size());
-                for(unsigned int i = 0; i < m->scores.size(); i++){
-					m->scores[i].resize(m->scores.size());
-                    for(unsigned int j = 0; j < m->scores.size(); j++){
-						m->scores[i][j] = 0;
-					}
-				}
-
-				std::vector<int> inds;
-				for(unsigned int i = 0; i < partition.size(); i++){
-                    if(partition[i] == int(part)){inds.push_back(i);}
-				}
-
-                for(unsigned int i = 0; i < m->scores.size(); i++){
-                    for(unsigned int j = 0; j < m->scores.size(); j++){
-						 m->scores[i][j] += scores[inds[i]][inds[j]];
-					}
-				}
-
-				m->total_scores = 0;
-                for(unsigned int i = 0; i < m->scores.size(); i++){
-                    for(unsigned int j = 0; j < m->scores.size(); j++){
-						m->total_scores += m->scores[i][j];
-					}
-				}
-
-//				printf("total_scores %f\n",m->total_scores);
-//				for(int i = 0; i < m->scores.size(); i++){
-//					for(int j = 0; j < m->scores.size(); j++){
-//						printf("%3.3f ",m->scores[i][j]*0.0001);
-//					}
-//					printf("\n");
-//				}
-
-				m->print();
+			for(unsigned int part = 0; part < retval.new_models.size(); part++){
+				retval.new_models[part]->recomputeModelPoints();
 			}
 
 			retval.deleted_models.push_back(model1);
 			retval.deleted_models.push_back(model2);
-		}else{//
+		}else{
 			retval.unchanged_models.push_back(model1);
 			retval.unchanged_models.push_back(model2);
 		}
 		return retval;
-//		MassRegistrationPPR * massreg = new MassRegistrationPPR(0.05,false);
-//		massreg->setData(current_frames,current_masks);
-//		MassFusionResults mfr = massreg->getTransforms(current_poses);
-//		current_poses = mfr.poses;
-//		std::vector<std::vector < OcclusionScore > > ocs2 = getOcclusionScores(current_poses, current_frames,current_masks,current_modelmasks);
-//        std::vector<std::vector < float > > scores2 = getScores(ocs2);
-//		std::vector<int> partition2 = getPartition(scores2,2,5,2);
-
-//		double sumscore_after = 0;
-//		for(int i = 0; i < scores2.size(); i++){
-//			for(int j = 0; j < scores2.size(); j++){
-//				if(partition2[i] == partition2[j]){sumscore_after += scores2[i][j];}
-//			}
-//		}
-
-//		if(sumscore_after > sumscore){
-//			printf("sumscore after: %f\n",sumscore_after);
-
-//			std::vector<int> count2;
-//			for(unsigned int i = 0; i < partition2.size(); i++){
-//				if(count2.size() <= partition2[i]){count2.resize(partition2[i]+1);}
-//				count2[partition2[i]]++;
-//			}
-
-//			int minpart2 = count2[0];
-//			for(unsigned int i = 1; i < count2.size(); i++){minpart2 = std::min(minpart2,count2[i]);}
-
-//			printf("----------------------------\n");
-//			printf("partition2: ");for(unsigned int i = 0; i < partition2.size(); i++){printf("%i ",partition2[i]);}printf("\n");
-//			for(unsigned int i = 0; i < count2.size(); i++){printf("count2 %i -> %i\n",i,count2[i]);}
-
-//			if(count2.size() == 1){//Mass registration solved the problem, GREAT
-//				printf("Mass registration solved the problem, GREAT\n");
-//				for(unsigned int i = 0; i < model2->frames.size();i++){
-//					model1->frames.push_back(model2->frames[i]);
-//					model1->masks.push_back(model2->masks[i]);
-//				}
-//				model1->relativeposes = current_poses;
-//				model1->recomputeModelPoints();
-
-//				retval.updated_models.push_back(model1);
-//				retval.deleted_models.push_back(model2);
-//			}else{//Mass registration did NOT solve the problem
-//				printf("Mass registration did NOT solve the problem UPDATE THIS PART\n");
-
-//				//Check if identical to input partition
-//				int c = 0;
-
-//				int model1_ind = partition2.front();
-//				bool model1_same = true;
-//				for(int i = 0; i < model1->frames.size(); i++){
-//					if(partition2[c] != model1_ind){model1_same = false;}
-//					c++;
-//				}
-
-//				int model2_ind = partition2.back();
-//				bool model2_same = true;
-//				for(int i = 0; i < model2->frames.size(); i++){
-//					if(partition2[c] != model2_ind){model2_same = false;}
-//					c++;
-//				}
-
-//				if(!model1_same || !model2_same){//If something changed, update models
-//					printf("SOMETHING CHANGED\n");
-//					retval.updated_models.push_back(model1);
-//					retval.updated_models.push_back(model2);
-//					exit(0);
-//				}else{//
-//					retval.unchanged_models.push_back(model1);
-//					retval.unchanged_models.push_back(model2);
-//				}
-
-//			}
-//		}
 	}
 
 	retval.unchanged_models.push_back(model1);
@@ -369,23 +275,10 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1,
 
 void ModelUpdaterBasicFuse::computeMassRegistration(std::vector<Eigen::Matrix4d> current_poses, std::vector<RGBDFrame*> current_frames,std::vector<cv::Mat> current_masks){
 	printf("void ModelUpdaterBasicFuse::computeMassRegistration\n");
-/*
-	MassRegistrationPPR * massreg = new MassRegistrationPPR();
-	//massreg->viewer = viewer;
-	massreg->setData(current_frames,current_masks);
-	MassFusionResults mfr = massreg->getTransforms(current_poses);
-
-	std::vector<std::vector < OcclusionScore > > ocs = getOcclusionScores(current_poses, current_frames,current_masks);
-	std::vector<std::vector < float > > scores = getScores(ocs,10);
-	std::vector<int> partition = getPartition(scores,2,5,2);
-	printf("new partition: ");for(unsigned int i = 0; i < partition.size(); i++){printf("%i ",partition[i]);}printf("\n");
-*/
 	printf("WARNING: THIS METHOD NOT IMPLEMENTED\n");
 	exit(0);
 }
 
-//void ModelUpdaterBasicFuse::refine(){}//No refinement behaviour added yet
-
 void ModelUpdaterBasicFuse::setRegistration( Registration * registration_){
 	if(registration != 0){delete registration;}
 	registration = registration;
diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/CHANGES.TXT b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/CHANGES.TXT
new file mode 100644
index 00000000..edd859d1
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/CHANGES.TXT
@@ -0,0 +1,29 @@
+List of changes from version 3.0:
+- Moved line
+	#include "instances.inc"
+to the end of cpp files to make it compile under GNU c++ compilers 4.2(?) and above
+
+List of changes from version 2.2:
+
+- Added functions for accessing graph structure, residual capacities, etc.
+  (They are needed for implementing maxflow-based algorithms such as primal-dual algorithm for convex MRFs.)
+- Added option of reusing trees.
+- node_id's are now integers starting from 0. Thus, it is not necessary to store node_id's in a separate array.
+- Capacity types are now templated.
+- Fixed bug in block.h. (After Block<type>::Reset, ScanFirst() and ScanNext() did not work properly).
+- Implementation with a forward star representation of the graph is no longer supported. (It needs less memory, but slightly slower than adjacency list representation.) If you still wish to use it, download version 2.2.
+- Note: version 3.0 is released under a different license than version 2.2.
+
+List of changes from version 2.1:
+
+- Put the code under GPL license
+
+List of changes from version 2.02:
+
+- Fixed a bug in the implementation that uses forward star representation
+
+List of changes from version 2.01:
+
+- Added new interface function - Graph::add_tweights(Node_id, captype, captype)
+  (necessary for the "ENERGY" software package)
+
diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/README.TXT b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/README.TXT
new file mode 100644
index 00000000..367afefe
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/README.TXT
@@ -0,0 +1,123 @@
+###################################################################
+#                                                                 #
+#    MAXFLOW - software for computing mincut/maxflow in a graph   #
+#                        Version 3.01                             #
+#    http://www.cs.ucl.ac.uk/staff/V.Kolmogorov/software.html     #
+#                                                                 #
+#    Yuri Boykov (yuri@csd.uwo.ca)                                #
+#    Vladimir Kolmogorov (v.kolmogorov@cs.ucl.ac.uk)              #
+#    2001-2006                                                    #
+#                                                                 #
+###################################################################
+
+1. Introduction.
+
+This software library implements the maxflow algorithm described in
+
+	"An Experimental Comparison of Min-Cut/Max-Flow Algorithms for Energy Minimization in Vision."
+	Yuri Boykov and Vladimir Kolmogorov.
+	In IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), 
+	September 2004
+
+This algorithm was developed by Yuri Boykov and Vladimir Kolmogorov
+at Siemens Corporate Research. To make it available for public use,
+it was later reimplemented by Vladimir Kolmogorov based on open publications.
+
+If you use this software for research purposes, you should cite
+the aforementioned paper in any resulting publication.
+
+----------------------------------------------------------------------
+
+REUSING TREES:
+
+Starting with version 3.0, there is a also an option of reusing search
+trees from one maxflow computation to the next, as described in
+
+	"Efficiently Solving Dynamic Markov Random Fields Using Graph Cuts."
+	Pushmeet Kohli and Philip H.S. Torr
+	International Conference on Computer Vision (ICCV), 2005
+
+If you use this option, you should cite
+the aforementioned paper in any resulting publication.
+
+Tested under windows, Visual C++ 6.0 compiler and unix (SunOS 5.8
+and RedHat Linux 7.0, GNU c++ compiler).
+
+##################################################################
+
+2. License & disclaimer.
+
+    Copyright 2001-2006 Vladimir Kolmogorov (v.kolmogorov@cs.ucl.ac.uk), Yuri Boykov (yuri@csd.uwo.ca).
+
+    This software can be used for research purposes only.
+    If you require another license, you may consider using version 2.21
+    (which implements exactly the same algorithm, but does not have the option of reusing search trees).
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+    OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+    SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+    LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+    DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+    THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+##################################################################
+
+3. Example usage.
+
+This section shows how to use the library to compute
+a minimum cut on the following graph:
+
+		        SOURCE
+		       /       \
+		     1/         \2
+		     /      3    \
+		   node0 -----> node1
+		     |   <-----   |
+		     |      4     |
+		     \            /
+		     5\          /6
+		       \        /
+		          SINK
+
+///////////////////////////////////////////////////
+
+#include <stdio.h>
+#include "graph.h"
+
+int main()
+{
+	typedef Graph<int,int,int> GraphType;
+	GraphType *g = new GraphType(/*estimated # of nodes*/ 2, /*estimated # of edges*/ 1); 
+
+	g -> add_node(); 
+	g -> add_node(); 
+
+	g -> add_tweights( 0,   /* capacities */  1, 5 );
+	g -> add_tweights( 1,   /* capacities */  2, 6 );
+	g -> add_edge( 0, 1,    /* capacities */  3, 4 );
+
+	int flow = g -> maxflow();
+
+	printf("Flow = %d\n", flow);
+	printf("Minimum cut:\n");
+	if (g->what_segment(0) == GraphType::SOURCE)
+		printf("node0 is in the SOURCE set\n");
+	else
+		printf("node0 is in the SINK set\n");
+	if (g->what_segment(1) == GraphType::SOURCE)
+		printf("node1 is in the SOURCE set\n");
+	else
+		printf("node1 is in the SINK set\n");
+
+	delete g;
+
+	return 0;
+}
+
+
+///////////////////////////////////////////////////
diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/block.h b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/block.h
new file mode 100644
index 00000000..c4dfa467
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/block.h
@@ -0,0 +1,268 @@
+/* block.h */
+/*
+	Template classes Block and DBlock
+	Implement adding and deleting items of the same type in blocks.
+
+	If there there are many items then using Block or DBlock
+	is more efficient than using 'new' and 'delete' both in terms
+	of memory and time since
+	(1) On some systems there is some minimum amount of memory
+	    that 'new' can allocate (e.g., 64), so if items are
+	    small that a lot of memory is wasted.
+	(2) 'new' and 'delete' are designed for items of varying size.
+	    If all items has the same size, then an algorithm for
+	    adding and deleting can be made more efficient.
+	(3) All Block and DBlock functions are inline, so there are
+	    no extra function calls.
+
+	Differences between Block and DBlock:
+	(1) DBlock allows both adding and deleting items,
+	    whereas Block allows only adding items.
+	(2) Block has an additional operation of scanning
+	    items added so far (in the order in which they were added).
+	(3) Block allows to allocate several consecutive
+	    items at a time, whereas DBlock can add only a single item.
+
+	Note that no constructors or destructors are called for items.
+
+	Example usage for items of type 'MyType':
+
+	///////////////////////////////////////////////////
+	#include "block.h"
+	#define BLOCK_SIZE 1024
+	typedef struct { int a, b; } MyType;
+	MyType *ptr, *array[10000];
+
+	...
+
+	Block<MyType> *block = new Block<MyType>(BLOCK_SIZE);
+
+	// adding items
+	for (int i=0; i<sizeof(array); i++)
+	{
+		ptr = block -> New();
+		ptr -> a = ptr -> b = rand();
+	}
+
+	// reading items
+	for (ptr=block->ScanFirst(); ptr; ptr=block->ScanNext())
+	{
+		printf("%d %d\n", ptr->a, ptr->b);
+	}
+
+	delete block;
+
+	...
+
+	DBlock<MyType> *dblock = new DBlock<MyType>(BLOCK_SIZE);
+	
+	// adding items
+	for (int i=0; i<sizeof(array); i++)
+	{
+		array[i] = dblock -> New();
+	}
+
+	// deleting items
+	for (int i=0; i<sizeof(array); i+=2)
+	{
+		dblock -> Delete(array[i]);
+	}
+
+	// adding items
+	for (int i=0; i<sizeof(array); i++)
+	{
+		array[i] = dblock -> New();
+	}
+
+	delete dblock;
+
+	///////////////////////////////////////////////////
+
+	Note that DBlock deletes items by marking them as
+	empty (i.e., by adding them to the list of free items),
+	so that this memory could be used for subsequently
+	added items. Thus, at each moment the memory allocated
+	is determined by the maximum number of items allocated
+	simultaneously at earlier moments. All memory is
+	deallocated only when the destructor is called.
+*/
+
+#ifndef __BLOCK_H__
+#define __BLOCK_H__
+
+#include <stdlib.h>
+
+/***********************************************************************/
+/***********************************************************************/
+/***********************************************************************/
+
+template <class Type> class Block
+{
+public:
+	/* Constructor. Arguments are the block size and
+	   (optionally) the pointer to the function which
+	   will be called if allocation failed; the message
+	   passed to this function is "Not enough memory!" */
+	Block(int size, void (*err_function)(char *) = NULL) { first = last = NULL; block_size = size; error_function = err_function; }
+
+	/* Destructor. Deallocates all items added so far */
+	~Block() { while (first) { block *next = first -> next; delete[] ((char*)first); first = next; } }
+
+	/* Allocates 'num' consecutive items; returns pointer
+	   to the first item. 'num' cannot be greater than the
+	   block size since items must fit in one block */
+	Type *New(int num = 1)
+	{
+		Type *t;
+
+		if (!last || last->current + num > last->last)
+		{
+			if (last && last->next) last = last -> next;
+			else
+			{
+				block *next = (block *) new char [sizeof(block) + (block_size-1)*sizeof(Type)];
+				if (!next) { if (error_function) (*error_function)("Not enough memory!"); exit(1); }
+				if (last) last -> next = next;
+				else first = next;
+				last = next;
+				last -> current = & ( last -> data[0] );
+				last -> last = last -> current + block_size;
+				last -> next = NULL;
+			}
+		}
+
+		t = last -> current;
+		last -> current += num;
+		return t;
+	}
+
+	/* Returns the first item (or NULL, if no items were added) */
+	Type *ScanFirst()
+	{
+		for (scan_current_block=first; scan_current_block; scan_current_block = scan_current_block->next)
+		{
+			scan_current_data = & ( scan_current_block -> data[0] );
+			if (scan_current_data < scan_current_block -> current) return scan_current_data ++;
+		}
+		return NULL;
+	}
+
+	/* Returns the next item (or NULL, if all items have been read)
+	   Can be called only if previous ScanFirst() or ScanNext()
+	   call returned not NULL. */
+	Type *ScanNext()
+	{
+		while (scan_current_data >= scan_current_block -> current)
+		{
+			scan_current_block = scan_current_block -> next;
+			if (!scan_current_block) return NULL;
+			scan_current_data = & ( scan_current_block -> data[0] );
+		}
+		return scan_current_data ++;
+	}
+
+	/* Marks all elements as empty */
+	void Reset()
+	{
+		block *b;
+		if (!first) return;
+		for (b=first; ; b=b->next)
+		{
+			b -> current = & ( b -> data[0] );
+			if (b == last) break;
+		}
+		last = first;
+	}
+
+/***********************************************************************/
+
+private:
+
+	typedef struct block_st
+	{
+		Type					*current, *last;
+		struct block_st			*next;
+		Type					data[1];
+	} block;
+
+	int		block_size;
+	block	*first;
+	block	*last;
+
+	block	*scan_current_block;
+	Type	*scan_current_data;
+
+	void	(*error_function)(char *);
+};
+
+/***********************************************************************/
+/***********************************************************************/
+/***********************************************************************/
+
+template <class Type> class DBlock
+{
+public:
+	/* Constructor. Arguments are the block size and
+	   (optionally) the pointer to the function which
+	   will be called if allocation failed; the message
+	   passed to this function is "Not enough memory!" */
+	DBlock(int size, void (*err_function)(char *) = NULL) { first = NULL; first_free = NULL; block_size = size; error_function = err_function; }
+
+	/* Destructor. Deallocates all items added so far */
+	~DBlock() { while (first) { block *next = first -> next; delete[] ((char*)first); first = next; } }
+
+	/* Allocates one item */
+	Type *New()
+	{
+		block_item *item;
+
+		if (!first_free)
+		{
+			block *next = first;
+			first = (block *) new char [sizeof(block) + (block_size-1)*sizeof(block_item)];
+			if (!first) { if (error_function) (*error_function)("Not enough memory!"); exit(1); }
+			first_free = & (first -> data[0] );
+			for (item=first_free; item<first_free+block_size-1; item++)
+				item -> next_free = item + 1;
+			item -> next_free = NULL;
+			first -> next = next;
+		}
+
+		item = first_free;
+		first_free = item -> next_free;
+		return (Type *) item;
+	}
+
+	/* Deletes an item allocated previously */
+	void Delete(Type *t)
+	{
+		((block_item *) t) -> next_free = first_free;
+		first_free = (block_item *) t;
+	}
+
+/***********************************************************************/
+
+private:
+
+	typedef union block_item_st
+	{
+		Type			t;
+		block_item_st	*next_free;
+	} block_item;
+
+	typedef struct block_st
+	{
+		struct block_st			*next;
+		block_item				data[1];
+	} block;
+
+	int			block_size;
+	block		*first;
+	block_item	*first_free;
+
+	void	(*error_function)(char *);
+};
+
+
+#endif
+
diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.cpp b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.cpp
new file mode 100644
index 00000000..fad38542
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.cpp
@@ -0,0 +1,115 @@
+/* graph.cpp */
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "graph.h"
+
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	Graph<captype, tcaptype, flowtype>::Graph(int node_num_max, int edge_num_max, void (*err_function)(char *))
+	: node_num(0),
+	  nodeptr_block(NULL),
+	  error_function(err_function)
+{
+	if (node_num_max < 16) node_num_max = 16;
+	if (edge_num_max < 16) edge_num_max = 16;
+
+	nodes = (node*) malloc(node_num_max*sizeof(node));
+	arcs = (arc*) malloc(2*edge_num_max*sizeof(arc));
+	if (!nodes || !arcs) { if (error_function) (*error_function)("Not enough memory!"); exit(1); }
+
+	node_last = nodes;
+	node_max = nodes + node_num_max;
+	arc_last = arcs;
+	arc_max = arcs + 2*edge_num_max;
+
+	maxflow_iteration = 0;
+	flow = 0;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	Graph<captype,tcaptype,flowtype>::~Graph()
+{
+	if (nodeptr_block) 
+	{ 
+		delete nodeptr_block; 
+		nodeptr_block = NULL; 
+	}
+	free(nodes);
+	free(arcs);
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::reset()
+{
+	node_last = nodes;
+	arc_last = arcs;
+	node_num = 0;
+
+	if (nodeptr_block) 
+	{ 
+		delete nodeptr_block; 
+		nodeptr_block = NULL; 
+	}
+
+	maxflow_iteration = 0;
+	flow = 0;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::reallocate_nodes(int num)
+{
+	int node_num_max = (int)(node_max - nodes);
+	node* nodes_old = nodes;
+
+	node_num_max += node_num_max / 2;
+	if (node_num_max < node_num + num) node_num_max = node_num + num;
+	nodes = (node*) realloc(nodes_old, node_num_max*sizeof(node));
+	if (!nodes) { if (error_function) (*error_function)("Not enough memory!"); exit(1); }
+
+	node_last = nodes + node_num;
+	node_max = nodes + node_num_max;
+
+	if (nodes != nodes_old)
+	{
+		arc* a;
+		for (a=arcs; a<arc_last; a++)
+		{
+			a->head = (node*) ((char*)a->head + (((char*) nodes) - ((char*) nodes_old)));
+		}
+	}
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::reallocate_arcs()
+{
+	int arc_num_max = (int)(arc_max - arcs);
+	int arc_num = (int)(arc_last - arcs);
+	arc* arcs_old = arcs;
+
+	arc_num_max += arc_num_max / 2; if (arc_num_max & 1) arc_num_max ++;
+	arcs = (arc*) realloc(arcs_old, arc_num_max*sizeof(arc));
+	if (!arcs) { if (error_function) (*error_function)("Not enough memory!"); exit(1); }
+
+	arc_last = arcs + arc_num;
+	arc_max = arcs + arc_num_max;
+
+	if (arcs != arcs_old)
+	{
+		node* i;
+		arc* a;
+		for (i=nodes; i<node_last; i++)
+		{
+			if (i->first) i->first = (arc*) ((char*)i->first + (((char*) arcs) - ((char*) arcs_old)));
+		}
+		for (a=arcs; a<arc_last; a++)
+		{
+			if (a->next) a->next = (arc*) ((char*)a->next + (((char*) arcs) - ((char*) arcs_old)));
+			a->sister = (arc*) ((char*)a->sister + (((char*) arcs) - ((char*) arcs_old)));
+		}
+	}
+}
+
+#include "instances.inc"
diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.h b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.h
new file mode 100644
index 00000000..87efe7ef
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.h
@@ -0,0 +1,506 @@
+/* graph.h */
+/*
+	This software library implements the maxflow algorithm
+	described in
+
+		"An Experimental Comparison of Min-Cut/Max-Flow Algorithms for Energy Minimization in Vision."
+		Yuri Boykov and Vladimir Kolmogorov.
+		In IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), 
+		September 2004
+
+	This algorithm was developed by Yuri Boykov and Vladimir Kolmogorov
+	at Siemens Corporate Research. To make it available for public use,
+	it was later reimplemented by Vladimir Kolmogorov based on open publications.
+
+	If you use this software for research purposes, you should cite
+	the aforementioned paper in any resulting publication.
+
+	----------------------------------------------------------------------
+
+	REUSING TREES:
+
+	Starting with version 3.0, there is a also an option of reusing search
+	trees from one maxflow computation to the next, as described in
+
+		"Efficiently Solving Dynamic Markov Random Fields Using Graph Cuts."
+		Pushmeet Kohli and Philip H.S. Torr
+		International Conference on Computer Vision (ICCV), 2005
+
+	If you use this option, you should cite
+	the aforementioned paper in any resulting publication.
+*/
+	
+
+
+/*
+	For description, license, example usage see README.TXT.
+*/
+
+#ifndef __GRAPH_H__
+#define __GRAPH_H__
+
+#include <string.h>
+#include "block.h"
+
+#include <assert.h>
+// NOTE: in UNIX you need to use -DNDEBUG preprocessor option to supress assert's!!!
+
+
+
+// captype: type of edge capacities (excluding t-links)
+// tcaptype: type of t-links (edges between nodes and terminals)
+// flowtype: type of total flow
+//
+// Current instantiations are in instances.inc
+template <typename captype, typename tcaptype, typename flowtype> class Graph
+{
+public:
+	typedef enum
+	{
+		SOURCE	= 0,
+		SINK	= 1
+	} termtype; // terminals 
+	typedef int node_id;
+
+	/////////////////////////////////////////////////////////////////////////
+	//                     BASIC INTERFACE FUNCTIONS                       //
+    //              (should be enough for most applications)               //
+	/////////////////////////////////////////////////////////////////////////
+
+	// Constructor. 
+	// The first argument gives an estimate of the maximum number of nodes that can be added
+	// to the graph, and the second argument is an estimate of the maximum number of edges.
+	// The last (optional) argument is the pointer to the function which will be called 
+	// if an error occurs; an error message is passed to this function. 
+	// If this argument is omitted, exit(1) will be called.
+	//
+	// IMPORTANT: It is possible to add more nodes to the graph than node_num_max 
+	// (and node_num_max can be zero). However, if the count is exceeded, then 
+	// the internal memory is reallocated (increased by 50%) which is expensive. 
+	// Also, temporarily the amount of allocated memory would be more than twice than needed.
+	// Similarly for edges.
+	// If you wish to avoid this overhead, you can download version 2.2, where nodes and edges are stored in blocks.
+	Graph(int node_num_max, int edge_num_max, void (*err_function)(char *) = NULL);
+
+	// Destructor
+	~Graph();
+
+	// Adds node(s) to the graph. By default, one node is added (num=1); then first call returns 0, second call returns 1, and so on. 
+	// If num>1, then several nodes are added, and node_id of the first one is returned.
+	// IMPORTANT: see note about the constructor 
+	node_id add_node(int num = 1);
+
+	// Adds a bidirectional edge between 'i' and 'j' with the weights 'cap' and 'rev_cap'.
+	// IMPORTANT: see note about the constructor 
+	void add_edge(node_id i, node_id j, captype cap, captype rev_cap);
+
+	// Adds new edges 'SOURCE->i' and 'i->SINK' with corresponding weights.
+	// Can be called multiple times for each node.
+	// Weights can be negative.
+	// NOTE: the number of such edges is not counted in edge_num_max.
+	//       No internal memory is allocated by this call.
+	void add_tweights(node_id i, tcaptype cap_source, tcaptype cap_sink);
+
+
+	// Computes the maxflow. Can be called several times.
+	// FOR DESCRIPTION OF reuse_trees, SEE mark_node().
+	// FOR DESCRIPTION OF changed_list, SEE remove_from_changed_list().
+	flowtype maxflow(bool reuse_trees = false, Block<node_id>* changed_list = NULL);
+
+	// After the maxflow is computed, this function returns to which
+	// segment the node 'i' belongs (Graph<captype,tcaptype,flowtype>::SOURCE or Graph<captype,tcaptype,flowtype>::SINK).
+	//
+	// Occasionally there may be several minimum cuts. If a node can be assigned
+	// to both the source and the sink, then default_segm is returned.
+	termtype what_segment(node_id i, termtype default_segm = SOURCE);
+
+
+
+	//////////////////////////////////////////////
+	//       ADVANCED INTERFACE FUNCTIONS       //
+	//      (provide access to the graph)       //
+	//////////////////////////////////////////////
+
+private:
+	struct node;
+	struct arc;
+
+public:
+
+	////////////////////////////
+	// 1. Reallocating graph. //
+	////////////////////////////
+
+	// Removes all nodes and edges. 
+	// After that functions add_node() and add_edge() must be called again. 
+	//
+	// Advantage compared to deleting Graph and allocating it again:
+	// no calls to delete/new (which could be quite slow).
+	//
+	// If the graph structure stays the same, then an alternative
+	// is to go through all nodes/edges and set new residual capacities
+	// (see functions below).
+	void reset();
+
+	////////////////////////////////////////////////////////////////////////////////
+	// 2. Functions for getting pointers to arcs and for reading graph structure. //
+	//    NOTE: adding new arcs may invalidate these pointers (if reallocation    //
+	//    happens). So it's best not to add arcs while reading graph structure.   //
+	////////////////////////////////////////////////////////////////////////////////
+
+	// The following two functions return arcs in the same order that they
+	// were added to the graph. NOTE: for each call add_edge(i,j,cap,cap_rev)
+	// the first arc returned will be i->j, and the second j->i.
+	// If there are no more arcs, then the function can still be called, but
+	// the returned arc_id is undetermined.
+	typedef arc* arc_id;
+	arc_id get_first_arc();
+	arc_id get_next_arc(arc_id a);
+
+	// other functions for reading graph structure
+	int get_node_num() { return node_num; }
+	int get_arc_num() { return (int)(arc_last - arcs); }
+	void get_arc_ends(arc_id a, node_id& i, node_id& j); // returns i,j to that a = i->j
+
+	///////////////////////////////////////////////////
+	// 3. Functions for reading residual capacities. //
+	///////////////////////////////////////////////////
+
+	// returns residual capacity of SOURCE->i minus residual capacity of i->SINK
+	tcaptype get_trcap(node_id i); 
+	// returns residual capacity of arc a
+	captype get_rcap(arc* a);
+
+	/////////////////////////////////////////////////////////////////
+	// 4. Functions for setting residual capacities.               //
+	//    NOTE: If these functions are used, the value of the flow //
+	//    returned by maxflow() will not be valid!                 //
+	/////////////////////////////////////////////////////////////////
+
+	void set_trcap(node_id i, tcaptype trcap); 
+	void set_rcap(arc* a, captype rcap);
+
+	////////////////////////////////////////////////////////////////////
+	// 5. Functions related to reusing trees & list of changed nodes. //
+	////////////////////////////////////////////////////////////////////
+
+	// If flag reuse_trees is true while calling maxflow(), then search trees
+	// are reused from previous maxflow computation. 
+	// In this case before calling maxflow() the user must
+	// specify which parts of the graph have changed by calling mark_node():
+	//   add_tweights(i),set_trcap(i)    => call mark_node(i)
+	//   add_edge(i,j),set_rcap(a)       => call mark_node(i); mark_node(j)
+	//
+	// This option makes sense only if a small part of the graph is changed.
+	// The initialization procedure goes only through marked nodes then.
+	// 
+	// mark_node(i) can either be called before or after graph modification.
+	// Can be called more than once per node, but calls after the first one
+	// do not have any effect.
+	// 
+	// NOTE: 
+	//   - This option cannot be used in the first call to maxflow().
+	//   - It is not necessary to call mark_node() if the change is ``not essential'',
+	//     i.e. sign(trcap) is preserved for a node and zero/nonzero status is preserved for an arc.
+	//   - To check that you marked all necessary nodes, you can call maxflow(false) after calling maxflow(true).
+	//     If everything is correct, the two calls must return the same value of flow. (Useful for debugging).
+	void mark_node(node_id i);
+
+	// If changed_list is not NULL while calling maxflow(), then the algorithm
+	// keeps a list of nodes which could potentially have changed their segmentation label.
+	// Nodes which are not in the list are guaranteed to keep their old segmentation label (SOURCE or SINK).
+	// Example usage:
+	//
+	//		typedef Graph<int,int,int> G;
+	//		G* g = new Graph(nodeNum, edgeNum);
+	//		Block<G::node_id>* changed_list = new Block<G::node_id>(128);
+	//
+	//		... // add nodes and edges
+	//
+	//		g->maxflow(); // first call should be without arguments
+	//		for (int iter=0; iter<10; iter++)
+	//		{
+	//			... // change graph, call mark_node() accordingly
+	//
+	//			g->maxflow(true, changed_list);
+	//			G::node_id* ptr;
+	//			for (ptr=changed_list->ScanFirst(); ptr; ptr=changed_list->ScanNext())
+	//			{
+	//				G::node_id i = *ptr; assert(i>=0 && i<nodeNum);
+	//				g->remove_from_changed_list(i);
+	//				// do something with node i...
+	//				if (g->what_segment(i) == G::SOURCE) { ... }
+	//			}
+	//			changed_list->Reset();
+	//		}
+	//		delete changed_list;
+	//		
+	// NOTE:
+	//  - If changed_list option is used, then reuse_trees must be used as well.
+	//  - In the example above, the user may omit calls g->remove_from_changed_list(i) and changed_list->Reset() in a given iteration.
+	//    Then during the next call to maxflow(true, &changed_list) new nodes will be added to changed_list.
+	//  - If the next call to maxflow() does not use option reuse_trees, then calling remove_from_changed_list()
+	//    is not necessary. ("changed_list->Reset()" or "delete changed_list" should still be called, though).
+	void remove_from_changed_list(node_id i) 
+	{ 
+		assert(i>=0 && i<node_num && nodes[i].is_in_changed_list); 
+		nodes[i].is_in_changed_list = 0;
+	}
+
+
+
+
+
+
+/////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////
+	
+private:
+	// internal variables and functions
+
+	struct node
+	{
+		arc			*first;		// first outcoming arc
+
+		arc			*parent;	// node's parent
+		node		*next;		// pointer to the next active node
+								//   (or to itself if it is the last node in the list)
+		int			TS;			// timestamp showing when DIST was computed
+		int			DIST;		// distance to the terminal
+		int			is_sink : 1;	// flag showing whether the node is in the source or in the sink tree (if parent!=NULL)
+		int			is_marked : 1;	// set by mark_node()
+		int			is_in_changed_list : 1; // set by maxflow if 
+
+		tcaptype	tr_cap;		// if tr_cap > 0 then tr_cap is residual capacity of the arc SOURCE->node
+								// otherwise         -tr_cap is residual capacity of the arc node->SINK 
+
+	};
+
+	struct arc
+	{
+		node		*head;		// node the arc points to
+		arc			*next;		// next arc with the same originating node
+		arc			*sister;	// reverse arc
+
+		captype		r_cap;		// residual capacity
+	};
+
+	struct nodeptr
+	{
+		node    	*ptr;
+		nodeptr		*next;
+	};
+	static const int NODEPTR_BLOCK_SIZE = 128;
+
+	node				*nodes, *node_last, *node_max; // node_last = nodes+node_num, node_max = nodes+node_num_max;
+	arc					*arcs, *arc_last, *arc_max; // arc_last = arcs+2*edge_num, arc_max = arcs+2*edge_num_max;
+
+	int					node_num;
+
+	DBlock<nodeptr>		*nodeptr_block;
+
+	void	(*error_function)(char *);	// this function is called if a error occurs,
+										// with a corresponding error message
+										// (or exit(1) is called if it's NULL)
+
+	flowtype			flow;		// total flow
+
+	// reusing trees & list of changed pixels
+	int					maxflow_iteration; // counter
+	Block<node_id>		*changed_list;
+
+	/////////////////////////////////////////////////////////////////////////
+
+	node				*queue_first[2], *queue_last[2];	// list of active nodes
+	nodeptr				*orphan_first, *orphan_last;		// list of pointers to orphans
+	int					TIME;								// monotonically increasing global counter
+
+	/////////////////////////////////////////////////////////////////////////
+
+	void reallocate_nodes(int num); // num is the number of new nodes
+	void reallocate_arcs();
+
+	// functions for processing active list
+	void set_active(node *i);
+	node *next_active();
+
+	// functions for processing orphans list
+	void set_orphan_front(node* i); // add to the beginning of the list
+	void set_orphan_rear(node* i);  // add to the end of the list
+
+	void add_to_changed_list(node* i);
+
+	void maxflow_init();             // called if reuse_trees == false
+	void maxflow_reuse_trees_init(); // called if reuse_trees == true
+	void augment(arc *middle_arc);
+	void process_source_orphan(node *i);
+	void process_sink_orphan(node *i);
+
+	void test_consistency(node* current_node=NULL); // debug function
+};
+
+
+
+
+
+
+
+
+
+
+
+///////////////////////////////////////
+// Implementation - inline functions //
+///////////////////////////////////////
+
+
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline typename Graph<captype,tcaptype,flowtype>::node_id Graph<captype,tcaptype,flowtype>::add_node(int num)
+{
+	assert(num > 0);
+
+	if (node_last + num > node_max) reallocate_nodes(num);
+
+	if (num == 1)
+	{
+		node_last -> first = NULL;
+		node_last -> tr_cap = 0;
+		node_last -> is_marked = 0;
+		node_last -> is_in_changed_list = 0;
+
+		node_last ++;
+		return node_num ++;
+	}
+	else
+	{
+		memset(node_last, 0, num*sizeof(node));
+
+		node_id i = node_num;
+		node_num += num;
+		node_last += num;
+		return i;
+	}
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::add_tweights(node_id i, tcaptype cap_source, tcaptype cap_sink)
+{
+	assert(i >= 0 && i < node_num);
+
+	tcaptype delta = nodes[i].tr_cap;
+	if (delta > 0) cap_source += delta;
+	else           cap_sink   -= delta;
+	flow += (cap_source < cap_sink) ? cap_source : cap_sink;
+	nodes[i].tr_cap = cap_source - cap_sink;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::add_edge(node_id _i, node_id _j, captype cap, captype rev_cap)
+{
+	assert(_i >= 0 && _i < node_num);
+	assert(_j >= 0 && _j < node_num);
+	assert(_i != _j);
+	assert(cap >= 0);
+	assert(rev_cap >= 0);
+
+	if (arc_last == arc_max) reallocate_arcs();
+
+	arc *a = arc_last ++;
+	arc *a_rev = arc_last ++;
+
+	node* i = nodes + _i;
+	node* j = nodes + _j;
+
+	a -> sister = a_rev;
+	a_rev -> sister = a;
+	a -> next = i -> first;
+	i -> first = a;
+	a_rev -> next = j -> first;
+	j -> first = a_rev;
+	a -> head = j;
+	a_rev -> head = i;
+	a -> r_cap = cap;
+	a_rev -> r_cap = rev_cap;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline typename Graph<captype,tcaptype,flowtype>::arc* Graph<captype,tcaptype,flowtype>::get_first_arc()
+{
+	return arcs;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline typename Graph<captype,tcaptype,flowtype>::arc* Graph<captype,tcaptype,flowtype>::get_next_arc(arc* a) 
+{
+	return a + 1; 
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::get_arc_ends(arc* a, node_id& i, node_id& j)
+{
+	assert(a >= arcs && a < arc_last);
+	i = (node_id) (a->sister->head - nodes);
+	j = (node_id) (a->head - nodes);
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline tcaptype Graph<captype,tcaptype,flowtype>::get_trcap(node_id i)
+{
+	assert(i>=0 && i<node_num);
+	return nodes[i].tr_cap;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline captype Graph<captype,tcaptype,flowtype>::get_rcap(arc* a)
+{
+	assert(a >= arcs && a < arc_last);
+	return a->r_cap;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::set_trcap(node_id i, tcaptype trcap)
+{
+	assert(i>=0 && i<node_num); 
+	nodes[i].tr_cap = trcap;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::set_rcap(arc* a, captype rcap)
+{
+	assert(a >= arcs && a < arc_last);
+	a->r_cap = rcap;
+}
+
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline typename Graph<captype,tcaptype,flowtype>::termtype Graph<captype,tcaptype,flowtype>::what_segment(node_id i, termtype default_segm)
+{
+	if (nodes[i].parent)
+	{
+		return (nodes[i].is_sink) ? SINK : SOURCE;
+	}
+	else
+	{
+		return default_segm;
+	}
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::mark_node(node_id _i)
+{
+	node* i = nodes + _i;
+	if (!i->next)
+	{
+		/* it's not in the list yet */
+		if (queue_last[1]) queue_last[1] -> next = i;
+		else               queue_first[1]        = i;
+		queue_last[1] = i;
+		i -> next = i;
+	}
+	i->is_marked = 1;
+}
+
+
+#endif
diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/instances.inc b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/instances.inc
new file mode 100644
index 00000000..1ea80f55
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/instances.inc
@@ -0,0 +1,16 @@
+#include "graph.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable: 4661)
+#endif
+
+// Instantiations: <captype, tcaptype, flowtype>
+// IMPORTANT: 
+//    flowtype should be 'larger' than tcaptype 
+//    tcaptype should be 'larger' than captype
+
+//template class Graph<int,int,int>;
+//template class Graph<short,int,int>;
+//template class Graph<float,float,float>;
+//template class Graph<double,double,double>;
+
diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/maxflow.cpp b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/maxflow.cpp
new file mode 100644
index 00000000..a2fc042b
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/maxflow.cpp
@@ -0,0 +1,684 @@
+/* maxflow.cpp */
+
+
+#include <stdio.h>
+#include "graph.h"
+
+
+/*
+	special constants for node->parent
+*/
+#define TERMINAL ( (arc *) 1 )		/* to terminal */
+#define ORPHAN   ( (arc *) 2 )		/* orphan */
+
+
+#define INFINITE_D ((int)(((unsigned)-1)/2))		/* infinite distance to the terminal */
+
+/***********************************************************************/
+
+/*
+	Functions for processing active list.
+	i->next points to the next node in the list
+	(or to i, if i is the last node in the list).
+	If i->next is NULL iff i is not in the list.
+
+	There are two queues. Active nodes are added
+	to the end of the second queue and read from
+	the front of the first queue. If the first queue
+	is empty, it is replaced by the second queue
+	(and the second queue becomes empty).
+*/
+
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::set_active(node *i)
+{
+	if (!i->next)
+	{
+		/* it's not in the list yet */
+		if (queue_last[1]) queue_last[1] -> next = i;
+		else               queue_first[1]        = i;
+		queue_last[1] = i;
+		i -> next = i;
+	}
+}
+
+/*
+	Returns the next active node.
+	If it is connected to the sink, it stays in the list,
+	otherwise it is removed from the list
+*/
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline typename Graph<captype,tcaptype,flowtype>::node* Graph<captype,tcaptype,flowtype>::next_active()
+{
+	node *i;
+
+	while ( 1 )
+	{
+		if (!(i=queue_first[0]))
+		{
+			queue_first[0] = i = queue_first[1];
+			queue_last[0]  = queue_last[1];
+			queue_first[1] = NULL;
+			queue_last[1]  = NULL;
+			if (!i) return NULL;
+		}
+
+		/* remove it from the active list */
+		if (i->next == i) queue_first[0] = queue_last[0] = NULL;
+		else              queue_first[0] = i -> next;
+		i -> next = NULL;
+
+		/* a node in the list is active iff it has a parent */
+		if (i->parent) return i;
+	}
+}
+
+/***********************************************************************/
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::set_orphan_front(node *i)
+{
+	nodeptr *np;
+	i -> parent = ORPHAN;
+	np = nodeptr_block -> New();
+	np -> ptr = i;
+	np -> next = orphan_first;
+	orphan_first = np;
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::set_orphan_rear(node *i)
+{
+	nodeptr *np;
+	i -> parent = ORPHAN;
+	np = nodeptr_block -> New();
+	np -> ptr = i;
+	if (orphan_last) orphan_last -> next = np;
+	else             orphan_first        = np;
+	orphan_last = np;
+	np -> next = NULL;
+}
+
+/***********************************************************************/
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	inline void Graph<captype,tcaptype,flowtype>::add_to_changed_list(node *i)
+{
+	if (changed_list && !i->is_in_changed_list)
+	{
+		node_id* ptr = changed_list->New();
+		*ptr = (node_id)(i - nodes);
+		i->is_in_changed_list = true;
+	}
+}
+
+/***********************************************************************/
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::maxflow_init()
+{
+	node *i;
+
+	queue_first[0] = queue_last[0] = NULL;
+	queue_first[1] = queue_last[1] = NULL;
+	orphan_first = NULL;
+
+	TIME = 0;
+
+	for (i=nodes; i<node_last; i++)
+	{
+		i -> next = NULL;
+		i -> is_marked = 0;
+		i -> is_in_changed_list = 0;
+		i -> TS = TIME;
+		if (i->tr_cap > 0)
+		{
+			/* i is connected to the source */
+			i -> is_sink = 0;
+			i -> parent = TERMINAL;
+			set_active(i);
+			i -> DIST = 1;
+		}
+		else if (i->tr_cap < 0)
+		{
+			/* i is connected to the sink */
+			i -> is_sink = 1;
+			i -> parent = TERMINAL;
+			set_active(i);
+			i -> DIST = 1;
+		}
+		else
+		{
+			i -> parent = NULL;
+		}
+	}
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::maxflow_reuse_trees_init()
+{
+	node* i;
+	node* j;
+	node* queue = queue_first[1];
+	arc* a;
+	nodeptr* np;
+
+	queue_first[0] = queue_last[0] = NULL;
+	queue_first[1] = queue_last[1] = NULL;
+	orphan_first = orphan_last = NULL;
+
+	TIME ++;
+
+	while ((i=queue))
+	{
+		queue = i->next;
+		if (queue == i) queue = NULL;
+		i->next = NULL;
+		i->is_marked = 0;
+		set_active(i);
+
+		if (i->tr_cap == 0)
+		{
+			if (i->parent) set_orphan_rear(i);
+			continue;
+		}
+
+		if (i->tr_cap > 0)
+		{
+			if (!i->parent || i->is_sink)
+			{
+				i->is_sink = 0;
+				for (a=i->first; a; a=a->next)
+				{
+					j = a->head;
+					if (!j->is_marked)
+					{
+						if (j->parent == a->sister) set_orphan_rear(j);
+						if (j->parent && j->is_sink && a->r_cap > 0) set_active(j);
+					}
+				}
+				add_to_changed_list(i);
+			}
+		}
+		else
+		{
+			if (!i->parent || !i->is_sink)
+			{
+				i->is_sink = 1;
+				for (a=i->first; a; a=a->next)
+				{
+					j = a->head;
+					if (!j->is_marked)
+					{
+						if (j->parent == a->sister) set_orphan_rear(j);
+						if (j->parent && !j->is_sink && a->sister->r_cap > 0) set_active(j);
+					}
+				}
+				add_to_changed_list(i);
+			}
+		}
+		i->parent = TERMINAL;
+		i -> TS = TIME;
+		i -> DIST = 1;
+	}
+
+	//test_consistency();
+
+	/* adoption */
+	while ((np=orphan_first))
+	{
+		orphan_first = np -> next;
+		i = np -> ptr;
+		nodeptr_block -> Delete(np);
+		if (!orphan_first) orphan_last = NULL;
+		if (i->is_sink) process_sink_orphan(i);
+		else            process_source_orphan(i);
+	}
+	/* adoption end */
+
+	//test_consistency();
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::augment(arc *middle_arc)
+{
+	node *i;
+	arc *a;
+	tcaptype bottleneck;
+
+
+	/* 1. Finding bottleneck capacity */
+	/* 1a - the source tree */
+	bottleneck = middle_arc -> r_cap;
+	for (i=middle_arc->sister->head; ; i=a->head)
+	{
+		a = i -> parent;
+		if (a == TERMINAL) break;
+		if (bottleneck > a->sister->r_cap) bottleneck = a -> sister -> r_cap;
+	}
+	if (bottleneck > i->tr_cap) bottleneck = i -> tr_cap;
+	/* 1b - the sink tree */
+	for (i=middle_arc->head; ; i=a->head)
+	{
+		a = i -> parent;
+		if (a == TERMINAL) break;
+		if (bottleneck > a->r_cap) bottleneck = a -> r_cap;
+	}
+	if (bottleneck > - i->tr_cap) bottleneck = - i -> tr_cap;
+
+
+	/* 2. Augmenting */
+	/* 2a - the source tree */
+	middle_arc -> sister -> r_cap += bottleneck;
+	middle_arc -> r_cap -= bottleneck;
+	for (i=middle_arc->sister->head; ; i=a->head)
+	{
+		a = i -> parent;
+		if (a == TERMINAL) break;
+		a -> r_cap += bottleneck;
+		a -> sister -> r_cap -= bottleneck;
+		if (!a->sister->r_cap)
+		{
+			set_orphan_front(i); // add i to the beginning of the adoption list
+		}
+	}
+	i -> tr_cap -= bottleneck;
+	if (!i->tr_cap)
+	{
+		set_orphan_front(i); // add i to the beginning of the adoption list
+	}
+	/* 2b - the sink tree */
+	for (i=middle_arc->head; ; i=a->head)
+	{
+		a = i -> parent;
+		if (a == TERMINAL) break;
+		a -> sister -> r_cap += bottleneck;
+		a -> r_cap -= bottleneck;
+		if (!a->r_cap)
+		{
+			set_orphan_front(i); // add i to the beginning of the adoption list
+		}
+	}
+	i -> tr_cap += bottleneck;
+	if (!i->tr_cap)
+	{
+		set_orphan_front(i); // add i to the beginning of the adoption list
+	}
+
+
+	flow += bottleneck;
+}
+
+/***********************************************************************/
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::process_source_orphan(node *i)
+{
+	node *j;
+	arc *a0, *a0_min = NULL, *a;
+	int d, d_min = INFINITE_D;
+
+	/* trying to find a new parent */
+	for (a0=i->first; a0; a0=a0->next)
+	if (a0->sister->r_cap)
+	{
+		j = a0 -> head;
+		if (!j->is_sink && (a=j->parent))
+		{
+			/* checking the origin of j */
+			d = 0;
+			while ( 1 )
+			{
+				if (j->TS == TIME)
+				{
+					d += j -> DIST;
+					break;
+				}
+				a = j -> parent;
+				d ++;
+				if (a==TERMINAL)
+				{
+					j -> TS = TIME;
+					j -> DIST = 1;
+					break;
+				}
+				if (a==ORPHAN) { d = INFINITE_D; break; }
+				j = a -> head;
+			}
+			if (d<INFINITE_D) /* j originates from the source - done */
+			{
+				if (d<d_min)
+				{
+					a0_min = a0;
+					d_min = d;
+				}
+				/* set marks along the path */
+				for (j=a0->head; j->TS!=TIME; j=j->parent->head)
+				{
+					j -> TS = TIME;
+					j -> DIST = d --;
+				}
+			}
+		}
+	}
+
+	if (i->parent = a0_min)
+	{
+		i -> TS = TIME;
+		i -> DIST = d_min + 1;
+	}
+	else
+	{
+		/* no parent is found */
+		add_to_changed_list(i);
+
+		/* process neighbors */
+		for (a0=i->first; a0; a0=a0->next)
+		{
+			j = a0 -> head;
+			if (!j->is_sink && (a=j->parent))
+			{
+				if (a0->sister->r_cap) set_active(j);
+				if (a!=TERMINAL && a!=ORPHAN && a->head==i)
+				{
+					set_orphan_rear(j); // add j to the end of the adoption list
+				}
+			}
+		}
+	}
+}
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::process_sink_orphan(node *i)
+{
+	node *j;
+	arc *a0, *a0_min = NULL, *a;
+	int d, d_min = INFINITE_D;
+
+	/* trying to find a new parent */
+	for (a0=i->first; a0; a0=a0->next)
+	if (a0->r_cap)
+	{
+		j = a0 -> head;
+		if (j->is_sink && (a=j->parent))
+		{
+			/* checking the origin of j */
+			d = 0;
+			while ( 1 )
+			{
+				if (j->TS == TIME)
+				{
+					d += j -> DIST;
+					break;
+				}
+				a = j -> parent;
+				d ++;
+				if (a==TERMINAL)
+				{
+					j -> TS = TIME;
+					j -> DIST = 1;
+					break;
+				}
+				if (a==ORPHAN) { d = INFINITE_D; break; }
+				j = a -> head;
+			}
+			if (d<INFINITE_D) /* j originates from the sink - done */
+			{
+				if (d<d_min)
+				{
+					a0_min = a0;
+					d_min = d;
+				}
+				/* set marks along the path */
+				for (j=a0->head; j->TS!=TIME; j=j->parent->head)
+				{
+					j -> TS = TIME;
+					j -> DIST = d --;
+				}
+			}
+		}
+	}
+
+	if (i->parent = a0_min)
+	{
+		i -> TS = TIME;
+		i -> DIST = d_min + 1;
+	}
+	else
+	{
+		/* no parent is found */
+		add_to_changed_list(i);
+
+		/* process neighbors */
+		for (a0=i->first; a0; a0=a0->next)
+		{
+			j = a0 -> head;
+			if (j->is_sink && (a=j->parent))
+			{
+				if (a0->r_cap) set_active(j);
+				if (a!=TERMINAL && a!=ORPHAN && a->head==i)
+				{
+					set_orphan_rear(j); // add j to the end of the adoption list
+				}
+			}
+		}
+	}
+}
+
+/***********************************************************************/
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	flowtype Graph<captype,tcaptype,flowtype>::maxflow(bool reuse_trees, Block<node_id>* _changed_list)
+{
+	node *i, *j, *current_node = NULL;
+	arc *a;
+	nodeptr *np, *np_next;
+
+	if (!nodeptr_block)
+	{
+		nodeptr_block = new DBlock<nodeptr>(NODEPTR_BLOCK_SIZE, error_function);
+	}
+
+	changed_list = _changed_list;
+	if (maxflow_iteration == 0 && reuse_trees) { if (error_function) (*error_function)("reuse_trees cannot be used in the first call to maxflow()!"); exit(1); }
+	if (changed_list && !reuse_trees) { if (error_function) (*error_function)("changed_list cannot be used without reuse_trees!"); exit(1); }
+
+	if (reuse_trees) maxflow_reuse_trees_init();
+	else             maxflow_init();
+
+	// main loop
+	while ( 1 )
+	{
+		// test_consistency(current_node);
+
+		if ((i=current_node))
+		{
+			i -> next = NULL; /* remove active flag */
+			if (!i->parent) i = NULL;
+		}
+		if (!i)
+		{
+			if (!(i = next_active())) break;
+		}
+
+		/* growth */
+		if (!i->is_sink)
+		{
+			/* grow source tree */
+			for (a=i->first; a; a=a->next)
+			if (a->r_cap)
+			{
+				j = a -> head;
+				if (!j->parent)
+				{
+					j -> is_sink = 0;
+					j -> parent = a -> sister;
+					j -> TS = i -> TS;
+					j -> DIST = i -> DIST + 1;
+					set_active(j);
+					add_to_changed_list(j);
+				}
+				else if (j->is_sink) break;
+				else if (j->TS <= i->TS &&
+				         j->DIST > i->DIST)
+				{
+					/* heuristic - trying to make the distance from j to the source shorter */
+					j -> parent = a -> sister;
+					j -> TS = i -> TS;
+					j -> DIST = i -> DIST + 1;
+				}
+			}
+		}
+		else
+		{
+			/* grow sink tree */
+			for (a=i->first; a; a=a->next)
+			if (a->sister->r_cap)
+			{
+				j = a -> head;
+				if (!j->parent)
+				{
+					j -> is_sink = 1;
+					j -> parent = a -> sister;
+					j -> TS = i -> TS;
+					j -> DIST = i -> DIST + 1;
+					set_active(j);
+					add_to_changed_list(j);
+				}
+				else if (!j->is_sink) { a = a -> sister; break; }
+				else if (j->TS <= i->TS &&
+				         j->DIST > i->DIST)
+				{
+					/* heuristic - trying to make the distance from j to the sink shorter */
+					j -> parent = a -> sister;
+					j -> TS = i -> TS;
+					j -> DIST = i -> DIST + 1;
+				}
+			}
+		}
+
+		TIME ++;
+
+		if (a)
+		{
+			i -> next = i; /* set active flag */
+			current_node = i;
+
+			/* augmentation */
+			augment(a);
+			/* augmentation end */
+
+			/* adoption */
+			while ((np=orphan_first))
+			{
+				np_next = np -> next;
+				np -> next = NULL;
+
+				while ((np=orphan_first))
+				{
+					orphan_first = np -> next;
+					i = np -> ptr;
+					nodeptr_block -> Delete(np);
+					if (!orphan_first) orphan_last = NULL;
+					if (i->is_sink) process_sink_orphan(i);
+					else            process_source_orphan(i);
+				}
+
+				orphan_first = np_next;
+			}
+			/* adoption end */
+		}
+		else current_node = NULL;
+	}
+	// test_consistency();
+
+	if (!reuse_trees || (maxflow_iteration % 64) == 0)
+	{
+		delete nodeptr_block; 
+		nodeptr_block = NULL; 
+	}
+
+	maxflow_iteration ++;
+	return flow;
+}
+
+/***********************************************************************/
+
+
+template <typename captype, typename tcaptype, typename flowtype> 
+	void Graph<captype,tcaptype,flowtype>::test_consistency(node* current_node)
+{
+	node *i;
+	arc *a;
+	int r;
+	int num1 = 0, num2 = 0;
+
+	// test whether all nodes i with i->next!=NULL are indeed in the queue
+	for (i=nodes; i<node_last; i++)
+	{
+		if (i->next || i==current_node) num1 ++;
+	}
+	for (r=0; r<3; r++)
+	{
+		i = (r == 2) ? current_node : queue_first[r];
+		if (i)
+		for ( ; ; i=i->next)
+		{
+			num2 ++;
+			if (i->next == i)
+			{
+				if (r<2) assert(i == queue_last[r]);
+				else     assert(i == current_node);
+				break;
+			}
+		}
+	}
+	assert(num1 == num2);
+
+	for (i=nodes; i<node_last; i++)
+	{
+		// test whether all edges in seach trees are non-saturated
+		if (i->parent == NULL) {}
+		else if (i->parent == ORPHAN) {}
+		else if (i->parent == TERMINAL)
+		{
+			if (!i->is_sink) assert(i->tr_cap > 0);
+			else             assert(i->tr_cap < 0);
+		}
+		else
+		{
+			if (!i->is_sink) assert (i->parent->sister->r_cap > 0);
+			else             assert (i->parent->r_cap > 0);
+		}
+		// test whether passive nodes in search trees have neighbors in
+		// a different tree through non-saturated edges
+		if (i->parent && !i->next)
+		{
+			if (!i->is_sink)
+			{
+				assert(i->tr_cap >= 0);
+				for (a=i->first; a; a=a->next)
+				{
+					if (a->r_cap > 0) assert(a->head->parent && !a->head->is_sink);
+				}
+			}
+			else
+			{
+				assert(i->tr_cap <= 0);
+				for (a=i->first; a; a=a->next)
+				{
+					if (a->sister->r_cap > 0) assert(a->head->parent && a->head->is_sink);
+				}
+			}
+		}
+		// test marking invariants
+		if (i->parent && i->parent!=ORPHAN && i->parent!=TERMINAL)
+		{
+			assert(i->TS <= i->parent->head->TS);
+			if (i->TS == i->parent->head->TS) assert(i->DIST > i->parent->head->DIST);
+		}
+	}
+}
+
+#include "instances.inc"
diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp
index b30466f2..68fa68f4 100644
--- a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp
+++ b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp
@@ -3,12 +3,21 @@
 namespace reglib
 {
 
+bool MassRegistration::okVal(double v){return !std::isnan(v) && !(v == std::numeric_limits<double>::infinity());}
+
+bool MassRegistration::isValidPoint(pcl::PointXYZRGBNormal p){
+	return	okVal (p.x)         && okVal (p.y)			&& okVal (p.z) &&			//No nans or inf in position
+			okVal (p.normal_x)  && okVal (p.normal_y)	&& okVal (p.normal_z) &&	//No nans or inf in normal
+			!(p.x == 0			&& p.y == 0				&& p.z == 0 ) &&						//not a zero point
+			!(p.normal_x == 0	&& p.normal_y == 0		&& p.normal_z == 0);					//not a zero normal
+}
+
 MassRegistration::MassRegistration(){
 	visualizationLvl = 0;
 	nomask = true;
 	maskstep = 1;
 	nomaskstep = 100000;
-    timeout = 60;//1 minute timeout
+	timeout = 60;//1 minute timeout
 }
 MassRegistration::~MassRegistration(){}
 
@@ -27,42 +36,38 @@ MassFusionResults MassRegistration::getTransforms(std::vector<Eigen::Matrix4d> g
 	return MassFusionResults(guess,0);
 }
 
-void MassRegistration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){
+void MassRegistration::clearData(){}
+
+void MassRegistration::addModelData(Model * model, bool submodels){
+	if(submodels){
+		for(unsigned int i = 0; i < model->submodels.size(); i++){
+			addData(model->submodels[i]->getPCLnormalcloud(1,false));
+		}
+	}else{
 
+	}
+}
+
+void MassRegistration::addData(RGBDFrame* frame, ModelMask * mmask){}
+void MassRegistration::addData(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud){}
+
+void MassRegistration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	unsigned int s_nr_data = X.cols();
 	unsigned int d_nr_data = Y.cols();
 
-//printf("nr datas: %i %i\n",s_nr_data,d_nr_data);
-
 	scloud->points.clear();
 	dcloud->points.clear();
 
 	for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);}
 	for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);}
-	//printf("nr datas: %i %i\n",scloud->points.size(),dcloud->points.size());
 
-	for(unsigned int i = 0; i < s_nr_data; i++){
-		//if(i%100 == 0){printf("x: %f %f %f\n",X(0,i),X(1,i),X(2,i));}
-	}
-
-	for(unsigned int i = 0; i < d_nr_data; i++){
-		//if(i%100 == 0){printf("y: %f %f %f\n",Y(0,i),Y(1,i),Y(2,i));}
-	}
-
-viewer->removeAllPointClouds();
-viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
-viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
-viewer->spin();
-
-//viewer->removeAllPointClouds();
+	viewer->removeAllPointClouds();
+	viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+	viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+	viewer->spin();
 
-//	viewer->removeAllPointClouds();
-//	viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
-//	viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
-//	viewer->spin();
-//	viewer->removeAllPointClouds();
 }
 
 Eigen::MatrixXd MassRegistration::getMat(int rows, int cols, double * datas){
@@ -77,15 +82,20 @@ Eigen::MatrixXd MassRegistration::getMat(int rows, int cols, double * datas){
 
 
 void MassRegistration::show(std::vector<Eigen::MatrixXd> Xv, bool save, std::string filename, bool stop){
+	printf("show\n");
+	//for(unsigned int a = 0; a < Xv.size(); a++){
 	viewer->removeAllPointClouds();
 
 	srand(0);
-    for(unsigned int xi = 0; xi < Xv.size(); xi++){
+	for(unsigned int xi = 0; xi < Xv.size(); xi++){
+
 		Eigen::MatrixXd X = Xv[xi];
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-		int r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1);
-		int g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1);
-		int b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1);
+		int r,g,b;
+
+		r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1);
+		g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1);
+		b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1);
 
 		unsigned int nr_data = X.cols();
 		cloud->points.clear();
@@ -99,10 +109,17 @@ void MassRegistration::show(std::vector<Eigen::MatrixXd> Xv, bool save, std::str
 			p.r = b;
 			cloud->points.push_back(p);
 		}
+
+		//printf("cloud->points: %i\n",cloud->points.size());
+
 		char buf [1024];
 		sprintf(buf,"cloud%i",xi);
 		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud), buf);
+
 	}
+
+
+
 	if(!save){
 		viewer->spin();
 	}else{
@@ -114,6 +131,37 @@ void MassRegistration::show(std::vector<Eigen::MatrixXd> Xv, bool save, std::str
 		viewer->saveScreenshot(filename);
 	}
 	viewer->removeAllPointClouds();
+	//}
+}
+
+void MassRegistration::savePCD(std::vector<Eigen::MatrixXd> Xv, std::string path){
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	srand(0);
+	for(unsigned int xi = 0; xi < Xv.size(); xi++){
+		Eigen::MatrixXd X = Xv[xi];
+		int r,g,b;
+		r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1);
+		g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1);
+		b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1);
+
+		unsigned int nr_data = X.cols();
+		for(unsigned int i = 0; i < nr_data; i++){
+			pcl::PointXYZRGBNormal p;
+			p.x = X(0,i);
+			p.y = X(1,i);
+			p.z = X(2,i);
+			p.b = r;
+			p.g = g;
+			p.r = b;
+			cloud->points.push_back(p);
+		}
+	}
+	cloud->width = cloud->points.size();
+	cloud->height = 1;
+
+	//pcl::io::savePCDFileBinaryCompressed (cloud,path);
+	pcl::io::savePCDFileBinaryCompressed (path, *cloud);
 }
 /*
 void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){
@@ -128,11 +176,11 @@ void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){
 	scloud->points.clear();
 	dcloud->points.clear();
 	for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);}
-	for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);}		
+	for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);}
 	viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
 	viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
 	//printf("pre\n");
-    viewer->spin();
+	viewer->spin();
 	//printf("post\n");
 	viewer->removeAllPointClouds();
 
@@ -201,7 +249,7 @@ void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y, Eigen::VectorXd W)
 	viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
 	viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
 	//printf("pre spin\n");
-    viewer->spin();
+	viewer->spin();
 	//printf("post spin\n");
 	viewer->removeAllPointClouds();
 }
diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR.cpp
index 7ce5365c..91d369dd 100644
--- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR.cpp
+++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR.cpp
@@ -21,7 +21,7 @@ MassRegistrationPPR::MassRegistrationPPR(double startreg, bool visualize){
 	dwf->startreg			= startreg;
 	dwf->debugg_print		= false;
 	func					= dwf;
-	
+
 	fast_opt				= true;
 
 	nomask = true;
@@ -43,180 +43,56 @@ MassRegistrationPPR::MassRegistrationPPR(double startreg, bool visualize){
 		visualizationLvl = 0;
 	}
 
+
+	Qp_arr = new double[3*maxcount+0];
+	Qn_arr = new double[3*maxcount+0];
+	Xp_arr = new double[3*maxcount+0];
+	Xn_arr = new double[3*maxcount+0];
+	rangeW_arr = new double[maxcount+0];
 }
 MassRegistrationPPR::~MassRegistrationPPR(){
+
 	for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];}
+	for(unsigned int i = 0; i < arraynormals.size(); i++){delete[] arraynormals[i];}
+	for(unsigned int i = 0; i < arraycolors.size(); i++){delete[] arraycolors[i];}
+	for(unsigned int i = 0; i < arrayinformations.size(); i++){delete[] arrayinformations[i];}
+
 	for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];}
 	for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];}
 	delete func;
-}
-
-//double getTime(){
-//	struct timeval start1;
-//	gettimeofday(&start1, NULL);
-//	return double(start1.tv_sec+(start1.tv_usec/1000000.0));
-//}
-
-void point_to_plane3(		Eigen::Matrix3Xd & X,
-							Eigen::Matrix3Xd & Xn,
-							Eigen::Matrix3Xd & Y,
-							Eigen::Matrix3Xd & Yn,
-							Eigen::VectorXd & W){
-	typedef Eigen::Matrix<double, 6, 1> Vector6d;
-	typedef Eigen::Matrix<double, 6, 6> Matrix6d;
-
-	Matrix6d ATA;
-	Vector6d ATb;
-	ATA.setZero ();
-	ATb.setZero ();
-
-	Matrix6d ATA2;
-	Vector6d ATb2;
-	ATA2.setZero ();
-	ATb2.setZero ();
-
-	double dsum  = 0;
-	unsigned int xcols = X.cols();
-	for(unsigned int i=0; i < xcols; i++) {
-		const double & sx = X(0,i);
-		const double & sy = X(1,i);
-		const double & sz = X(2,i);
-		const double & dx = Y(0,i);
-		const double & dy = Y(1,i);
-		const double & dz = Y(2,i);
-		const double & nx = Xn(0,i);
-		const double & ny = Xn(1,i);
-		const double & nz = Xn(2,i);
-		const double & weight = W(i);
-
-		double a = nz*sy - ny*sz;
-		double b = nx*sz - nz*sx;
-		double c = ny*sx - nx*sy;
-
-		ATA.coeffRef (0) += weight * a * a;
-		ATA.coeffRef (1) += weight * a * b;
-		ATA.coeffRef (2) += weight * a * c;
-		ATA.coeffRef (3) += weight * a * nx;
-		ATA.coeffRef (4) += weight * a * ny;
-		ATA.coeffRef (5) += weight * a * nz;
-		ATA.coeffRef (7) += weight * b * b;
-		ATA.coeffRef (8) += weight * b * c;
-		ATA.coeffRef (9) += weight * b * nx;
-		ATA.coeffRef (10) += weight * b * ny;
-		ATA.coeffRef (11) += weight * b * nz;
-		ATA.coeffRef (14) += weight * c * c;
-		ATA.coeffRef (15) += weight * c * nx;
-		ATA.coeffRef (16) += weight * c * ny;
-		ATA.coeffRef (17) += weight * c * nz;
-		ATA.coeffRef (21) += weight * nx * nx;
-		ATA.coeffRef (22) += weight * nx * ny;
-		ATA.coeffRef (23) += weight * nx * nz;
-		ATA.coeffRef (28) += weight * ny * ny;
-		ATA.coeffRef (29) += weight * ny * nz;
-		ATA.coeffRef (35) += weight * nz * nz;
-
-		double d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz);
-		dsum += d;
-		ATb.coeffRef (0) += a * d;
-		ATb.coeffRef (1) += b * d;
-		ATb.coeffRef (2) += c * d;
-		ATb.coeffRef (3) += nx * d;
-		ATb.coeffRef (4) += ny * d;
-		ATb.coeffRef (5) += nz * d;
-	}
-
-	for(unsigned int i=0; i < xcols; i++) {
-		const double & sx = Y(0,i);
-		const double & sy = Y(1,i);
-		const double & sz = Y(2,i);
-		const double & dx = X(0,i);
-		const double & dy = X(1,i);
-		const double & dz = X(2,i);
-		const double & nx = Yn(0,i);
-		const double & ny = Yn(1,i);
-		const double & nz = Yn(2,i);
-		const double & weight = W(i);
-
-		double a = nz*sy - ny*sz;
-		double b = nx*sz - nz*sx;
-		double c = ny*sx - nx*sy;
-
-		ATA.coeffRef (0) += weight * a * a;
-		ATA.coeffRef (1) += weight * a * b;
-		ATA.coeffRef (2) += weight * a * c;
-		ATA.coeffRef (3) += weight * a * nx;
-		ATA.coeffRef (4) += weight * a * ny;
-		ATA.coeffRef (5) += weight * a * nz;
-		ATA.coeffRef (7) += weight * b * b;
-		ATA.coeffRef (8) += weight * b * c;
-		ATA.coeffRef (9) += weight * b * nx;
-		ATA.coeffRef (10) += weight * b * ny;
-		ATA.coeffRef (11) += weight * b * nz;
-		ATA.coeffRef (14) += weight * c * c;
-		ATA.coeffRef (15) += weight * c * nx;
-		ATA.coeffRef (16) += weight * c * ny;
-		ATA.coeffRef (17) += weight * c * nz;
-		ATA.coeffRef (21) += weight * nx * nx;
-		ATA.coeffRef (22) += weight * nx * ny;
-		ATA.coeffRef (23) += weight * nx * nz;
-		ATA.coeffRef (28) += weight * ny * ny;
-		ATA.coeffRef (29) += weight * ny * nz;
-		ATA.coeffRef (35) += weight * nz * nz;
-
-		double d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz);
-		dsum += d;
-		ATb.coeffRef (0) -= a * d;
-		ATb.coeffRef (1) -= b * d;
-		ATb.coeffRef (2) -= c * d;
-		ATb.coeffRef (3) -= nx * d;
-		ATb.coeffRef (4) -= ny * d;
-		ATb.coeffRef (5) -= nz * d;
-	}
 
+	delete[] Qp_arr;
+	delete[] Qn_arr;
+	delete[] Xp_arr;
+	delete[] Xn_arr;
+	delete[] rangeW_arr;
+}
 
+void MassRegistrationPPR::addModelData(Model * model_, bool submodels){
+	//model = model_;
+	printf("addModelData\n");
 
-	ATA.coeffRef (6) = ATA.coeff (1);
-	ATA.coeffRef (12) = ATA.coeff (2);
-	ATA.coeffRef (13) = ATA.coeff (8);
-	ATA.coeffRef (18) = ATA.coeff (3);
-	ATA.coeffRef (19) = ATA.coeff (9);
-	ATA.coeffRef (20) = ATA.coeff (15);
-	ATA.coeffRef (24) = ATA.coeff (4);
-	ATA.coeffRef (25) = ATA.coeff (10);
-	ATA.coeffRef (26) = ATA.coeff (16);
-	ATA.coeffRef (27) = ATA.coeff (22);
-	ATA.coeffRef (30) = ATA.coeff (5);
-	ATA.coeffRef (31) = ATA.coeff (11);
-	ATA.coeffRef (32) = ATA.coeff (17);
-	ATA.coeffRef (33) = ATA.coeff (23);
-	ATA.coeffRef (34) = ATA.coeff (29);
-
-	// Solve A*x = b
-	Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
-	Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)));
-
-	X = transformation*X;
-	transformation(0,3) = 0;
-	transformation(1,3) = 0;
-	transformation(2,3) = 0;
-	Xn = transformation*Xn;
-	//std::cout << x << std::endl;
-
-	//// Construct the transformation matrix from x
-	//constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
-}
+	if(submodels){
+		for(unsigned int i = 0; i < model_->submodels.size(); i++){
+			addData(model_->submodels[i]->getPCLnormalcloud(1,false));
+		}
+	}else{
+		//setData(model_->frames,model_->modelmasks);
 
-bool okVal(double v){return !std::isnan(v) && !(v == std::numeric_limits<double>::infinity());}
+//		for(unsigned int i = 0; i < model->submodels.size(); i++){
+//			addData(model->submodels[i]->getPCLnormalcloud(1,false));
+//		}
 
-bool isValidPoint(pcl::PointXYZRGBNormal p){
-	return	!okVal (p.x)		&& !okVal (p.y)			&& !okVal (p.z) &&			//No nans or inf in position
-			!okVal (p.normal_x) && !okVal (p.normal_y)	&& !okVal (p.normal_z) &&	//No nans or inf in normal
-			!(p.x == 0			&& p.y == 0				&& p.z == 0 ) &&						//not a zero point
-			!(p.normal_x == 0	&& p.normal_y == 0		&& p.normal_z == 0);					//not a zero normal
+		unsigned int nr_frames = model_->frames.size();
+		for(unsigned int i = 0; i < nr_frames; i++){
+			addData(model_->frames[i], model_->modelmasks[i]);
+		}
+	}
 }
 
 
 void MassRegistrationPPR::setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > all_clouds){
+	double total_load_time_start = getTime();
 	unsigned int nr_frames = all_clouds.size();
 
 	if(arraypoints.size() > 0){
@@ -248,19 +124,22 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBN
 	informations.resize(nr_frames);
 
 	nr_arraypoints.resize(nr_frames);
+
 	arraypoints.resize(nr_frames);
+	arraynormals.resize(nr_frames);
+	arraycolors.resize(nr_frames);
+	arrayinformations.resize(nr_frames);
+
 	trees3d.resize(nr_frames);
 	a3dv.resize(nr_frames);
 	is_ok.resize(nr_frames);
 
 	for(unsigned int i = 0; i < nr_frames; i++){
-		printf("loading data for %i\n",i);
+		//printf("loading data for %i\n",i);
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = all_clouds[i];
 		int count = 0;
 		for(unsigned int i = 0; i < cloud->points.size(); i++){
-			if (isValidPoint(cloud->points[i])){
-				count++;
-			}
+			if (isValidPoint(cloud->points[i])){count++;}
 		}
 
 		if(count < 10){
@@ -271,6 +150,10 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBN
 		}
 
 		double * ap = new double[3*count];
+		double * an = new double[3*count];
+		double * ac = new double[3*count];
+		double * ai = new double[3*count];
+
 		nr_datas[i] = count;
 		matchids[i].resize(nr_frames);
 		points[i].resize(Eigen::NoChange,count);
@@ -280,13 +163,16 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBN
 		transformed_normals[i].resize(Eigen::NoChange,count);
 
 		nr_arraypoints[i] = count;
+
 		arraypoints[i] = ap;
+		arraynormals[i] = an;
+		arraycolors[i] = ac;
+		arrayinformations[i] = ai;
 
 		Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
 		Eigen::Matrix<double, 3, Eigen::Dynamic> & C	= colors[i];
 		Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
-		Eigen::Matrix<double, 3, Eigen::Dynamic> & tX	= transformed_points[i];
-		Eigen::Matrix<double, 3, Eigen::Dynamic> & tXn	= transformed_normals[i];
+
 		Eigen::VectorXd information (count);
 
 		int c = 0;
@@ -306,6 +192,16 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBN
 				ap[3*c+1] =y;
 				ap[3*c+2] =z;
 
+				an[3*c+0] =xn;
+				an[3*c+1] =yn;
+				an[3*c+2] =zn;
+
+				ac[3*c+0] =p.r;
+				ac[3*c+1] =p.g;
+				ac[3*c+2] =p.b;
+
+				ai[c] = 1.0/(x*x+y*y+z*z);
+
 				X(0,c)	= x;
 				X(1,c)	= y;
 				X(2,c)	= z;
@@ -313,7 +209,7 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBN
 				Xn(1,c)	= yn;
 				Xn(2,c)	= zn;
 
-				information(c) = 1.0/(z*z);
+				information(c) = ai[c];//1.0/(z*z);
 				C(0,c) = p.r;
 				C(1,c) = p.g;
 				C(2,c) = p.b;
@@ -331,13 +227,16 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBN
 		trees3d[i]	= new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
 		trees3d[i]->buildIndex();
 	}
+
+	//printf("total load time:          %5.5f\n",getTime()-total_load_time_start);
 }
 
 void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<ModelMask *> mmasks_){
-	frames = frames_;
-	mmasks = mmasks_;
+	double total_load_time_start = getTime();
+	//	frames = frames_;
+	//	mmasks = mmasks_;
 
-	unsigned int nr_frames = frames.size();
+	unsigned int nr_frames = frames_.size();
 
 	if(arraypoints.size() > 0){
 		for(unsigned int i = 0; i < arraypoints.size(); i++){
@@ -357,6 +256,17 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 		}
 	}
 
+	for(unsigned int i = 0; i < nr_frames; i++){
+        //printf("loading data for %i, nomask %i\n",i,nomask);
+		addData(frames_[i], mmasks_[i]);
+	}
+	/*
+	for(unsigned int i = 0; i < nr_frames; i++){
+		printf("loading data for %i, nomask %i\n",i,nomask);
+		addData(frames[i], mmasks[i]);
+	}
+*/
+	/*
 	nr_matches.resize(nr_frames);
 	matchids.resize(nr_frames);
 	nr_datas.resize(nr_frames);
@@ -368,23 +278,24 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 	informations.resize(nr_frames);
 
 	nr_arraypoints.resize(nr_frames);
+
 	arraypoints.resize(nr_frames);
+	arraynormals.resize(nr_frames);
+	arraycolors.resize(nr_frames);
+	arrayinformations.resize(nr_frames);
+
 	trees3d.resize(nr_frames);
 	a3dv.resize(nr_frames);
 	is_ok.resize(nr_frames);
-
+*/
+	/*
 	for(unsigned int i = 0; i < nr_frames; i++){
-		printf("loading data for %i\n",i);
+		printf("loading data for %i, nomask %i\n",i,nomask);
 		bool * maskvec		= mmasks[i]->maskvec;
 		unsigned char  * rgbdata		= (unsigned char	*)(frames[i]->rgb.data);
 		unsigned short * depthdata		= (unsigned short	*)(frames[i]->depth.data);
 		float		   * normalsdata	= (float			*)(frames[i]->normals.data);
 
-//		Eigen::Matrix4d p = poses[i];
-//		float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
-//		float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
-//		float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
-
 		Camera * camera				= frames[i]->camera;
 		const unsigned int width	= camera->width;
 		const unsigned int height	= camera->height;
@@ -394,24 +305,11 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 		const float ifx				= 1.0/camera->fx;
 		const float ify				= 1.0/camera->fy;
 
-
-		int count1 = 0;
-		for(unsigned int w = 0; w < width; w++){
-			for(unsigned int h = 0; h < height; h++){
-				int ind = h*width+w;
-				if(maskvec[ind]){
-					float z = idepth*float(depthdata[ind]);
-					float xn = normalsdata[3*ind+0];
-					if(z > 0.2 && xn != 2){count1++;}
-				}
-			}
-		}
-
 		int count = 0;
 		for(unsigned int w = 0; w < width; w+=maskstep){
 			for(unsigned int h = 0; h < height; h+=maskstep){
 				int ind = h*width+w;
-				if(maskvec[ind]){
+				if(maskvec[ind] || nomask){
 					float z = idepth*float(depthdata[ind]);
 					float xn = normalsdata[3*ind+0];
 					if(z > 0.2 && xn != 2){count++;}
@@ -419,6 +317,8 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 			}
 		}
 
+		printf("count: %i\n",count);
+
 		if(count < 10){
 			is_ok[i] = false;
 			continue;
@@ -427,6 +327,10 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 		}
 
 		double * ap = new double[3*count];
+		double * an = new double[3*count];
+		double * ac = new double[3*count];
+		double * ai = new double[3*count];
+
 		nr_datas[i] = count;
 		matchids[i].resize(nr_frames);
 		points[i].resize(Eigen::NoChange,count);
@@ -436,7 +340,11 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 		transformed_normals[i].resize(Eigen::NoChange,count);
 
 		nr_arraypoints[i] = count;
+
 		arraypoints[i] = ap;
+		arraynormals[i] = an;
+		arraycolors[i] = ac;
+		arrayinformations[i] = ai;
 
 		Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
 		Eigen::Matrix<double, 3, Eigen::Dynamic> & C	= colors[i];
@@ -450,7 +358,7 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 			for(unsigned int h = 0; h < height;h+=maskstep){
 				if(c == count){continue;}
 				int ind = h*width+w;
-				if(maskvec[ind]){
+				if(maskvec[ind] || nomask){
 					float z = idepth*float(depthdata[ind]);
 					float xn = normalsdata[3*ind+0];
 
@@ -460,10 +368,21 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 
 						float x = (w - cx) * z * ifx;
 						float y = (h - cy) * z * ify;
+
 						ap[3*c+0] =x;
 						ap[3*c+1] =y;
 						ap[3*c+2] =z;
 
+						an[3*c+0] =xn;
+						an[3*c+1] =yn;
+						an[3*c+2] =zn;
+
+						ac[3*c+0] =rgbdata[3*ind+2];
+						ac[3*c+1] =rgbdata[3*ind+1];
+						ac[3*c+2] =rgbdata[3*ind+0];
+
+						ai[c] = pow(fabs(z),-1);//1.0/(z*z);
+
 						X(0,c)	= x;
 						X(1,c)	= y;
 						X(2,c)	= z;
@@ -471,7 +390,7 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 						Xn(1,c)	= yn;
 						Xn(2,c)	= zn;
 
-						information(c) = 1.0/(z*z);
+						information(c) = ai[c];//1.0/(z*z);
 						C(0,c) = rgbdata[3*ind+0];
 						C(1,c) = rgbdata[3*ind+1];
 						C(2,c) = rgbdata[3*ind+2];
@@ -489,18 +408,1235 @@ void MassRegistrationPPR::setData(std::vector<RGBDFrame*> frames_,std::vector<Mo
 		trees3d[i]	= new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
 		trees3d[i]->buildIndex();
 	}
+*/
+    //printf("total load time:          %5.5f\n",getTime()-total_load_time_start);
+}
+
+void MassRegistrationPPR::rematch(std::vector<Eigen::Matrix4d> poses, std::vector<Eigen::Matrix4d> prev_poses, bool first){
+	//printf("rematch\n");
+	double new_good_rematches = 0;
+	double new_total_rematches = 0;
+	unsigned int nr_frames = poses.size();
+
+	int rmt = 2;
+
+	if(rmt==2){
+		int ignores = 0;
+		for(unsigned int i = 0; i < nr_frames; i++){
+			if(!is_ok[i]){continue;}
+			nr_matches[i] = 0;
+
+			double * ap = arraypoints[i];
+			const unsigned int nr_ap = nr_arraypoints[i];
+
+			for(unsigned int j = 0; j < nr_frames; j++){
+				if(!is_ok[j]){continue;}
+				if(i == j){continue;}
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+				if(!first){
+					Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]);
+
+					Eigen::Affine3d diff = prev_rp.inverse()*rp;
+
+					double change_trans = 0;
+					double change_rot = 0;
+					double dt = 0;
+					for(unsigned int k = 0; k < 3; k++){
+						dt += diff(k,3)*diff(k,3);
+						for(unsigned int l = 0; l < 3; l++){
+							if(k == l){ change_rot += fabs(1-diff(k,l));}
+							else{		change_rot += fabs(diff(k,l));}
+						}
+					}
+					change_trans += sqrt(dt);
+
+					//printf("%f %f\n",change_trans,change_rot);
+
+					if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;}
+				}
+
+				const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+				const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+				const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+				std::vector<int> & matchid = matchids[i][j];
+				matchid.resize(nr_ap);
+				Tree3d * t3d = trees3d[j];
+
+				#pragma omp parallel for num_threads(8)
+				for(unsigned int k = 0; k < nr_ap; ++k) {
+					int prev = matchid[k];
+					double qp [3];
+					const double & src_x = ap[3*k+0];
+					const double & src_y = ap[3*k+1];
+					const double & src_z = ap[3*k+2];
+
+					qp[0] = m00*src_x + m01*src_y + m02*src_z + m03;
+					qp[1] = m10*src_x + m11*src_y + m12*src_z + m13;
+					qp[2] = m20*src_x + m21*src_y + m22*src_z + m23;
+
+					size_t ret_index; double out_dist_sqr;
+					nanoflann::KNNResultSet<double> resultSet(1);
+					resultSet.init(&ret_index, &out_dist_sqr );
+					t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
+
+					int current = ret_index;
+					new_good_rematches += prev != current;
+					new_total_rematches++;
+					matchid[k] = current;
+				}
+				nr_matches[i] += matchid.size();
+			}
+		}
+		//printf("ignores: %i rematches: %i\n",ignores,nr_frames*(nr_frames-1)-ignores);
+	}
+
+	if(rmt==1){
+		for(unsigned int i = 0; i < nr_frames; i++){
+			if(!is_ok[i]){continue;}
+			nr_matches[i] = 0;
+
+			double * ap = arraypoints[i];
+			const unsigned int nr_ap = nr_arraypoints[i];
+
+			for(unsigned int j = 0; j < nr_frames; j++){
+				if(!is_ok[j]){continue;}
+				if(i == j){continue;}
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+				const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+				const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+				const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+				std::vector<int> & matchid = matchids[i][j];
+				matchid.resize(nr_ap);
+				Tree3d * t3d = trees3d[j];
+
+				for(unsigned int k = 0; k < nr_ap; ++k) {
+					int prev = matchid[k];
+					double qp [3];
+					const double & src_x = ap[3*k+0];
+					const double & src_y = ap[3*k+1];
+					const double & src_z = ap[3*k+2];
+
+					qp[0] = m00*src_x + m01*src_y + m02*src_z + m03;
+					qp[1] = m10*src_x + m11*src_y + m12*src_z + m13;
+					qp[2] = m20*src_x + m21*src_y + m22*src_z + m23;
+
+					size_t ret_index; double out_dist_sqr;
+					nanoflann::KNNResultSet<double> resultSet(1);
+					resultSet.init(&ret_index, &out_dist_sqr );
+					t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
+
+					int current = ret_index;
+					new_good_rematches += prev != current;
+					new_total_rematches++;
+					matchid[k] = current;
+				}
+				nr_matches[i] += matchid.size();
+			}
+		}
+	}
+	if(rmt==0){
+		for(unsigned int i = 0; i < nr_frames; i++){
+			if(!is_ok[i]){continue;}
+			nr_matches[i] = 0;
+
+			double * ap = arraypoints[i];
+			int nr_ap = nr_arraypoints[i];
+
+			for(unsigned int j = 0; j < nr_frames; j++){
+				if(!is_ok[j]){continue;}
+				if(i == j){continue;}
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+				const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+				const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+				const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+				Eigen::Matrix<double, 3, Eigen::Dynamic> tX	= rp*points[i];
+
+				unsigned int nr_data = nr_datas[i];
+				std::vector<int> & matchid = matchids[i][j];
+				matchid.resize(nr_data);
+				Tree3d * t3d = trees3d[j];
+
+				for(unsigned int k = 0; k < nr_data; ++k) {
+					int prev = matchid[k];
+					double * qp = tX.col(k).data();
+
+					size_t ret_index; double out_dist_sqr;
+					nanoflann::KNNResultSet<double> resultSet(1);
+					resultSet.init(&ret_index, &out_dist_sqr );
+					t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
+
+					int current = ret_index;
+					new_good_rematches += prev != current;
+					new_total_rematches++;
+					matchid[k] = current;
+				}
+				nr_matches[i] += matchid.size();
+			}
+		}
+	}
+	//	good_rematches += new_good_rematches;
+	//	total_rematches += new_total_rematches
+}
+
+
+Eigen::MatrixXd MassRegistrationPPR::getAllResiduals(std::vector<Eigen::Matrix4d> poses){
+	unsigned int nr_frames = poses.size();
+	Eigen::MatrixXd all_residuals;
+
+	int total_matches = 0;
+	for(unsigned int i = 0; i < nr_frames; i++){
+		if(!is_ok[i]){continue;}
+		for(unsigned int j = 0; j < nr_frames; j++){
+			if(!is_ok[j]){continue;}
+			total_matches += matchids[i][j].size();
+		}
+	}
+
+	int all_residuals_type = 1;
+
+	if(all_residuals_type == 1){
+		switch(type) {
+		case PointToPoint:	{all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break;
+		case PointToPlane:	{all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break;
+		default:			{printf("type not set\n");}					break;
+		}
+
+		int count = 0;
+		for(unsigned int i = 0; i < nr_frames; i++){
+			if(!is_ok[i]){continue;}
+
+			double * api = arraypoints[i];
+			double * ani = arraynormals[i];
+			double * aci = arraycolors[i];
+			double * aii = arrayinformations[i];
+			const unsigned int nr_api = nr_arraypoints[i];
+			for(unsigned int j = 0; j < nr_frames; j++){
+				if(!is_ok[j]){continue;}
+				if(i == j){continue;}
+
+				double * apj = arraypoints[j];
+				double * anj = arraynormals[j];
+				double * acj = arraycolors[j];
+				double * aij = arrayinformations[j];
+				const unsigned int nr_apj = nr_arraypoints[j];
+
+				std::vector<int> & matchidi = matchids[i][j];
+				unsigned int matchesi = matchidi.size();
+
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+				const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+				const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+				const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+				if(type == PointToPlane){
+					for(unsigned int ki = 0; ki < nr_api; ++ki) {
+						int kj = matchidi[ki];
+						if( kj < 0 || kj >= nr_apj){continue;}
+						const double & src_x = api[3*ki+0];
+						const double & src_y = api[3*ki+1];
+						const double & src_z = api[3*ki+2];
+
+						const double & src_nx = ani[3*ki+0];
+						const double & src_ny = ani[3*ki+1];
+						const double & src_nz = ani[3*ki+2];
+
+						const double & info_i = aii[ki];
+
+						const double & dst_x = apj[3*kj+0];
+						const double & dst_y = apj[3*kj+1];
+						const double & dst_z = apj[3*kj+2];
+
+
+						const double & info_j = aij[kj];
+
+						float tx = m00*src_x + m01*src_y + m02*src_z + m03;
+						float ty = m10*src_x + m11*src_y + m12*src_z + m13;
+						float tz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+						float txn = m00*src_nx + m01*src_ny + m02*src_nz;
+						float tyn = m10*src_nx + m11*src_ny + m12*src_nz;
+						float tzn = m20*src_nx + m21*src_ny + m22*src_nz;
+
+						const double rw = 1.0/(1.0/info_i+1.0/info_j);
+						const double di = (txn*(tx-dst_x) + tyn*(ty-dst_y) + tzn*(tz-dst_z))*rw;
+						all_residuals(0,count) = di;
+						count++;
+					}
+				}
+
+				if(type == PointToPoint){
+					for(unsigned int ki = 0; ki < nr_api; ++ki) {
+						int kj = matchidi[ki];
+						if( kj < 0 ){continue;}
+						const double & src_x = api[3*ki+0];
+						const double & src_y = api[3*ki+1];
+						const double & src_z = api[3*ki+2];
+
+						const double & info_i = aii[ki];
+
+						const double & dst_x = apj[3*kj+0];
+						const double & dst_y = apj[3*kj+1];
+						const double & dst_z = apj[3*kj+2];
+
+						const double & info_j = aij[kj];
+
+						float tx = m00*src_x + m01*src_y + m02*src_z + m03;
+						float ty = m10*src_x + m11*src_y + m12*src_z + m13;
+						float tz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+						const double rw = 1.0/(1.0/info_i+1.0/info_j);
+						all_residuals(0,count) = (tx-dst_x)*rw;
+						all_residuals(1,count) = (ty-dst_y)*rw;
+						all_residuals(2,count) = (tz-dst_z)*rw;
+						count++;
+					}
+				}
+			}
+		}
+	}
+
+	if(all_residuals_type == 0){
+		switch(type) {
+		case PointToPoint:	{all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break;
+		case PointToPlane:	{all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break;
+		default:			{printf("type not set\n");}					break;
+		}
+
+		int count = 0;
+		for(unsigned int i = 0; i < nr_frames; i++){
+			if(!is_ok[i]){continue;}
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[i];
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[i];
+			Eigen::VectorXd & informationi					= informations[i];
+			for(unsigned int j = 0; j < nr_frames; j++){
+				if(!is_ok[j]){continue;}
+				if(i == j){continue;}
+				std::vector<int> & matchidi = matchids[i][j];
+				unsigned int matchesi = matchidi.size();
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj	= transformed_points[j];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj	= transformed_normals[j];
+				Eigen::VectorXd & informationj					= informations[j];
+				Eigen::Matrix3Xd Xp		= Eigen::Matrix3Xd::Zero(3,	matchesi);
+				Eigen::Matrix3Xd Xn		= Eigen::Matrix3Xd::Zero(3,	matchesi);
+				Eigen::Matrix3Xd Qp		= Eigen::Matrix3Xd::Zero(3,	matchesi);
+				Eigen::Matrix3Xd Qn		= Eigen::Matrix3Xd::Zero(3,	matchesi);
+				Eigen::VectorXd  rangeW	= Eigen::VectorXd::Zero(	matchesi);
+
+				for(unsigned int ki = 0; ki < matchesi; ki++){
+					int kj = matchidi[ki];
+					if( ki >= Qp.cols() || kj < 0 || kj >= tXj.cols() ){continue;}
+					Qp.col(ki) = tXj.col(kj);
+					Qn.col(ki) = tXnj.col(kj);
+					Xp.col(ki) = tXi.col(ki);
+					Xn.col(ki) = tXni.col(ki);
+					rangeW(ki) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj));
+				}
+				Eigen::MatrixXd residuals;
+				switch(type) {
+				case PointToPoint:	{residuals = Xp-Qp;} 						break;
+				case PointToPlane:	{
+					residuals		= Eigen::MatrixXd::Zero(1,	Xp.cols());
+					for(int i=0; i<Xp.cols(); ++i) {
+						float dx = Xp(0,i)-Qp(0,i);
+						float dy = Xp(1,i)-Qp(1,i);
+						float dz = Xp(2,i)-Qp(2,i);
+						float qx = Qn(0,i);
+						float qy = Qn(1,i);
+						float qz = Qn(2,i);
+						float di = qx*dx + qy*dy + qz*dz;
+						residuals(0,i) = di;
+					}
+				}break;
+				default:			{printf("type not set\n");}					break;
+				}
+				for(unsigned int k=0; k < matchesi; ++k) {residuals.col(k) *= rangeW(k);}
+				all_residuals.block(0,count,residuals.rows(),residuals.cols()) = residuals;
+				count += residuals.cols();
+			}
+		}
+	}
+	return all_residuals;
+}
+
+void MassRegistrationPPR::clearData(){}
+
+void MassRegistrationPPR::addData(RGBDFrame* frame, ModelMask * mmask){
+	double total_load_time_start = getTime();
+	frames.push_back(frame);
+	mmasks.push_back(mmask);
+
+	nr_matches.push_back(	0);
+	matchids.push_back(		std::vector< std::vector<int> >() );
+	nr_datas.push_back(		0);
+
+	points.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	colors.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	normals.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_points.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_normals.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+
+	informations.push_back(			Eigen::VectorXd());
+
+	nr_arraypoints.push_back(0);
+
+	arraypoints.push_back(0);
+	arraynormals.push_back(0);
+	arraycolors.push_back(0);
+	arrayinformations.push_back(0);
+
+	trees3d.push_back(0);
+	a3dv.push_back(0);
+	is_ok.push_back(false);
+
+	unsigned int i = frames.size()-1;
+	//printf("loading data for %i, nomask %i\n",i,nomask);
+	bool * maskvec		= mmasks[i]->maskvec;
+	unsigned char  * rgbdata		= (unsigned char	*)(frames[i]->rgb.data);
+	unsigned short * depthdata		= (unsigned short	*)(frames[i]->depth.data);
+	float		   * normalsdata	= (float			*)(frames[i]->normals.data);
+
+	Camera * camera				= frames[i]->camera;
+	const unsigned int width	= camera->width;
+	const unsigned int height	= camera->height;
+	const float idepth			= camera->idepth_scale;
+	const float cx				= camera->cx;
+	const float cy				= camera->cy;
+	const float ifx				= 1.0/camera->fx;
+	const float ify				= 1.0/camera->fy;
+
+	int count = 0;
+	for(unsigned int w = 0; w < width; w+=maskstep){
+		for(unsigned int h = 0; h < height; h+=maskstep){
+			int ind = h*width+w;
+			if(maskvec[ind] || nomask){
+				float z = idepth*float(depthdata[ind]);
+				float xn = normalsdata[3*ind+0];
+				if(z > 0.2 && xn != 2){count++;}
+			}
+		}
+	}
+
+//	printf("count: %i\n",count);
+
+	if(count < 10){
+		is_ok[i] = false;
+		return;
+	}else{
+		is_ok[i] = true;
+	}
+
+	double * ap = new double[3*count];
+	double * an = new double[3*count];
+	double * ac = new double[3*count];
+	double * ai = new double[3*count];
+
+	nr_datas[i] = count;
+	//matchids[i].resize(nr_frames);
+	points[i].resize(Eigen::NoChange,count);
+	colors[i].resize(Eigen::NoChange,count);
+	normals[i].resize(Eigen::NoChange,count);
+	transformed_points[i].resize(Eigen::NoChange,count);
+	transformed_normals[i].resize(Eigen::NoChange,count);
+
+	nr_arraypoints[i] = count;
+
+	arraypoints[i] = ap;
+	arraynormals[i] = an;
+	arraycolors[i] = ac;
+	arrayinformations[i] = ai;
+
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & C	= colors[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & tX	= transformed_points[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & tXn	= transformed_normals[i];
+	Eigen::VectorXd information (count);
+
+	int c = 0;
+	for(unsigned int w = 0; w < width; w+=maskstep){
+		for(unsigned int h = 0; h < height;h+=maskstep){
+			if(c == count){continue;}
+			int ind = h*width+w;
+			if(maskvec[ind] || nomask){
+				float z = idepth*float(depthdata[ind]);
+				float xn = normalsdata[3*ind+0];
+
+				if(z > 0.2 && xn != 2){
+					float yn = normalsdata[3*ind+1];
+					float zn = normalsdata[3*ind+2];
+
+					float x = (w - cx) * z * ifx;
+					float y = (h - cy) * z * ify;
+
+					ap[3*c+0] =x;
+					ap[3*c+1] =y;
+					ap[3*c+2] =z;
+
+					an[3*c+0] =xn;
+					an[3*c+1] =yn;
+					an[3*c+2] =zn;
+
+					ac[3*c+0] =rgbdata[3*ind+2];
+					ac[3*c+1] =rgbdata[3*ind+1];
+					ac[3*c+2] =rgbdata[3*ind+0];
+
+					ai[c] = pow(fabs(z),-2);//1.0/(z*z);
+
+					X(0,c)	= x;
+					X(1,c)	= y;
+					X(2,c)	= z;
+					Xn(0,c)	= xn;
+					Xn(1,c)	= yn;
+					Xn(2,c)	= zn;
+
+					information(c) = ai[c];//1.0/(z*z);
+					C(0,c) = rgbdata[3*ind+0];
+					C(1,c) = rgbdata[3*ind+1];
+					C(2,c) = rgbdata[3*ind+2];
+					c++;
+				}
+			}
+		}
+	}
+	informations[i] = information;
+
+	ArrayData3D<double> * a3d = new ArrayData3D<double>;
+	a3d->data	= ap;
+	a3d->rows	= count;
+	a3dv[i]		= a3d;
+	trees3d[i]	= new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+	trees3d[i]->buildIndex();
+	//printf("total load time:          %5.5f\n",getTime()-total_load_time_start);
+}
+
+void MassRegistrationPPR::addData(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud){
+	double total_load_time_start = getTime();
+
+	nr_matches.push_back(	0);
+	matchids.push_back(		std::vector< std::vector<int> >() );
+	nr_datas.push_back(		0);
+
+	points.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	colors.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	normals.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_points.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_normals.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+
+	informations.push_back(			Eigen::VectorXd());
+
+	nr_arraypoints.push_back(0);
+
+	arraypoints.push_back(0);
+	arraynormals.push_back(0);
+	arraycolors.push_back(0);
+	arrayinformations.push_back(0);
+
+	trees3d.push_back(0);
+	a3dv.push_back(0);
+	is_ok.push_back(false);
+
+	unsigned int i = points.size()-1;
+
+	int count = 0;
+	for(unsigned int j = 0; j < cloud->points.size(); j+=maskstep){
+		if (isValidPoint(cloud->points[j])){count++;}
+	}
+
+	printf("count: %i\n",count);
+
+	if(count < 10){
+		is_ok[i] = false;
+		return;
+	}else{
+		is_ok[i] = true;
+	}
+
+	double * ap = new double[3*count];
+	double * an = new double[3*count];
+	double * ac = new double[3*count];
+	double * ai = new double[3*count];
+
+	nr_datas[i] = count;
+	points[i].resize(Eigen::NoChange,count);
+	colors[i].resize(Eigen::NoChange,count);
+	normals[i].resize(Eigen::NoChange,count);
+	transformed_points[i].resize(Eigen::NoChange,count);
+	transformed_normals[i].resize(Eigen::NoChange,count);
+
+	nr_arraypoints[i] = count;
+
+	arraypoints[i] = ap;
+	arraynormals[i] = an;
+	arraycolors[i] = ac;
+	arrayinformations[i] = ai;
+
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & C	= colors[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
+
+	Eigen::VectorXd information (count);
+
+	int c = 0;
+	for(unsigned int j = 0; j < cloud->points.size(); j+=maskstep){
+		pcl::PointXYZRGBNormal p = cloud->points[j];
+		if (isValidPoint(p)){
+
+			float xn = p.normal_x;
+			float yn = p.normal_y;
+			float zn = p.normal_z;
+
+			float x = p.x;
+			float y = p.y;
+			float z = p.z;
+
+			//if(j % 1000 == 0){printf("%i -> %f %f %f\n",j,x,y,z);}
+
+			ap[3*c+0] =x;
+			ap[3*c+1] =y;
+			ap[3*c+2] =z;
+
+			an[3*c+0] =xn;
+			an[3*c+1] =yn;
+			an[3*c+2] =zn;
+
+			ac[3*c+0] =p.r;
+			ac[3*c+1] =p.g;
+			ac[3*c+2] =p.b;
+
+			ai[c] = 1/sqrt(x*x+y*y+z*z);
+
+			X(0,c)	= x;
+			X(1,c)	= y;
+			X(2,c)	= z;
+			Xn(0,c)	= xn;
+			Xn(1,c)	= yn;
+			Xn(2,c)	= zn;
+
+			information(c) = ai[c];//1/sqrt(2+x*x+y*y+z*z);//1.0/(z*z);
+			C(0,c) = p.r;
+			C(1,c) = p.g;
+			C(2,c) = p.b;
+			c++;
+
+		}
+	}
+
+	informations[i] = information;
+
+	ArrayData3D<double> * a3d = new ArrayData3D<double>;
+	a3d->data	= ap;
+	a3d->rows	= count;
+	a3dv[i]		= a3d;
+	trees3d[i]	= new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+	trees3d[i]->buildIndex();
+	if(visualizationLvl > 0){
+		printf("total load time:          %5.5f\n",getTime()-total_load_time_start);
+	}
+}
+
+std::vector<Eigen::Matrix4d> MassRegistrationPPR::optimize(std::vector<Eigen::Matrix4d> poses){
+	bool onetoone = true;
+	unsigned int nr_frames = poses.size();
+	Eigen::MatrixXd Xo1;
+
+	int optt = 2;
+	for(int outer=0; outer < 30; ++outer) {
+		if(getTime()-total_time_start > timeout){break;}
+
+		//printf("outer: %i\n",outer);
+
+		std::vector<Eigen::Matrix4d> poses2 = poses;
+		if(type == PointToPlane && optt == 2){
+			for(unsigned int i = 0; i < nr_frames; i++){
+				if(getTime()-total_time_start > timeout){break;}
+				if(!is_ok[i]){continue;}
+
+				unsigned int count = 0;
+				double * api = arraypoints[i];
+				double * ani = arraynormals[i];
+				double * aci = arraycolors[i];
+				double * aii = arrayinformations[i];
+				const unsigned int nr_api = nr_arraypoints[i];
+
+				Eigen::Affine3d rpi = Eigen::Affine3d(poses[i]);
+
+				for(unsigned int j = 0; j < nr_frames; j++){
+					if(!is_ok[j]){continue;}
+					if(i == j){continue;}
+
+					double * apj = arraypoints[j];
+					double * anj = arraynormals[j];
+					double * acj = arraycolors[j];
+					double * aij = arrayinformations[j];
+					const unsigned int nr_apj = nr_arraypoints[j];
+
+					std::vector<int> & matchidj = matchids[j][i];
+					unsigned int matchesj = matchidj.size();
+					std::vector<int> & matchidi = matchids[i][j];
+					unsigned int matchesi = matchidi.size();
+
+
+
+					Eigen::Affine3d rpj = Eigen::Affine3d(poses[j]);
+					const double & mj00 = rpj(0,0); const double & mj01 = rpj(0,1); const double & mj02 = rpj(0,2); const double & mj03 = rpj(0,3);
+					const double & mj10 = rpj(1,0); const double & mj11 = rpj(1,1); const double & mj12 = rpj(1,2); const double & mj13 = rpj(1,3);
+					const double & mj20 = rpj(2,0); const double & mj21 = rpj(2,1); const double & mj22 = rpj(2,2); const double & mj23 = rpj(2,3);
+
+					for(unsigned int ki = 0; ki < matchesi; ki++){
+						int kj = matchidi[ki];
+						if( kj == -1 ){continue;}
+						if( kj >=  matchesj){continue;}
+						if(!onetoone || matchidj[kj] == ki){
+							Xp_arr[3*count+0] = api[3*ki+0];
+							Xp_arr[3*count+1] = api[3*ki+1];
+							Xp_arr[3*count+2] = api[3*ki+2];
+
+							Xn_arr[3*count+0] = ani[3*ki+0];
+							Xn_arr[3*count+1] = ani[3*ki+1];
+							Xn_arr[3*count+2] = ani[3*ki+2];
+
+							const double & info_i = aii[ki];
+
+							const double & dst_x = apj[3*kj+0];
+							const double & dst_y = apj[3*kj+1];
+							const double & dst_z = apj[3*kj+2];
+
+							const double & dst_nx = anj[3*kj+0];
+							const double & dst_ny = anj[3*kj+1];
+							const double & dst_nz = anj[3*kj+2];
+
+							const double & info_j = aij[kj];
+
+							Qp_arr[3*count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03;
+							Qp_arr[3*count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13;
+							Qp_arr[3*count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23;
+
+							Qn_arr[3*count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz;
+							Qn_arr[3*count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz;
+							Qn_arr[3*count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz;
+
+							rangeW_arr[count] = 1.0/(1.0/info_i+1.0/info_j);
+
+							count++;
+						}
+					}
+				}
+
+				if(count == 0){break;}
+
+				typedef Eigen::Matrix<double, 6, 1> Vector6d;
+				typedef Eigen::Matrix<double, 6, 6> Matrix6d;
+
+				Matrix6d ATA;
+				Vector6d ATb;
+
+				//std::vector<Eigen::MatrixXd> Xv;
+				//if(visualizationLvl == 4){for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);}
+
+
+
+				Eigen::Affine3d p = rpi;
+				bool do_inner = true;
+				for(int inner=0; inner < 15 && do_inner; ++inner) {
+
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+					ATA.setZero ();
+					ATb.setZero ();
+					const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3);
+					const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3);
+					const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3);
+					for(unsigned int co = 0; co < count; co++){
+						const double & src_x = Xp_arr[3*co+0];
+						const double & src_y = Xp_arr[3*co+1];
+						const double & src_z = Xp_arr[3*co+2];
+						const double & src_nx = Xn_arr[3*co+0];
+						const double & src_ny = Xn_arr[3*co+1];
+						const double & src_nz = Xn_arr[3*co+2];
+
+						const double & dx = Qp_arr[3*co+0];
+						const double & dy = Qp_arr[3*co+1];
+						const double & dz = Qp_arr[3*co+2];
+						const double & dnx = Qn_arr[3*co+0];
+						const double & dny = Qn_arr[3*co+1];
+						const double & dnz = Qn_arr[3*co+2];
+
+						const double & rw = rangeW_arr[co];
+
+						const double & sx = m00*src_x + m01*src_y + m02*src_z + m03;
+						const double & sy = m10*src_x + m11*src_y + m12*src_z + m13;
+						const double & sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+						const double & nx = m00*src_nx + m01*src_ny + m02*src_nz;
+						const double & ny = m10*src_nx + m11*src_ny + m12*src_nz;
+						const double & nz = m20*src_nx + m21*src_ny + m22*src_nz;
+
+						const double & angle = nx*dnx+ny*dny+nz*dnz;
+						//printf("%f\n",angle);
+						//if(angle < 0){exit(0);continue;}
+
+						if(angle < 0){continue;}
+
+						double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz));
+						//double weight = angle*angle*angle*angle*func->getProb(di)*rw*rw;
+						double prob = func->getProb(di);
+						double weight = prob*rw*rw;
+
+						if(visualizationLvl == 5){
+							pcl::PointXYZRGBNormal p;
+							p.x = sx;
+							p.y = sy;
+							p.z = sz;
+							p.b = 0;
+							p.g = 255;
+							p.r = 0;
+							scloud->points.push_back(p);
+
+							pcl::PointXYZRGBNormal p1;
+							p1.x = dx;
+							p1.y = dy;
+							p1.z = dz;
+							p1.b = 255.0*prob;
+							p1.g = 255.0*prob;
+							p1.r = 255.0*prob;
+							dcloud->points.push_back(p1);
+						}
+
+						const double & a = nz*sy - ny*sz;
+						const double & b = nx*sz - nz*sx;
+						const double & c = ny*sx - nx*sy;
+
+						ATA.coeffRef (0) += weight * a * a;
+						ATA.coeffRef (1) += weight * a * b;
+						ATA.coeffRef (2) += weight * a * c;
+						ATA.coeffRef (3) += weight * a * nx;
+						ATA.coeffRef (4) += weight * a * ny;
+						ATA.coeffRef (5) += weight * a * nz;
+						ATA.coeffRef (7) += weight * b * b;
+						ATA.coeffRef (8) += weight * b * c;
+						ATA.coeffRef (9) += weight * b * nx;
+						ATA.coeffRef (10) += weight * b * ny;
+						ATA.coeffRef (11) += weight * b * nz;
+						ATA.coeffRef (14) += weight * c * c;
+						ATA.coeffRef (15) += weight * c * nx;
+						ATA.coeffRef (16) += weight * c * ny;
+						ATA.coeffRef (17) += weight * c * nz;
+						ATA.coeffRef (21) += weight * nx * nx;
+						ATA.coeffRef (22) += weight * nx * ny;
+						ATA.coeffRef (23) += weight * nx * nz;
+						ATA.coeffRef (28) += weight * ny * ny;
+						ATA.coeffRef (29) += weight * ny * nz;
+						ATA.coeffRef (35) += weight * nz * nz;
+
+						const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz);
+
+						ATb.coeffRef (0) += a * d;
+						ATb.coeffRef (1) += b * d;
+						ATb.coeffRef (2) += c * d;
+						ATb.coeffRef (3) += nx * d;
+						ATb.coeffRef (4) += ny * d;
+						ATb.coeffRef (5) += nz * d;
+					}
+
+					ATA.coeffRef (6) = ATA.coeff (1);
+					ATA.coeffRef (12) = ATA.coeff (2);
+					ATA.coeffRef (13) = ATA.coeff (8);
+					ATA.coeffRef (18) = ATA.coeff (3);
+					ATA.coeffRef (19) = ATA.coeff (9);
+					ATA.coeffRef (20) = ATA.coeff (15);
+					ATA.coeffRef (24) = ATA.coeff (4);
+					ATA.coeffRef (25) = ATA.coeff (10);
+					ATA.coeffRef (26) = ATA.coeff (16);
+					ATA.coeffRef (27) = ATA.coeff (22);
+					ATA.coeffRef (30) = ATA.coeff (5);
+					ATA.coeffRef (31) = ATA.coeff (11);
+					ATA.coeffRef (32) = ATA.coeff (17);
+					ATA.coeffRef (33) = ATA.coeff (23);
+					ATA.coeffRef (34) = ATA.coeff (29);
+
+					// Solve A*x = b
+					Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
+					Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)));
+					p = transformation*p;
+
+					double change_t = 0;
+					double change_r = 0;
+					for(unsigned int k = 0; k < 3; k++){
+						change_t += transformation(k,3)*transformation(k,3);
+						for(unsigned int l = 0; l < 3; l++){
+							if(k == l){ change_r += fabs(1-transformation(k,l));}
+							else{		change_r += fabs(transformation(k,l));}
+						}
+					}
+					change_t = sqrt(change_t);
+
+					if(visualizationLvl == 5){
+						viewer->removeAllPointClouds();
+						viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+						viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+						viewer->spin();
+					}
+
+					if(change_t < stopval && change_r < stopval){do_inner = false;}
+				}
+
+
+				//new_opt_time += getTime()-new_opt_start;
+				//std::cout << p.matrix() << std::endl;
+				//Eigen::Matrix4d newpose = p.matrix()*poses[i];
+				poses[i] = p.matrix();//newpose;
+			}
+		}else if(type == PointToPlane && optt == 1){
+			for(unsigned int i = 0; i < nr_frames; i++){
+				if(getTime()-total_time_start > timeout){break;}
+				if(!is_ok[i]){continue;}
+				unsigned int count = 0;
+
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi    = transformed_points[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi     = points[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni   = transformed_normals[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni    = normals[i];
+				Eigen::VectorXd & informationi                    = informations[i];
+
+
+				for(unsigned int j = 0; j < nr_frames; j++){
+					if(!is_ok[j]){continue;}
+					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj    = transformed_points[j];
+					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj   = transformed_normals[j];
+					Eigen::VectorXd & informationj                    = informations[j];
+
+					std::vector<int> & matchidj = matchids[j][i];
+					unsigned int matchesj = matchidj.size();
+					std::vector<int> & matchidi = matchids[i][j];
+					unsigned int matchesi = matchidi.size();
+
+					for(unsigned int ki = 0; ki < matchesi; ki++){
+						int kj = matchidi[ki];
+						if( kj == -1 ){continue;}
+						if( kj >=  matchesj){continue;}
+						if(!onetoone || matchidj[kj] == ki){
+							Qp_arr[3*count+0] = tXj(0,kj);
+							Qp_arr[3*count+1] = tXj(1,kj);
+							Qp_arr[3*count+2] = tXj(2,kj);
+							Qn_arr[3*count+0] = tXnj(0,kj);
+							Qn_arr[3*count+1] = tXnj(1,kj);
+							Qn_arr[3*count+2] = tXnj(2,kj);
+
+							Xp_arr[3*count+0] = tXi(0,ki);
+							Xp_arr[3*count+1] = tXi(1,ki);
+							Xp_arr[3*count+2] = tXi(2,ki);
+							Xn_arr[3*count+0] = tXni(0,ki);
+							Xn_arr[3*count+1] = tXni(1,ki);
+							Xn_arr[3*count+2] = tXni(2,ki);
+
+							rangeW_arr[count] = 1.0/(1.0/informationi(ki)+1.0/informationj(kj));
+							count++;
+						}
+					}
+				}
+
+				if(count == 0){break;}
+
+				typedef Eigen::Matrix<double, 6, 1> Vector6d;
+				typedef Eigen::Matrix<double, 6, 6> Matrix6d;
+
+				Matrix6d ATA;
+				Vector6d ATb;
+
+
+				Eigen::Affine3d p = Eigen::Affine3d::Identity();
+				bool do_inner = true;
+				for(int inner=0; inner < 5 && do_inner; ++inner) {
+					ATA.setZero ();
+					ATb.setZero ();
+					const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3);
+					const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3);
+					const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3);
+					for(unsigned int co = 0; co < count; co++){
+						const double & src_x = Xp_arr[3*co+0];
+						const double & src_y = Xp_arr[3*co+1];
+						const double & src_z = Xp_arr[3*co+2];
+						const double & src_nx = Xn_arr[3*co+0];
+						const double & src_ny = Xn_arr[3*co+1];
+						const double & src_nz = Xn_arr[3*co+2];
+
+						const double & dx = Qp_arr[3*co+0];
+						const double & dy = Qp_arr[3*co+1];
+						const double & dz = Qp_arr[3*co+2];
+						const double & dnx = Qn_arr[3*co+0];
+						const double & dny = Qn_arr[3*co+1];
+						const double & dnz = Qn_arr[3*co+2];
+
+						const double & rw = rangeW_arr[co];
+
+						const double & sx = m00*src_x + m01*src_y + m02*src_z + m03;
+						const double & sy = m10*src_x + m11*src_y + m12*src_z + m13;
+						const double & sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+						const double & nx = m00*src_nx + m01*src_ny + m02*src_nz;
+						const double & ny = m10*src_nx + m11*src_ny + m12*src_nz;
+						const double & nz = m20*src_nx + m21*src_ny + m22*src_nz;
+
+						if(nx*dnx+ny*dny+nz*dnz < 0){continue;}
+
+						double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz));
+						double weight = func->getProb(di)*rw*rw;
+
+						const double & a = nz*sy - ny*sz;
+						const double & b = nx*sz - nz*sx;
+						const double & c = ny*sx - nx*sy;
+
+						ATA.coeffRef (0) += weight * a * a;
+						ATA.coeffRef (1) += weight * a * b;
+						ATA.coeffRef (2) += weight * a * c;
+						ATA.coeffRef (3) += weight * a * nx;
+						ATA.coeffRef (4) += weight * a * ny;
+						ATA.coeffRef (5) += weight * a * nz;
+						ATA.coeffRef (7) += weight * b * b;
+						ATA.coeffRef (8) += weight * b * c;
+						ATA.coeffRef (9) += weight * b * nx;
+						ATA.coeffRef (10) += weight * b * ny;
+						ATA.coeffRef (11) += weight * b * nz;
+						ATA.coeffRef (14) += weight * c * c;
+						ATA.coeffRef (15) += weight * c * nx;
+						ATA.coeffRef (16) += weight * c * ny;
+						ATA.coeffRef (17) += weight * c * nz;
+						ATA.coeffRef (21) += weight * nx * nx;
+						ATA.coeffRef (22) += weight * nx * ny;
+						ATA.coeffRef (23) += weight * nx * nz;
+						ATA.coeffRef (28) += weight * ny * ny;
+						ATA.coeffRef (29) += weight * ny * nz;
+						ATA.coeffRef (35) += weight * nz * nz;
+
+						const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz);
+
+						ATb.coeffRef (0) += a * d;
+						ATb.coeffRef (1) += b * d;
+						ATb.coeffRef (2) += c * d;
+						ATb.coeffRef (3) += nx * d;
+						ATb.coeffRef (4) += ny * d;
+						ATb.coeffRef (5) += nz * d;
+					}
+
+					ATA.coeffRef (6) = ATA.coeff (1);
+					ATA.coeffRef (12) = ATA.coeff (2);
+					ATA.coeffRef (13) = ATA.coeff (8);
+					ATA.coeffRef (18) = ATA.coeff (3);
+					ATA.coeffRef (19) = ATA.coeff (9);
+					ATA.coeffRef (20) = ATA.coeff (15);
+					ATA.coeffRef (24) = ATA.coeff (4);
+					ATA.coeffRef (25) = ATA.coeff (10);
+					ATA.coeffRef (26) = ATA.coeff (16);
+					ATA.coeffRef (27) = ATA.coeff (22);
+					ATA.coeffRef (30) = ATA.coeff (5);
+					ATA.coeffRef (31) = ATA.coeff (11);
+					ATA.coeffRef (32) = ATA.coeff (17);
+					ATA.coeffRef (33) = ATA.coeff (23);
+					ATA.coeffRef (34) = ATA.coeff (29);
+
+					// Solve A*x = b
+					Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
+					Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)));
+					p = transformation*p;
+
+					double change_t = 0;
+					double change_r = 0;
+					for(unsigned int k = 0; k < 3; k++){
+						change_t += transformation(k,3)*transformation(k,3);
+						for(unsigned int l = 0; l < 3; l++){
+							if(k == l){ change_r += fabs(1-transformation(k,l));}
+							else{		change_r += fabs(transformation(k,l));}
+						}
+					}
+					change_t = sqrt(change_t);
+					if(change_t < stopval && change_r < stopval){do_inner = false;}
+				}
+
+				//new_opt_time += getTime()-new_opt_start;
+				//std::cout << p.matrix() << std::endl;
+				Eigen::Matrix4d newpose = p.matrix()*poses[i];
+				poses[i] = newpose;
+			}
+		}else{
+
+			for(unsigned int i = 0; i < nr_frames; i++){
+				if(getTime()-total_time_start > timeout){break;}
+				if(!is_ok[i]){continue;}
+				unsigned int nr_match = 0;
+				{
+					for(unsigned int j = 0; j < nr_frames; j++){
+						if(!is_ok[j]){continue;}
+						std::vector<int> & matchidj = matchids[j][i];
+						unsigned int matchesj = matchidj.size();
+						std::vector<int> & matchidi = matchids[i][j];
+						unsigned int matchesi = matchidi.size();
+
+						for(unsigned int ki = 0; ki < matchesi; ki++){
+							int kj = matchidi[ki];
+							if( kj == -1 ){continue;}
+							if( kj >=  matchesj){continue;}
+							if(!onetoone || matchidj[kj] == ki){	nr_match++;}
+						}
+					}
+				}
+
+				Eigen::Matrix3Xd Xp		= Eigen::Matrix3Xd::Zero(3,	nr_match);
+				Eigen::Matrix3Xd Xp_ori	= Eigen::Matrix3Xd::Zero(3,	nr_match);
+				Eigen::Matrix3Xd Xn		= Eigen::Matrix3Xd::Zero(3,	nr_match);
+
+				Eigen::Matrix3Xd Qp		= Eigen::Matrix3Xd::Zero(3,	nr_match);
+				Eigen::Matrix3Xd Qn		= Eigen::Matrix3Xd::Zero(3,	nr_match);
+				Eigen::VectorXd  rangeW	= Eigen::VectorXd::Zero(	nr_match);
+
+				int count = 0;
+
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi	= points[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni	= normals[i];
+				Eigen::VectorXd & informationi					= informations[i];
+
+				for(unsigned int j = 0; j < nr_frames; j++){
+					if(!is_ok[j]){continue;}
+					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj	= transformed_points[j];
+					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj	= transformed_normals[j];
+					Eigen::VectorXd & informationj					= informations[j];
+
+					std::vector<int> & matchidj = matchids[j][i];
+					unsigned int matchesj = matchidj.size();
+					std::vector<int> & matchidi = matchids[i][j];
+					unsigned int matchesi = matchidi.size();
+
+					for(unsigned int ki = 0; ki < matchesi; ki++){
+						int kj = matchidi[ki];
+						if( kj == -1 ){continue;}
+						if( kj >=  matchesj){continue;}
+						if(!onetoone || matchidj[kj] == ki){
+							Qp.col(count) = tXj.col(kj);
+							Qn.col(count) = tXnj.col(kj);
+
+							Xp_ori.col(count) = Xi.col(ki);
+							Xp.col(count) = tXi.col(ki);
+
+							Xn.col(count) = tXni.col(ki);
+							rangeW(count) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj));
+							count++;
+						}
+					}
+				}
+
+				if(count == 0){break;}
+
+				//showMatches(Xp,Qp);
+				for(int inner=0; inner < 5; ++inner) {
+					Eigen::MatrixXd residuals;
+					switch(type) {
+					case PointToPoint:	{residuals = Xp-Qp;} 						break;
+					case PointToPlane:	{
+						residuals		= Eigen::MatrixXd::Zero(1,	Xp.cols());
+						for(int i=0; i<Xp.cols(); ++i) {
+							float dx = Xp(0,i)-Qp(0,i);
+							float dy = Xp(1,i)-Qp(1,i);
+							float dz = Xp(2,i)-Qp(2,i);
+							float qx = Qn(0,i);
+							float qy = Qn(1,i);
+							float qz = Qn(2,i);
+							float di = qx*dx + qy*dy + qz*dz;
+							residuals(0,i) = di;
+						}
+					}break;
+					default:			{printf("type not set\n");}					break;
+					}
+					for(unsigned int k=0; k < nr_match; ++k) {residuals.col(k) *= rangeW(k);}
+
+					Eigen::VectorXd  W;
+					switch(type) {
+					case PointToPoint:	{W = func->getProbs(residuals); } 					break;
+					case PointToPlane:	{
+						W = func->getProbs(residuals);
+						for(int k=0; k<nr_match; ++k) {W(k) = W(k)*float((Xn(0,k)*Qn(0,k) + Xn(1,k)*Qn(1,k) + Xn(2,k)*Qn(2,k)) > 0.0);}
+					}	break;
+					default:			{printf("type not set\n");} break;
+					}
+
+					W = W.array()*rangeW.array()*rangeW.array();
+					Xo1 = Xp;
+					switch(type) {
+					case PointToPoint:	{
+						//RigidMotionEstimator::point_to_point(Xp, Qp, W);
+						pcl::TransformationFromCorrespondences tfc1;
+						for(unsigned int c = 0; c < nr_match; c++){tfc1.add(Eigen::Vector3f(Xp(0,c), Xp(1,c),Xp(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));}
+						Eigen::Affine3d rot = tfc1.getTransformation().cast<double>();
+						Xp = rot*Xp;
+						Xn = rot.rotation()*Xn;
+					}		break;
+					case PointToPlane:	{
+						point_to_plane2(Xp, Xn, Qp, Qn, W);
+					}	break;
+					default:  			{printf("type not set\n"); } break;
+					}
+
+					double stop1 = (Xp-Xo1).colwise().norm().maxCoeff();
+					Xo1 = Xp;
+					if(stop1 < 0.001){break; }
+				}
+				//exit(0);
+				pcl::TransformationFromCorrespondences tfc;
+				for(unsigned int c = 0; c < nr_match; c++){tfc.add(Eigen::Vector3f(Xp_ori(0,c),Xp_ori(1,c),Xp_ori(2,c)),Eigen::Vector3f(Xp(0,c),Xp(1,c),Xp(2,c)));}
+				poses[i] = tfc.getTransformation().cast<double>().matrix();
+			}
+		}
+
+		Eigen::Matrix4d p0inv = poses[0].inverse();
+		for(unsigned int j = 0; j < nr_frames; j++){
+			if(!is_ok[j]){continue;}
+			poses[j] = p0inv*poses[j];
+
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[j];
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi	= points[j];
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[j];
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni	= normals[j];
+
+			Eigen::Matrix4d p = poses[j];
+			float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+			float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+			float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+			for(int c = 0; c < Xi.cols(); c++){
+				float x = Xi(0,c);
+				float y = Xi(1,c);
+				float z = Xi(2,c);
+
+				float nx = Xni(0,c);
+				float ny = Xni(1,c);
+				float nz = Xni(2,c);
+
+				tXi(0,c)		= m00*x + m01*y + m02*z + m03;
+				tXi(1,c)		= m10*x + m11*y + m12*z + m13;
+				tXi(2,c)		= m20*x + m21*y + m22*z + m23;
+
+				tXni(0,c)		= m00*nx + m01*ny + m02*nz;
+				tXni(1,c)		= m10*nx + m11*ny + m12*nz;
+				tXni(2,c)		= m20*nx + m21*ny + m22*nz;
+			}
+		}
+		if(isconverged(poses, poses2, stopval, stopval)){break;}
+	}
+	return poses;
 }
 
-int testcount = 0;
 MassFusionResults MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d> poses){
-	printf("start MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d> poses)\n");
+	if(visualizationLvl > 0){printf("start MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d> poses)\n");}
 
 	unsigned int nr_frames = informations.size();
 	if(poses.size() != nr_frames){
 		printf("ERROR: poses.size() != informations.size()\n");
 		return MassFusionResults();
 	}
-	bool internatldebug = testcount++ > 2;
 
 	fast_opt = false;
 	if(fast_opt){
@@ -509,7 +1645,6 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d
 	}
 
 	for(unsigned int i = 0; i < nr_frames; i++){
-		printf("loading data for %i\n",i);
 		Eigen::Matrix4d p = poses[i];
 		float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
 		float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
@@ -517,6 +1652,8 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d
 
 		if(!is_ok[i]){continue;}
 
+		matchids[i].resize(nr_frames);
+
 		Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
 		Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
 		Eigen::Matrix<double, 3, Eigen::Dynamic> & tX	= transformed_points[i];
@@ -537,33 +1674,12 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d
 			tXn(1,c)	= m10*xn + m11*yn + m12*zn;
 			tXn(2,c)	= m20*xn + m21*yn + m22*zn;
 		}
+
 	}
 
 	func->reset();
 
-	Eigen::MatrixXd Xo1;
-
 	int imgcount = 0;
-
-	double good_rematches = 0;
-	double total_rematches = 0;
-
-	double good_opt = 0;
-	double bad_opt = 0;
-
-	double rematch_time = 0;
-	double residuals_time = 0;
-	double computeModel_time = 0;
-	double setup_matches_time = 0;
-	double setup_equation_time = 0;
-	double setup_equation_time2 = 0;
-	double solve_equation_time = 0;
-	double total_time_start = getTime();
-
-	int savecounter = 0;
-
-	bool onetoone = true;
-
 	char buf [1024];
 	if(visualizationLvl > 0){
 		std::vector<Eigen::MatrixXd> Xv;
@@ -572,797 +1688,59 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d
 		show(Xv,false,std::string(buf),imgcount);
 	}
 
+	rematch_time = 0;
+	residuals_time = 0;
+	opt_time = 0;
+	computeModel_time = 0;
+	setup_matches_time = 0;
+	setup_equation_time = 0;
+	setup_equation_time2 = 0;
+	solve_equation_time = 0;
+	total_time_start = getTime();
+
+
+
+	bool first = true;
+	std::vector<Eigen::Matrix4d> poses0 = poses;
+
 	for(int funcupdate=0; funcupdate < 100; ++funcupdate) {
 		if(getTime()-total_time_start > timeout){break;}
 		if(visualizationLvl == 2){std::vector<Eigen::MatrixXd> Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);}
 
-		for(int rematching=0; rematching < 10; ++rematching) {
+		for(int rematching=0; rematching < 20; ++rematching) {
 			if(visualizationLvl == 3){std::vector<Eigen::MatrixXd> Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);}
 			std::vector<Eigen::Matrix4d> poses1 = poses;
-			double rematch_time_start = getTime();
-
-			double new_good_rematches = 0;
-			double new_total_rematches = 0;
-			for(unsigned int i = 0; i < nr_frames; i++){
-				if(!is_ok[i]){continue;}
-				nr_matches[i] = 0;
-
-				for(unsigned int j = 0; j < nr_frames; j++){
-					if(!is_ok[j]){continue;}
-					if(i == j){continue;}
-					Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
-					Eigen::Matrix<double, 3, Eigen::Dynamic> tX	= rp*points[i];
-
-					unsigned int nr_data = nr_datas[i];
-					std::vector<int> & matchid = matchids[i][j];
-					matchid.resize(nr_data);
-					Tree3d * t3d = trees3d[j];
-
-					for(unsigned int k = 0; k < nr_data; ++k) {
-						int prev = matchid[k];
-						double * qp = tX.col(k).data();
-						size_t ret_index;
-						double out_dist_sqr;
-						nanoflann::KNNResultSet<double> resultSet(1);
-						resultSet.init(&ret_index, &out_dist_sqr );
-						t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
-						int current = ret_index;
-						new_good_rematches += prev != current;
-						new_total_rematches++;
-						matchid[k] = current;
-					}
-					nr_matches[i] += matchid.size();
-				}
-			}
-
-			good_rematches += new_good_rematches;
-			total_rematches += new_total_rematches;
 
+			double rematch_time_start = getTime();
+			rematch(poses,poses0,first);
+			first = false;
+			poses0 = poses;
 			rematch_time += getTime()-rematch_time_start;
-		//	printf("rematch_time: %f\n",rematch_time);
-		//	printf("new percentage: %5.5f (good_rematches: %f total_rematches: %f)\n",new_good_rematches/new_total_rematches,new_good_rematches,new_total_rematches);
-		//	printf("tot percentage: %5.5f (good_rematches: %f total_rematches: %f)\n",good_rematches/total_rematches,good_rematches,total_rematches);
-			for(unsigned int i = 0; i < nr_frames; i++){
-				if(!is_ok[i]){continue;}
-				nr_matches[i] = 0;
-				for(unsigned int j = 0; j < nr_frames; j++){
-					if(!is_ok[j]){continue;}
-					nr_matches[i] += matchids[i][j].size()+matchids[j][i].size();
-				}
-			}
-
-			int total_matches = 0;
-			for(unsigned int i = 0; i < nr_frames; i++){
-				if(!is_ok[i]){continue;}
-				for(unsigned int j = 0; j < nr_frames; j++){
-					if(!is_ok[j]){continue;}
-					total_matches += matchids[i][j].size();
-				}
-			}
 
 			for(int lala = 0; lala < 1; lala++){
+				if(visualizationLvl > 0){
+					printf("funcupdate: %i rematching: %i lala: %i\n",funcupdate,rematching,lala);
+					printf("total_time:          %5.5f\n",getTime()-total_time_start);
+					printf("rematch_time:        %5.5f\n",rematch_time);
+					printf("compM residuals_time:%5.5f\n",residuals_time);
+					printf("computeModel:        %5.5f\n",computeModel_time);
+					printf("opt_time:            %5.5f\n",opt_time);
+				}
 				if(visualizationLvl == 4){std::vector<Eigen::MatrixXd> Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);}
 				std::vector<Eigen::Matrix4d> poses2b = poses;
-				double residuals_time_start = getTime();
-				Eigen::MatrixXd all_residuals;
-				switch(type) {
-					case PointToPoint:	{all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break;
-					case PointToPlane:	{all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break;
-					default:			{printf("type not set\n");}					break;
-				}
-
-				int count = 0;
-				for(unsigned int i = 0; i < nr_frames; i++){
-					if(!is_ok[i]){continue;}
-					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[i];
-					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[i];
-					Eigen::VectorXd & informationi					= informations[i];
-					for(unsigned int j = 0; j < nr_frames; j++){
-						if(!is_ok[j]){continue;}
-						if(i == j){continue;}
-						std::vector<int> & matchidi = matchids[i][j];
-						unsigned int matchesi = matchidi.size();
-						Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj	= transformed_points[j];
-						Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj	= transformed_normals[j];
-						Eigen::VectorXd & informationj					= informations[j];
-						Eigen::Matrix3Xd Xp		= Eigen::Matrix3Xd::Zero(3,	matchesi);
-						Eigen::Matrix3Xd Xn		= Eigen::Matrix3Xd::Zero(3,	matchesi);
-						Eigen::Matrix3Xd Qp		= Eigen::Matrix3Xd::Zero(3,	matchesi);
-						Eigen::Matrix3Xd Qn		= Eigen::Matrix3Xd::Zero(3,	matchesi);
-						Eigen::VectorXd  rangeW	= Eigen::VectorXd::Zero(	matchesi);
 
-						for(unsigned int ki = 0; ki < matchesi; ki++){
-							int kj = matchidi[ki];
-							if( ki >= Qp.cols() || kj < 0 || kj >= tXj.cols() ){continue;}
-							Qp.col(ki) = tXj.col(kj);
-							Qn.col(ki) = tXnj.col(kj);
-							Xp.col(ki) = tXi.col(ki);
-							Xn.col(ki) = tXni.col(ki);
-							rangeW(ki) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj));
-						}
-						Eigen::MatrixXd residuals;
-						switch(type) {
-						case PointToPoint:	{residuals = Xp-Qp;} 						break;
-						case PointToPlane:	{
-							residuals		= Eigen::MatrixXd::Zero(1,	Xp.cols());
-							for(int i=0; i<Xp.cols(); ++i) {
-								float dx = Xp(0,i)-Qp(0,i);
-								float dy = Xp(1,i)-Qp(1,i);
-								float dz = Xp(2,i)-Qp(2,i);
-								float qx = Qn(0,i);
-								float qy = Qn(1,i);
-								float qz = Qn(2,i);
-								float di = qx*dx + qy*dy + qz*dz;
-								residuals(0,i) = di;
-							}
-						}break;
-						default:			{printf("type not set\n");}					break;
-						}
-						for(unsigned int k=0; k < matchesi; ++k) {residuals.col(k) *= rangeW(k);}
-						all_residuals.block(0,count,residuals.rows(),residuals.cols()) = residuals;
-						count += residuals.cols();
-					}
-				}
+				double residuals_time_start = getTime();
+				Eigen::MatrixXd all_residuals = getAllResiduals(poses);
 				residuals_time += getTime()-residuals_time_start;
 
 				double computeModel_time_start = getTime();
 				func->computeModel(all_residuals);
 				computeModel_time += getTime()-computeModel_time_start;
 
-				if(fast_opt){
-					double setup_matches_time_start = getTime();
-
-					std::vector< Eigen::Matrix4d > localposes = poses;
-					std::vector< std::vector < std::vector< std::pair<int,int	> > > > current_matches;
-					std::vector< std::vector < std::vector<			double		  > > > rangeW;
-
-					current_matches.resize(nr_frames);
-					rangeW.resize(nr_frames);
-					for(unsigned int i = 0; i < nr_frames; i++){
-						current_matches[i].resize(nr_frames);
-						rangeW[i].resize(nr_frames);
-						if(!is_ok[i]){continue;}
-
-						Eigen::VectorXd & informationi					= informations[i];
-						for(unsigned int j = 0; j < nr_frames; j++){
-							if(!is_ok[j]){continue;}
-							if(i == j){continue;}
-							Eigen::VectorXd & informationj					= informations[j];
-
-							std::vector<int> & matchidj = matchids[j][i];
-							unsigned int matchesj = matchidj.size();
-							std::vector<int> & matchidi = matchids[i][j];
-							unsigned int matchesi = matchidi.size();
-
-							std::vector<std::pair<int,int> > & cm = current_matches[i][j];
-							std::vector<double > & rw = rangeW[i][j];
-
-							for(unsigned int ki = 0; ki < matchesi; ki++){
-								int kj = matchidi[ki];
-								if( kj < 0 || kj >=  matchesj){continue;} //Make sure that failed searches dont screw things up
-								if(matchidj[kj] != ki){continue;}//Only 1-to-1 matching
-
-								cm.push_back(std::make_pair(ki,kj));
-								rw.push_back(1.0/(1.0/informationi(ki)+1.0/informationj(kj)));
-							}
-						}
-					}
-
-					setup_matches_time += getTime()-setup_matches_time_start;
-
-					typedef Eigen::Matrix<double, 6, 1> Vector6d;
-					typedef Eigen::Matrix<double, 6, 6> Matrix6d;
+				double opt_time_start = getTime();
+				poses = optimize(poses);
+				opt_time += getTime()-opt_time_start;
 
-					std::vector<std::vector<Matrix6d>> A;
-					std::vector<std::vector<Vector6d>> b;
-					A.resize(nr_frames);
-					b.resize(nr_frames);
-					for(unsigned int i = 0; i < nr_frames; i++){
-						A[i].resize(nr_frames);
-						b[i].resize(nr_frames);
-					}
-
-					std::vector<std::vector<Matrix6d>> A2;
-					std::vector<std::vector<Vector6d>> b2;
-					A2.resize(nr_frames);
-					b2.resize(nr_frames);
-					for(unsigned int i = 0; i < nr_frames; i++){
-						A2[i].resize(nr_frames);
-						b2[i].resize(nr_frames);
-						for(unsigned int j = 0; j < nr_frames; j++){
-							Matrix6d & ATA = A[i][j];
-							Vector6d & ATb = b[i][j];
-							ATA.setZero ();
-							ATb.setZero ();
-						}
-					}
-
-					for(int iteration = 0; iteration < 5; iteration++){
-						printf("iteration: %i\n",iteration);
-
-						double total_score = 0;
-
-						double setup_equation_time_start = getTime();
-						for(unsigned int i = 0; i < nr_frames; i++){
-							if(!is_ok[i]){continue;}
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[i];
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[i];
-							for(unsigned int j = 0; j < nr_frames; j++){
-								if(!is_ok[j]){continue;}
-								if(i == j){continue;}
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj	= transformed_points[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj	= transformed_normals[j];
-
-								std::vector<std::pair<int,int> > & cm = current_matches[i][j];
-								std::vector<double > & rw = rangeW[i][j];
-								unsigned int current_nr_matches = cm.size();
-
-								Matrix6d & ATA = A[i][j];
-								Vector6d & ATb = b[i][j];
-								ATA.setZero ();
-								ATb.setZero ();
-
-								//Matches from ki to kj
-								for(unsigned int k = 0; k < current_nr_matches; k++){
-									unsigned int ki = cm[k].first;
-									unsigned int kj = cm[k].second;
-									double rwij = rw[k];
-
-									const float & sx = tXi(0,ki);
-									const float & sy = tXi(1,ki);
-									const float & sz = tXi(2,ki);
-
-									const float & dx = tXj(0,kj);
-									const float & dy = tXj(1,kj);
-									const float & dz = tXj(2,kj);
-
-									const float & nx = tXnj(0,kj);
-									const float & ny = tXnj(1,kj);
-									const float & nz = tXnj(2,kj);
-
-
-									double a = nz*sy - ny*sz;
-									double b = nx*sz - nz*sx;
-									double c = ny*sx - nx*sy;
-
-									//    0  1  2  3  4  5
-									//    6  7  8  9 10 11
-									//   12 13 14 15 16 17
-									//   18 19 20 21 22 23
-									//   24 25 26 27 28 29
-									//   30 31 32 33 34 35
-
-									ATA.coeffRef (0) += a * a;
-									ATA.coeffRef (1) += a * b;
-									ATA.coeffRef (2) += a * c;
-									ATA.coeffRef (3) += a * nx;
-									ATA.coeffRef (4) += a * ny;
-									ATA.coeffRef (5) += a * nz;
-									ATA.coeffRef (7) += b * b;
-									ATA.coeffRef (8) += b * c;
-									ATA.coeffRef (9) += b * nx;
-									ATA.coeffRef (10) += b * ny;
-									ATA.coeffRef (11) += b * nz;
-									ATA.coeffRef (14) += c * c;
-									ATA.coeffRef (15) += c * nx;
-									ATA.coeffRef (16) += c * ny;
-									ATA.coeffRef (17) += c * nz;
-									ATA.coeffRef (21) += nx * nx;
-									ATA.coeffRef (22) += nx * ny;
-									ATA.coeffRef (23) += nx * nz;
-									ATA.coeffRef (28) += ny * ny;
-									ATA.coeffRef (29) += ny * nz;
-									ATA.coeffRef (35) += nz * nz;
-
-									double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
-									total_score += d*d;
-									ATb.coeffRef (0) += a * d;
-									ATb.coeffRef (1) += b * d;
-									ATb.coeffRef (2) += c * d;
-									ATb.coeffRef (3) += nx * d;
-									ATb.coeffRef (4) += ny * d;
-									ATb.coeffRef (5) += nz * d;
-
-								}
-								ATA.coeffRef (6) = ATA.coeff (1);
-								ATA.coeffRef (12) = ATA.coeff (2);
-								ATA.coeffRef (13) = ATA.coeff (8);
-								ATA.coeffRef (18) = ATA.coeff (3);
-								ATA.coeffRef (19) = ATA.coeff (9);
-								ATA.coeffRef (20) = ATA.coeff (15);
-								ATA.coeffRef (24) = ATA.coeff (4);
-								ATA.coeffRef (25) = ATA.coeff (10);
-								ATA.coeffRef (26) = ATA.coeff (16);
-								ATA.coeffRef (27) = ATA.coeff (22);
-								ATA.coeffRef (30) = ATA.coeff (5);
-								ATA.coeffRef (31) = ATA.coeff (11);
-								ATA.coeffRef (32) = ATA.coeff (17);
-								ATA.coeffRef (33) = ATA.coeff (23);
-								ATA.coeffRef (34) = ATA.coeff (29);
-							}
-						}
-						setup_equation_time += getTime()-setup_equation_time_start;
-
-						double setup_equation_time_start2 = getTime();
-						for(unsigned int i = 0; i < nr_frames; i++){
-							if(!is_ok[i]){continue;}
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[i];
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[i];
-							for(unsigned int j = 0; j < nr_frames; j++){
-								if(!is_ok[j]){continue;}
-								if(i == j){continue;}
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj	= transformed_points[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj	= transformed_normals[j];
-
-								std::vector<std::pair<int,int> > & cm = current_matches[i][j];
-								std::vector<double > & rw = rangeW[i][j];
-								unsigned int current_nr_matches = cm.size();
-
-								Matrix6d & ATA = A2[i][j];
-								Vector6d & ATb = b2[i][j];
-								ATA.setZero ();
-								ATb.setZero ();
-
-								//Matches from ki to kj
-								for(unsigned int k = 0; k < current_nr_matches; k++){
-									unsigned int ki = cm[k].first;
-									unsigned int kj = cm[k].second;
-									double rwij = rw[k];
-
-
-
-									const float & sx = tXi(0,ki);
-									const float & sy = tXi(1,ki);
-									const float & sz = tXi(2,ki);
-
-									const float & snx = tXni(0,ki);
-									const float & sny = tXni(1,ki);
-									const float & snz = tXni(2,ki);
-
-									const float & dx = tXj(0,kj);
-									const float & dy = tXj(1,kj);
-									const float & dz = tXj(2,kj);
-
-									const float & nx = tXnj(0,kj);
-									const float & ny = tXnj(1,kj);
-									const float & nz = tXnj(2,kj);
-
-
-									double a = nz*sy - ny*sz;
-									double b = nx*sz - nz*sx;
-									double c = ny*sx - nx*sy;
-
-
-									double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
-
-									double angle = nx*snx + ny*sny + nz*snz;
-									if(angle < 0){continue;}
-
-									double prob = func->getProb(d);
-									double weight = prob*rwij;
-									//    0  1  2  3  4  5
-									//    6  7  8  9 10 11
-									//   12 13 14 15 16 17
-									//   18 19 20 21 22 23
-									//   24 25 26 27 28 29
-									//   30 31 32 33 34 35
-
-									ATA.coeffRef (0) += weight * a * a;
-									ATA.coeffRef (1) += weight * a * b;
-									ATA.coeffRef (2) += weight * a * c;
-									ATA.coeffRef (3) += weight * a * nx;
-									ATA.coeffRef (4) += weight * a * ny;
-									ATA.coeffRef (5) += weight * a * nz;
-									ATA.coeffRef (7) += weight * b * b;
-									ATA.coeffRef (8) += weight * b * c;
-									ATA.coeffRef (9) += weight * b * nx;
-									ATA.coeffRef (10) += weight * b * ny;
-									ATA.coeffRef (11) += weight * b * nz;
-									ATA.coeffRef (14) += weight * c * c;
-									ATA.coeffRef (15) += weight * c * nx;
-									ATA.coeffRef (16) += weight * c * ny;
-									ATA.coeffRef (17) += weight * c * nz;
-									ATA.coeffRef (21) += weight * nx * nx;
-									ATA.coeffRef (22) += weight * nx * ny;
-									ATA.coeffRef (23) += weight * nx * nz;
-									ATA.coeffRef (28) += weight * ny * ny;
-									ATA.coeffRef (29) += weight * ny * nz;
-									ATA.coeffRef (35) += weight * nz * nz;
-
-									ATb.coeffRef (0) += weight * a * d;
-									ATb.coeffRef (1) += weight * b * d;
-									ATb.coeffRef (2) += weight * c * d;
-									ATb.coeffRef (3) += weight * nx * d;
-									ATb.coeffRef (4) += weight * ny * d;
-									ATb.coeffRef (5) += weight * nz * d;
-
-								}
-								ATA.coeffRef (6) = ATA.coeff (1);
-								ATA.coeffRef (12) = ATA.coeff (2);
-								ATA.coeffRef (13) = ATA.coeff (8);
-								ATA.coeffRef (18) = ATA.coeff (3);
-								ATA.coeffRef (19) = ATA.coeff (9);
-								ATA.coeffRef (20) = ATA.coeff (15);
-								ATA.coeffRef (24) = ATA.coeff (4);
-								ATA.coeffRef (25) = ATA.coeff (10);
-								ATA.coeffRef (26) = ATA.coeff (16);
-								ATA.coeffRef (27) = ATA.coeff (22);
-								ATA.coeffRef (30) = ATA.coeff (5);
-								ATA.coeffRef (31) = ATA.coeff (11);
-								ATA.coeffRef (32) = ATA.coeff (17);
-								ATA.coeffRef (33) = ATA.coeff (23);
-								ATA.coeffRef (34) = ATA.coeff (29);
-							}
-						}
-						setup_equation_time2 += getTime()-setup_equation_time_start2;
-
-						printf("total_score: %f\n",total_score);
-
-						if(false){
-							Eigen::Matrix4d p0inv = poses[0].inverse();
-							for(unsigned int j = 0; j < nr_frames; j++){
-								if(!is_ok[j]){continue;}
-								poses[j] = p0inv*poses[j];
-
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi	= points[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni	= normals[j];
-
-								Eigen::Matrix4d p = poses[j];
-								float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
-								float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
-								float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
-
-								for(int c = 0; c < Xi.cols(); c++){
-									float x = Xi(0,c);
-									float y = Xi(1,c);
-									float z = Xi(2,c);
-
-									float nx = Xni(0,c);
-									float ny = Xni(1,c);
-									float nz = Xni(2,c);
-
-									tXi(0,c)		= m00*x + m01*y + m02*z + m03;
-									tXi(1,c)		= m10*x + m11*y + m12*z + m13;
-									tXi(2,c)		= m20*x + m21*y + m22*z + m23;
-
-									tXni(0,c)		= m00*nx + m01*ny + m02*nz;
-									tXni(1,c)		= m10*nx + m11*ny + m12*nz;
-									tXni(2,c)		= m20*nx + m21*ny + m22*nz;
-								}
-							}
-
-							std::vector<Eigen::MatrixXd> Xv;
-							for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
-							sprintf(buf,"image%5.5i.png",imgcount++);
-							show(Xv,false,std::string(buf),imgcount);
-						}
-
-						double solve_equation_time_start = getTime();
-						Eigen::MatrixXd fullA = Eigen::MatrixXd::Identity (6 * (nr_frames - 1), 6 * (nr_frames - 1));
-						fullA *= 0.00001;
-						Eigen::VectorXd fullB = Eigen::VectorXd::Zero (6 * (nr_frames - 1));
-
-						for(unsigned int i = 0; i < nr_frames; i++){
-							for(unsigned int j = 0; j == 0 && j < nr_frames; j++){
-								if(i == j){continue;}
-								Matrix6d & ATA = A[i][j];
-								Vector6d & ATb = b[i][j];
-								Vector6d x = ATA.inverse () * ATb;
-								//printf("%i %i x: %5.5f %5.5f %5.5f %5.5f %5.5f %5.5f\n",i,j,x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0));
-								//								Eigen::Matrix4d m = constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0));
-								//								std::cout << m << std::endl << std::endl;
-								//								printf("%i %i ->%i %i\n",i,j,6*(i-1), 6*j);
-								if (i > 0){
-									//fullA.block (6*(i-1), 6*j,6,6) -= ATA;
-									fullA.block (6*(i-1),6*(i-1),6,6) += ATA;
-									fullB.segment (6*(i-1), 6) += ATb;
-								}
-								//G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
-								//B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
-							}
-						}
-						//						std::cout << fullA << std::endl << std::endl;
-						//						std::cout << fullB << std::endl << std::endl;
-						//printf("--------------------------------------------------\n");
-						Eigen::VectorXd fullX = fullA.inverse()*fullB;
-						for(unsigned int i = 0; i < nr_frames; i++){
-							Eigen::Matrix4d m;
-							if(i == 0){
-								m = Eigen::Matrix4d::Identity();
-							}else{
-								m = constructTransformationMatrix(fullX(6*(i-1)+0),fullX(6*(i-1)+1),fullX(6*(i-1)+2),fullX(6*(i-1)+3),fullX(6*(i-1)+4),fullX(6*(i-1)+5));
-							}
-							//std::cout << m << std::endl << std::endl;
-						}
-
-						//					    // Start at 1 because 0 is the reference pose
-						//					    for (int vi = 1; vi != n; ++vi)
-						//					    {
-						//					      for (int vj = 0; vj != n; ++vj)
-						//					      {
-						//					        // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
-						//					        Edge e;
-						//					        bool present1, present2;
-						//					        boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
-						//					        if (!present1)
-						//					        {
-						//					          boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
-						//					          if (!present2)
-						//					            continue;
-						//					        }
-
-						//					        // Fill in elements of G and B
-						//					        if (vj > 0)
-						//					          G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
-						//					        G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
-						//					        B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
-						//					      }
-						//					    }
-
-						//					    // Computation of the linear equation system: GX = B
-						//					    // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
-						//					    Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);
-
-						//					    // Update the poses
-						//					    float sum = 0.0;
-						//					    for (int vi = 1; vi != n; ++vi)
-						//					    {
-						//					      Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
-						//					      sum += difference_pose.norm ();
-						//					      setPose (vi, getPose (vi) + difference_pose);
-						//					    }
-
-						//						for(unsigned int i = 0; i < nr_frames; i++){
-						//							for(unsigned int j = 0; j < nr_frames; j++){
-						//								if(i == j){continue;}
-						//								Matrix6d & ATA = A[i][j];
-						//								Vector6d & ATb = b[i][j];
-						//								Vector6d x = ATA.inverse () * ATb;
-						//								printf("%i %i x: %5.5f %5.5f %5.5f %5.5f %5.5f %5.5f\n",i,j,x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0));
-
-						//								Matrix6d & ATA2 = A2[i][j];
-						//								Vector6d & ATb2 = b2[i][j];
-						//								Vector6d x2 = ATA2.inverse () * ATb2;
-						//								printf("%i %i x: %5.5f %5.5f %5.5f %5.5f %5.5f %5.5f\n",i,j,x2(0,0),x2(1,0),x2(2,0),x2(3,0),x2(4,0),x2(5,0));
-
-						//								Eigen::Matrix4d m = constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0));
-						//								std::cout << m << std::endl << std::endl;
-						//							}
-						//						}
-
-						solve_equation_time += getTime()-solve_equation_time_start;
-
-						if(false){
-							Eigen::Matrix4d p0inv = poses[0].inverse();
-							for(unsigned int j = 0; j < nr_frames; j++){
-								if(!is_ok[j]){continue;}
-								poses[j] = p0inv*poses[j];
-
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi	= points[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni	= normals[j];
-
-								Eigen::Matrix4d p = poses[j];
-								float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
-								float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
-								float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
-
-								for(int c = 0; c < Xi.cols(); c++){
-									float x = Xi(0,c);
-									float y = Xi(1,c);
-									float z = Xi(2,c);
-
-									float nx = Xni(0,c);
-									float ny = Xni(1,c);
-									float nz = Xni(2,c);
-
-									tXi(0,c)		= m00*x + m01*y + m02*z + m03;
-									tXi(1,c)		= m10*x + m11*y + m12*z + m13;
-									tXi(2,c)		= m20*x + m21*y + m22*z + m23;
-
-									tXni(0,c)		= m00*nx + m01*ny + m02*nz;
-									tXni(1,c)		= m10*nx + m11*ny + m12*nz;
-									tXni(2,c)		= m20*nx + m21*ny + m22*nz;
-								}
-							}
-
-							std::vector<Eigen::MatrixXd> Xv;
-							for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
-							sprintf(buf,"image%5.5i.png",imgcount++);
-							show(Xv,false,std::string(buf),imgcount);
-						}
-						//Recover poses from solution
-						//Recompute points
-
-						//if(isconverged(poses, localposes, stopval, stopval)){break;}
-					}
-
-					printf("total_time:           %5.5f\n",getTime()-total_time_start);
-					printf("rematch_time:         %5.5f\n",rematch_time);
-					printf("computeModel:         %5.5f\n",computeModel_time);
-					printf("setup_matches_time:   %5.5f\n",setup_matches_time);
-					printf("setup_equation_time:  %5.5f\n",setup_equation_time);
-					printf("setup_equation_time2: %5.5f\n",setup_equation_time2);
-					printf("solve_equation_time:  %5.5f\n",solve_equation_time);
-					exit(0);
-				}else{
-					for(int outer=0; outer < 30; ++outer) {
-						if(getTime()-total_time_start > timeout){break;}
-
-						printf("funcupdate: %i rematching: %i lala: %i outer: %i\n",funcupdate,rematching,lala,outer);
-						std::vector<Eigen::Matrix4d> poses2 = poses;
-						for(unsigned int i = 0; i < nr_frames; i++){
-							if(getTime()-total_time_start > timeout){break;}
-							if(!is_ok[i]){continue;}
-							unsigned int nr_match = 0;
-							{
-								for(unsigned int j = 0; j < nr_frames; j++){
-									if(!is_ok[j]){continue;}
-									std::vector<int> & matchidj = matchids[j][i];
-									unsigned int matchesj = matchidj.size();
-									std::vector<int> & matchidi = matchids[i][j];
-									unsigned int matchesi = matchidi.size();
-
-									for(unsigned int ki = 0; ki < matchesi; ki++){
-										int kj = matchidi[ki];
-										if( kj == -1 ){continue;}
-										if( kj >=  matchesj){continue;}
-										if(!onetoone || matchidj[kj] == ki){	nr_match++;}
-									}
-								}
-							}
-
-							Eigen::Matrix3Xd Xp		= Eigen::Matrix3Xd::Zero(3,	nr_match);
-							Eigen::Matrix3Xd Xp_ori	= Eigen::Matrix3Xd::Zero(3,	nr_match);
-							Eigen::Matrix3Xd Xn		= Eigen::Matrix3Xd::Zero(3,	nr_match);
-
-							Eigen::Matrix3Xd Qp		= Eigen::Matrix3Xd::Zero(3,	nr_match);
-							Eigen::Matrix3Xd Qn		= Eigen::Matrix3Xd::Zero(3,	nr_match);
-							Eigen::VectorXd  rangeW	= Eigen::VectorXd::Zero(	nr_match);
-
-							int count = 0;
-
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[i];
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi	= points[i];
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[i];
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni	= normals[i];
-							Eigen::VectorXd & informationi					= informations[i];
-
-							for(unsigned int j = 0; j < nr_frames; j++){
-								if(!is_ok[j]){continue;}
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj	= transformed_points[j];
-								Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj	= transformed_normals[j];
-								Eigen::VectorXd & informationj					= informations[j];
-
-								std::vector<int> & matchidj = matchids[j][i];
-								unsigned int matchesj = matchidj.size();
-								std::vector<int> & matchidi = matchids[i][j];
-								unsigned int matchesi = matchidi.size();
-
-								for(unsigned int ki = 0; ki < matchesi; ki++){
-									int kj = matchidi[ki];
-									if( kj == -1 ){continue;}
-									if( kj >=  matchesj){continue;}
-									if(!onetoone || matchidj[kj] == ki){
-										Qp.col(count) = tXj.col(kj);
-										Qn.col(count) = tXnj.col(kj);
-
-										Xp_ori.col(count) = Xi.col(ki);
-										Xp.col(count) = tXi.col(ki);
-
-										Xn.col(count) = tXni.col(ki);
-										rangeW(count) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj));
-										count++;
-									}
-								}
-							}
-
-							if(count == 0){break;}
-
-							//showMatches(Xp,Qp);
-							for(int inner=0; inner < 5; ++inner) {
-								Eigen::MatrixXd residuals;
-								switch(type) {
-								case PointToPoint:	{residuals = Xp-Qp;} 						break;
-								case PointToPlane:	{
-									residuals		= Eigen::MatrixXd::Zero(1,	Xp.cols());
-									for(int i=0; i<Xp.cols(); ++i) {
-										float dx = Xp(0,i)-Qp(0,i);
-										float dy = Xp(1,i)-Qp(1,i);
-										float dz = Xp(2,i)-Qp(2,i);
-										float qx = Qn(0,i);
-										float qy = Qn(1,i);
-										float qz = Qn(2,i);
-										float di = qx*dx + qy*dy + qz*dz;
-										residuals(0,i) = di;
-									}
-								}break;
-								default:			{printf("type not set\n");}					break;
-								}
-								for(unsigned int k=0; k < nr_match; ++k) {residuals.col(k) *= rangeW(k);}
-
-								Eigen::VectorXd  W;
-								switch(type) {
-								case PointToPoint:	{W = func->getProbs(residuals); } 					break;
-								case PointToPlane:	{
-									W = func->getProbs(residuals);
-									for(int k=0; k<nr_match; ++k) {W(k) = W(k)*float((Xn(0,k)*Qn(0,k) + Xn(1,k)*Qn(1,k) + Xn(2,k)*Qn(2,k)) > 0.0);}
-								}	break;
-								default:			{printf("type not set\n");} break;
-								}
-
-
-								for(int k=0; k<nr_match; ++k) {
-									if(W(k) > 0.1){good_opt++;}
-									else{bad_opt++;}
-								}
-
-								W = W.array()*rangeW.array()*rangeW.array();
-								Xo1 = Xp;
-								switch(type) {
-									case PointToPoint:	{
-										//RigidMotionEstimator::point_to_point(Xp, Qp, W);
-										pcl::TransformationFromCorrespondences tfc1;
-										for(unsigned int c = 0; c < nr_match; c++){tfc1.add(Eigen::Vector3f(Xp(0,c), Xp(1,c),Xp(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));}
-										Eigen::Affine3d rot = tfc1.getTransformation().cast<double>();
-										Xp = rot*Xp;
-										Xn = rot.rotation()*Xn;
-									}		break;
-									case PointToPlane:	{
-										point_to_plane2(Xp, Xn, Qp, Qn, W);
-									}	break;
-								default:  			{printf("type not set\n"); } break;
-								}
-
-								double stop1 = (Xp-Xo1).colwise().norm().maxCoeff();
-								Xo1 = Xp;
-								if(stop1 < 0.001){break; }
-							}
-							//exit(0);
-							pcl::TransformationFromCorrespondences tfc;
-							for(unsigned int c = 0; c < nr_match; c++){tfc.add(Eigen::Vector3f(Xp_ori(0,c),Xp_ori(1,c),Xp_ori(2,c)),Eigen::Vector3f(Xp(0,c),Xp(1,c),Xp(2,c)));}
-							poses[i] = tfc.getTransformation().cast<double>().matrix();
-						}
-
-						Eigen::Matrix4d p0inv = poses[0].inverse();
-						for(unsigned int j = 0; j < nr_frames; j++){
-							if(!is_ok[j]){continue;}
-							poses[j] = p0inv*poses[j];
-
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[j];
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi	= points[j];
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[j];
-							Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni	= normals[j];
-
-							Eigen::Matrix4d p = poses[j];
-							float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
-							float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
-							float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
-
-							for(int c = 0; c < Xi.cols(); c++){
-								float x = Xi(0,c);
-								float y = Xi(1,c);
-								float z = Xi(2,c);
-
-								float nx = Xni(0,c);
-								float ny = Xni(1,c);
-								float nz = Xni(2,c);
-
-								tXi(0,c)		= m00*x + m01*y + m02*z + m03;
-								tXi(1,c)		= m10*x + m11*y + m12*z + m13;
-								tXi(2,c)		= m20*x + m21*y + m22*z + m23;
-
-								tXni(0,c)		= m00*nx + m01*ny + m02*nz;
-								tXni(1,c)		= m10*nx + m11*ny + m12*nz;
-								tXni(2,c)		= m20*nx + m21*ny + m22*nz;
-							}
-						}
-						if(isconverged(poses, poses2, stopval, stopval)){break;}
-					}
-				}
 				if(isconverged(poses, poses2b, stopval, stopval)){break;}
 			}
 			if(isconverged(poses, poses1, stopval, stopval)){break;}
@@ -1376,11 +1754,12 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d
 
 	printf("total_time:          %5.5f\n",getTime()-total_time_start);
 	printf("rematch_time:        %5.5f\n",rematch_time);
+	printf("compM residuals_time:%5.5f\n",residuals_time);
 	printf("computeModel:        %5.5f\n",computeModel_time);
-	printf("setup_matches_time:  %5.5f\n",setup_matches_time);
-	printf("setup_equation_time: %5.5f\n",setup_equation_time);
-	printf("solve_equation_time: %5.5f\n",solve_equation_time);
-	printf("good opt: %f bad opt: %f ratio: %f\n",good_opt,bad_opt,good_opt/(good_opt+bad_opt));
+	printf("opt_time:            %5.5f\n",opt_time);
+//	printf("setup_matches_time:  %5.5f\n",setup_matches_time);
+//	printf("setup_equation_time: %5.5f\n",setup_equation_time);
+//	printf("solve_equation_time: %5.5f\n",solve_equation_time);
 
 	if(visualizationLvl > 0){
 		std::vector<Eigen::MatrixXd> Xv;
@@ -1393,6 +1772,7 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d
 	for(int i = 0; i < nr_frames; i++){poses[i] = firstinv*poses[i];}
 
 	printf("stop MassRegistrationPPR::getTransforms(std::vector<Eigen::Matrix4d> guess)\n");
+	//exit(0);
 	return MassFusionResults(poses,-1);
 }
 
diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp
new file mode 100644
index 00000000..bcfca359
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp
@@ -0,0 +1,3275 @@
+#include "registration/MassRegistrationPPR2.h"
+
+#include <iostream>
+#include <fstream>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace reglib
+{
+
+//#define domultithread
+
+MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){
+	type					= PointToPlane;
+	//type					= PointToPoint;
+	use_PPR_weight			= true;
+	use_features			= true;
+	normalize_matchweights	= true;
+
+
+	use_surface = true;
+	use_depthedge = true;
+
+	DistanceWeightFunction2PPR2 * dwf = new DistanceWeightFunction2PPR2();
+	dwf->update_size		= true;
+	dwf->startreg			= startreg;
+	dwf->debugg_print		= false;
+	func					= dwf;
+
+
+	DistanceWeightFunction2PPR2 * kpdwf = new DistanceWeightFunction2PPR2();
+	kpdwf->update_size		= true;
+	kpdwf->startreg			= startreg;
+	kpdwf->debugg_print		= false;
+	kpfunc					= kpdwf;
+
+	DistanceWeightFunction2PPR2 * depthedge_dwf = new DistanceWeightFunction2PPR2();
+	depthedge_dwf->update_size		= true;
+	depthedge_dwf->startreg			= startreg;
+	depthedge_dwf->debugg_print		= false;
+	depthedge_func					= depthedge_dwf;
+
+	fast_opt				= true;
+
+	nomask = true;
+	maskstep = 1;
+	nomaskstep = 100000;
+
+	stopval = 0.001;
+	steps = 4;
+
+	timeout = 6000;
+
+	if(visualize){
+		visualizationLvl = 1;
+		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
+		viewer->setBackgroundColor (0, 0, 0);
+		viewer->addCoordinateSystem (1.0);
+		viewer->initCameraParameters ();
+	}else{
+		visualizationLvl = 0;
+	}
+
+
+	Qp_arr = new double[3*maxcount+0];
+	Qn_arr = new double[3*maxcount+0];
+	Xp_arr = new double[3*maxcount+0];
+	Xn_arr = new double[3*maxcount+0];
+	rangeW_arr = new double[maxcount+0];
+
+
+	kp_Qp_arr = new double[3*maxcount+0];
+	kp_Qn_arr = new double[3*maxcount+0];
+	kp_Xp_arr = new double[3*maxcount+0];
+	kp_Xn_arr = new double[3*maxcount+0];
+	kp_rangeW_arr = new double[maxcount+0];
+
+	depthedge_Qp_arr = new double[3*maxcount+0];
+	depthedge_Xp_arr = new double[3*maxcount+0];
+	depthedge_rangeW_arr = new double[maxcount+0];
+
+	//depthedge_nr_neighbours = 10;
+}
+MassRegistrationPPR2::~MassRegistrationPPR2(){
+
+	delete func;
+	delete kpfunc;
+	delete depthedge_func;
+
+	delete[] Qp_arr;
+	delete[] Qn_arr;
+	delete[] Xp_arr;
+	delete[] Xn_arr;
+	delete[] rangeW_arr;
+
+	delete[] kp_Qp_arr;
+	delete[] kp_Qn_arr;
+	delete[] kp_Xp_arr;
+	delete[] kp_Xn_arr;
+	delete[] kp_rangeW_arr;
+
+	delete[] depthedge_Qp_arr;
+	delete[] depthedge_Xp_arr;
+	delete[] depthedge_rangeW_arr;
+
+	clearData();
+}
+
+void MassRegistrationPPR2::addModel(Model * model){
+
+	double total_load_time_start = getTime();
+
+
+	nr_matches.push_back(	0);
+	matchids.push_back(		std::vector< std::vector<long > >() );
+	matchdists.push_back(	std::vector< std::vector<double> >() );
+	nr_datas.push_back(		0);
+
+	points.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	colors.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	normals.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_points.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_normals.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+
+	informations.push_back(			Eigen::VectorXd());
+
+	nr_arraypoints.push_back(0);
+	arraypoints.push_back(0);
+	arraynormals.push_back(0);
+	arraycolors.push_back(0);
+	arrayinformations.push_back(0);
+	trees3d.push_back(0);
+	a3dv.push_back(0);
+
+	depthedge_nr_matches.push_back(	0);
+	depthedge_matchids.push_back(		std::vector< std::vector<long > >() );
+	depthedge_matchdists.push_back(	std::vector< std::vector<double> >() );
+
+	depthedge_nr_arraypoints.push_back(0);
+	depthedge_arraypoints.push_back(0);
+	depthedge_arrayinformations.push_back(0);
+	depthedge_trees3d.push_back(0);
+	depthedge_a3dv.push_back(0);
+
+	long step = maskstep*maskstep;
+
+	is_ok.push_back(false);
+
+	long count = model->points.size()/step;
+	long i = points.size()-1;
+	if(count < 10){
+		is_ok[i] = false;
+		return;
+	}else{
+		is_ok[i] = true;
+	}
+
+	//    printf("%i is ok %i\n",i,is_ok[i]);
+
+	nr_datas[i] = count;
+	points[i].resize(Eigen::NoChange,count);
+	colors[i].resize(Eigen::NoChange,count);
+	normals[i].resize(Eigen::NoChange,count);
+	transformed_points[i].resize(Eigen::NoChange,count);
+	transformed_normals[i].resize(Eigen::NoChange,count);
+
+	double * ap = new double[3*count];
+	double * an = new double[3*count];
+	double * ac = new double[3*count];
+	double * ai = new double[3*count];
+	nr_arraypoints[i] = count;
+	arraypoints[i] = ap;
+	arraynormals[i] = an;
+	arraycolors[i] = ac;
+	arrayinformations[i] = ai;
+
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & C	= colors[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & tX	= transformed_points[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & tXn	= transformed_normals[i];
+	Eigen::VectorXd information (count);
+
+	std::vector<int> pind;
+	pind.resize(model->points.size());
+	for(unsigned long c = 0; c < model->points.size(); c++){
+		pind[c] = c;
+	}
+
+	for(unsigned long c = 0; c < count; c++){
+		long rind = rand() % model->points.size();
+		long pind1 = pind[c];
+		pind[c] = pind[rind];
+		pind[rind] = pind1;
+	}
+
+
+
+	for(unsigned long c = 0; c < count; c++){
+		//superpoint & sp = model->points[c*step];
+		superpoint & sp = model->points[pind[c]];
+		ap[3*c+0] = sp.point(0);
+		ap[3*c+1] = sp.point(1);
+		ap[3*c+2] = sp.point(2);
+
+		an[3*c+0] = sp.normal(0);
+		an[3*c+1] = sp.normal(1);
+		an[3*c+2] = sp.normal(2);
+
+		ac[3*c+0] = sp.feature(0);
+		ac[3*c+1] = sp.feature(1);
+		ac[3*c+2] = sp.feature(2);
+
+		ai[c] = sqrt(1.0/sp.point_information);//1.0/(z*z);
+
+		X(0,c)	= ap[3*c+0];
+		X(1,c)	= ap[3*c+1];
+		X(2,c)	= ap[3*c+2];
+		Xn(0,c)	= an[3*c+0];
+		Xn(1,c)	= an[3*c+1];
+		Xn(2,c)	= an[3*c+2];
+
+		information(c) = ai[c];//1.0/(z*z);
+		C(0,c) = ac[3*c+0];
+		C(1,c) = ac[3*c+1];
+		C(2,c) = ac[3*c+2];
+
+
+		//		ap[3*c+0] = model->points[c*step].point(0);
+		//		ap[3*c+1] = model->points[c*step].point(1);
+		//		ap[3*c+2] = model->points[c*step].point(2);
+
+		//		an[3*c+0] = model->points[c*step].normal(0);
+		//		an[3*c+1] = model->points[c*step].normal(1);
+		//		an[3*c+2] = model->points[c*step].normal(2);
+
+		//		ac[3*c+0] =model->points[c*step].feature(0);
+		//		ac[3*c+1] =model->points[c*step].feature(1);
+		//		ac[3*c+2] =model->points[c*step].feature(2);
+
+		//		ai[c] = 1.0/model->points[c*step].point_information;//1.0/(z*z);
+
+		//		X(0,c)	= ap[3*c+0];
+		//		X(1,c)	= ap[3*c+1];
+		//		X(2,c)	= ap[3*c+2];
+		//		Xn(0,c)	= an[3*c+0];
+		//		Xn(1,c)	= an[3*c+1];
+		//		Xn(2,c)	= an[3*c+2];
+
+		//		information(c) = ai[c];//1.0/(z*z);
+		//		C(0,c) = ac[3*c+0];
+		//		C(1,c) = ac[3*c+1];
+		//		C(2,c) = ac[3*c+2];
+	}
+
+	informations[i] = information;
+
+	ArrayData3D<double> * a3d = new ArrayData3D<double>;
+	a3d->data	= ap;
+	a3d->rows	= count;
+	a3dv[i]		= a3d;
+	trees3d[i]	= new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+	trees3d[i]->buildIndex();
+
+	/*
+	long depthedge_count = 0;
+	for(unsigned long w = 0; w < width; w++){
+		for(unsigned long h = 0; h < height; h++){
+			long ind = h*width+w;
+			if(maskvec[ind] && edgedata[ind] == 255){
+				float z = idepth*float(depthdata[ind]);
+				if(z > 0.2){depthedge_count++;}
+			}
+		}
+	}
+
+	if(depthedge_count < 10){
+		double * depthedge_ap = new double[3*depthedge_count];
+		double * depthedge_ai = new double[3*depthedge_count];
+		depthedge_nr_arraypoints[i] = depthedge_count;
+		depthedge_arraypoints[i] = depthedge_ap;
+		depthedge_arrayinformations[i] = depthedge_ai;
+
+		c = 0;
+		for(unsigned long w = 0; w < width; w++){
+			for(unsigned long h = 0; h < height; h++){
+				if(c == depthedge_count){continue;}
+				long ind = h*width+w;
+				if(maskvec[ind] && edgedata[ind] == 255){
+					float z = idepth*float(depthdata[ind]);
+					if(z > 0.2){
+						depthedge_ap[3*c+0] = (w - cx) * z * ifx;
+						depthedge_ap[3*c+1] = (h - cy) * z * ify;;
+						depthedge_ap[3*c+2] = z;
+						depthedge_ai[c] = pow(fabs(z),-2);//1.0/(z*z);
+						c++;
+					}
+				}
+			}
+		}
+
+		ArrayData3D<double> * depthedge_a3d = new ArrayData3D<double>;
+		depthedge_a3d->data					= depthedge_ap;
+		depthedge_a3d->rows					= depthedge_count;
+		depthedge_a3dv[i]					= depthedge_a3d;
+		depthedge_trees3d[i]				= new Tree3d(3, *depthedge_a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+		depthedge_trees3d[i]->buildIndex();
+	}
+*/
+	//printf("addModel total load time:          %5.5f points: %6.6i\n",getTime()-total_load_time_start,count);
+}
+
+void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){
+	model = model_;
+	printf("addModelData\n");
+
+	if(submodels){
+		for(unsigned long i = 0; i < model->submodels.size(); i++){
+			addData(model->submodels[i]->getPCLnormalcloud(1,false));
+		}
+	}else{
+		//setData(model->frames,model->modelmasks);
+
+		for(unsigned long i = 0; i < model->submodels.size(); i++){
+			addData(model->submodels[i]->getPCLnormalcloud(1,false));
+			//            kp_nr_arraypoints.push_back(0);
+			//            kp_arraypoints.push_back(0);
+			//            kp_arraynormals.push_back(0);
+			//            kp_arrayinformations.push_back(0);
+			//            kp_arraydescriptors.push_back(0);
+			//            frameid.push_back(-1);
+		}
+
+		unsigned long nr_frames = model->frames.size();
+
+		for(unsigned long i = 0; i < nr_frames; i++){
+			//            frameid.push_back(i);
+			addData(model->frames[i], model->modelmasks[i]);
+
+			//			RGBDFrame* frame = model->frames[i];
+			//			std::vector<cv::KeyPoint> & keypoints = model->all_keypoints[i];
+			//			cv::Mat & descriptors = model->all_descriptors[i];
+			//			//uint64_t * descriptorsdata		= (uint64_t			*)(descriptors.data);
+
+
+			//			float		   * normalsdata	= (float			*)(frame->normals.data);
+			//			unsigned short * depthdata		= (unsigned short	*)(frame->depth.data);
+
+			//			Camera * camera				= frame->camera;
+			//			const unsigned long width	= camera->width;
+			//			const unsigned long height	= camera->height;
+			//			const float idepth			= camera->idepth_scale;
+			//			const float cx				= camera->cx;
+			//			const float cy				= camera->cy;
+			//			const float ifx				= 1.0/camera->fx;
+			//			const float ify				= 1.0/camera->fy;
+
+			//			unsigned long nr_kp = keypoints.size();
+			//			if(nr_kp){
+			//				double * ap = new double[3*nr_kp];
+			//				double * an = new double[3*nr_kp];
+			//				double * ai = new double[nr_kp];
+			//				for(unsigned long k = 0; k < nr_kp; k++){
+			//					cv::KeyPoint & kp = keypoints[k];
+			//					double w = kp.pt.x;
+			//					double h = kp.pt.y;
+			//					long ind = int(h+0.5)*width+int(w+0.5);
+
+			//					float z = idepth*float(depthdata[ind]);
+			//					float x = (w - cx) * z * ifx;
+			//					float y = (h - cy) * z * ify;
+
+			//					ap[3*k+0] =x;
+			//					ap[3*k+1] =y;
+			//					ap[3*k+2] =z;
+
+			//					an[3*k+0] = normalsdata[3*ind+0];
+			//					an[3*k+1] = normalsdata[3*ind+1];
+			//					an[3*k+2] = normalsdata[3*ind+2];
+
+			//					ai[k] = pow(fabs(z),-2);
+			//				}
+
+			//				kp_nr_arraypoints.push_back(nr_kp);
+			//				kp_arraypoints.push_back(ap);
+			//				kp_arraynormals.push_back(an);
+			//				kp_arrayinformations.push_back(ai);
+			//				kp_arraydescriptors.push_back((uint64_t			*)(descriptors.data));
+			//			}else{
+			//				kp_nr_arraypoints.push_back(0);
+			//				kp_arraypoints.push_back(0);
+			//				kp_arraynormals.push_back(0);
+			//				kp_arrayinformations.push_back(0);
+			//				kp_arraydescriptors.push_back(0);
+			//			}
+		}
+
+		//        for(long i = 0; i < is_ok.size(); i++){
+		//            if(is_ok[i]){printf("%i is ok\n",i);
+		//            }else{printf("%i is not ok\n",i);}
+		//        }
+
+
+	}
+}
+
+
+void MassRegistrationPPR2::setData(std::vector< pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > all_clouds){
+	double total_load_time_start = getTime();
+	unsigned long nr_frames = all_clouds.size();
+
+	if(arraypoints.size() > 0){
+		for(unsigned long i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];}
+		arraypoints.resize(0);
+	}
+
+	if(a3dv.size() > 0){
+		for(unsigned long i = 0; i < a3dv.size(); i++){delete a3dv[i];}
+		a3dv.resize(0);
+	}
+
+	if(trees3d.size() > 0){
+		for(unsigned long i = 0; i < trees3d.size(); i++){delete trees3d[i];}
+		trees3d.resize(0);
+	}
+
+	nr_matches.resize(nr_frames);
+	matchids.resize(nr_frames);
+	matchdists.resize(nr_frames);
+	nr_datas.resize(nr_frames);
+	points.resize(nr_frames);
+	colors.resize(nr_frames);
+	normals.resize(nr_frames);
+	transformed_points.resize(nr_frames);
+	transformed_normals.resize(nr_frames);
+	informations.resize(nr_frames);
+
+	nr_arraypoints.resize(nr_frames);
+
+	arraypoints.resize(nr_frames);
+	arraynormals.resize(nr_frames);
+	arraycolors.resize(nr_frames);
+	arrayinformations.resize(nr_frames);
+
+	trees3d.resize(nr_frames);
+	a3dv.resize(nr_frames);
+	is_ok.resize(nr_frames);
+
+	for(unsigned long i = 0; i < nr_frames; i++){
+		//printf("loading data for %i\n",i);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = all_clouds[i];
+		long count = 0;
+		for(unsigned long i = 0; i < cloud->points.size(); i++){
+			if (isValidPoint(cloud->points[i])){count++;}
+		}
+
+		if(count < 10){
+			is_ok[i] = false;
+			continue;
+		}else{
+			is_ok[i] = true;
+		}
+
+		double * ap = new double[3*count];
+		double * an = new double[3*count];
+		double * ac = new double[3*count];
+		double * ai = new double[3*count];
+
+		nr_datas[i] = count;
+		matchids[i].resize(nr_frames);
+		matchdists[i].resize(nr_frames);
+		points[i].resize(Eigen::NoChange,count);
+		colors[i].resize(Eigen::NoChange,count);
+		normals[i].resize(Eigen::NoChange,count);
+		transformed_points[i].resize(Eigen::NoChange,count);
+		transformed_normals[i].resize(Eigen::NoChange,count);
+
+		nr_arraypoints[i] = count;
+
+		arraypoints[i] = ap;
+		arraynormals[i] = an;
+		arraycolors[i] = ac;
+		arrayinformations[i] = ai;
+
+		Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
+		Eigen::Matrix<double, 3, Eigen::Dynamic> & C	= colors[i];
+		Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
+
+		Eigen::VectorXd information (count);
+
+		long c = 0;
+		for(unsigned long i = 0; i < cloud->points.size(); i++){
+			pcl::PointXYZRGBNormal p = cloud->points[i];
+			if (isValidPoint(p)){
+
+				float xn = p.normal_x;
+				float yn = p.normal_y;
+				float zn = p.normal_z;
+
+				float x = p.x;
+				float y = p.y;
+				float z = p.z;
+
+				ap[3*c+0] =x;
+				ap[3*c+1] =y;
+				ap[3*c+2] =z;
+
+				an[3*c+0] =xn;
+				an[3*c+1] =yn;
+				an[3*c+2] =zn;
+
+				ac[3*c+0] =p.r;
+				ac[3*c+1] =p.g;
+				ac[3*c+2] =p.b;
+
+				ai[c] = 1.0/(x*x+y*y+z*z);
+
+				X(0,c)	= x;
+				X(1,c)	= y;
+				X(2,c)	= z;
+				Xn(0,c)	= xn;
+				Xn(1,c)	= yn;
+				Xn(2,c)	= zn;
+
+				information(c) = ai[c];//1.0/(z*z);
+				C(0,c) = p.r;
+				C(1,c) = p.g;
+				C(2,c) = p.b;
+				c++;
+
+			}
+		}
+
+		informations[i] = information;
+
+		ArrayData3D<double> * a3d = new ArrayData3D<double>;
+		a3d->data	= ap;
+		a3d->rows	= count;
+		a3dv[i]		= a3d;
+		trees3d[i]	= new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+		trees3d[i]->buildIndex();
+	}
+
+	//printf("total load time:          %5.5f\n",getTime()-total_load_time_start);
+}
+
+void MassRegistrationPPR2::setData(std::vector<RGBDFrame*> frames_,std::vector<ModelMask *> mmasks_){
+	double total_load_time_start = getTime();
+
+	unsigned long nr_frames = frames_.size();
+	for(unsigned long i = 0; i < nr_frames; i++){addData(frames_[i], mmasks_[i]);}
+}
+
+double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ap, double * ap, std::vector<long > & matchid, std::vector<double> & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){
+	const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+	const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+	const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+	matchid.resize(nr_ap);
+	matchdist.resize(nr_ap);
+
+	double threshold = pow(10*f->getNoise(),2);
+	double good = 0;
+	double bad = 0;
+
+#if defdomultithread
+#pragma omp parallel for num_threads(8)
+#endif
+	for(unsigned long k = 0; k < nr_ap; ++k) {
+		long prev = matchid[k];
+		double qp [3];
+		const double & src_x = ap[3*k+0];
+		const double & src_y = ap[3*k+1];
+		const double & src_z = ap[3*k+2];
+		qp[0] = m00*src_x + m01*src_y + m02*src_z + m03;
+		qp[1] = m10*src_x + m11*src_y + m12*src_z + m13;
+		qp[2] = m20*src_x + m21*src_y + m22*src_z + m23;
+
+		size_t ret_index; double out_dist_sqr;
+		nanoflann::KNNResultSet<double> resultSet(1);
+		resultSet.init(&ret_index, &out_dist_sqr );
+		t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
+
+		if(out_dist_sqr < threshold){
+			good++;
+		}else{
+			bad++;
+		}
+		long current = ret_index;
+		new_good_rematches += prev != current;
+		new_total_rematches++;
+		matchid[k] = current;
+		matchdist[k] = out_dist_sqr;
+	}
+	return good/(good+bad+0.001);
+}
+
+void MassRegistrationPPR2::rematch(std::vector<Eigen::Matrix4d> poses, std::vector<Eigen::Matrix4d> prev_poses, bool rematch_surface, bool rematch_edges, bool first){
+	if(!rematch_surface && !rematch_edges){return;}
+	double new_good_rematches = 0;
+	double new_total_rematches = 0;
+	unsigned long nr_frames = poses.size();
+
+	long rmt = 2;
+
+	if(rmt==2){
+		long overlapping = 0;
+
+		long work_done = 0;
+		long ignores_motion = 0;
+		long ignores_overlap = 0;
+		for(unsigned long i = 0; i < nr_frames; i++){
+			if(!is_ok[i]){continue;}
+			nr_matches[i] = 0;
+
+			for(unsigned long j = 0; j < nr_frames; j++){
+				if(!is_ok[j]){continue;}
+				if(i == j){continue;}
+				if((matchscores[i][j] <= 0.001) && (matchscores[j][i] <= 0.001) && (rand()%nr_frames > 1)){
+					ignores_overlap++;
+					continue;
+				}
+
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+				if(false && !first){
+					Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]);
+					Eigen::Affine3d diff = prev_rp.inverse()*rp;
+
+					double change_trans = 0;
+					double change_rot = 0;
+					double dt = 0;
+					for(unsigned long k = 0; k < 3; k++){
+						dt += diff(k,3)*diff(k,3);
+						for(unsigned long l = 0; l < 3; l++){
+							if(k == l){ change_rot += fabs(1-diff(k,l));}
+							else{		change_rot += fabs(diff(k,l));}
+						}
+					}
+					change_trans += sqrt(dt);
+
+					if(change_trans < 1.0*stopval && change_rot < 1.0*stopval){ignores_motion++;continue;}
+				}
+
+
+
+				double ratiosum = 0;
+				if(rematch_edges && depthedge_nr_arraypoints[i] > 10 && depthedge_nr_arraypoints[j] > 10){
+					ratiosum += matchframes(depthedge_func,rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j],	new_good_rematches,new_total_rematches);
+				}
+				if(rematch_surface && nr_arraypoints[i] > 10 && nr_arraypoints[j] > 10){
+					ratiosum += matchframes(func,rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j],	new_good_rematches,new_total_rematches);
+				}
+
+				matchscores[i][j] = ratiosum;
+				overlapping += ratiosum > 0;
+				work_done++;
+			}
+		}
+
+		//printf("ignores_overlap: %i/%i -> %5.5f\n",ignores_overlap,nr_frames*nr_frames - nr_frames, double(ignores_overlap)/double(nr_frames*nr_frames - nr_frames));
+		//printf("ignores_motion:  %i/%i -> %5.5f\n",ignores_motion,nr_frames*nr_frames - nr_frames, double(ignores_motion)/double(nr_frames*nr_frames - nr_frames));
+		//printf("total work done: %i/%i -> %5.5f\n",work_done,nr_frames*nr_frames - nr_frames, double(work_done)/double(nr_frames*nr_frames - nr_frames));
+	}
+
+	if(rmt==1){
+		for(unsigned long i = 0; i < nr_frames; i++){
+			if(!is_ok[i]){continue;}
+			nr_matches[i] = 0;
+
+			double * ap = arraypoints[i];
+			const unsigned long nr_ap = nr_arraypoints[i];
+
+			for(unsigned long j = 0; j < nr_frames; j++){
+				if(!is_ok[j]){continue;}
+				if(i == j){continue;}
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+				const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+				const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+				const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+				std::vector<long > & matchid = matchids[i][j];
+				matchid.resize(nr_ap);
+				Tree3d * t3d = trees3d[j];
+
+				for(unsigned long k = 0; k < nr_ap; ++k) {
+					long prev = matchid[k];
+					double qp [3];
+					const double & src_x = ap[3*k+0];
+					const double & src_y = ap[3*k+1];
+					const double & src_z = ap[3*k+2];
+
+					qp[0] = m00*src_x + m01*src_y + m02*src_z + m03;
+					qp[1] = m10*src_x + m11*src_y + m12*src_z + m13;
+					qp[2] = m20*src_x + m21*src_y + m22*src_z + m23;
+
+					size_t ret_index; double out_dist_sqr;
+					nanoflann::KNNResultSet<double> resultSet(1);
+					resultSet.init(&ret_index, &out_dist_sqr );
+					t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
+
+					long current = ret_index;
+					new_good_rematches += prev != current;
+					new_total_rematches++;
+					matchid[k] = current;
+				}
+				nr_matches[i] += matchid.size();
+			}
+		}
+	}
+	if(rmt==0){
+		for(unsigned long i = 0; i < nr_frames; i++){
+			if(!is_ok[i]){continue;}
+			nr_matches[i] = 0;
+
+			double * ap = arraypoints[i];
+			long nr_ap = nr_arraypoints[i];
+
+			for(unsigned long j = 0; j < nr_frames; j++){
+				if(!is_ok[j]){continue;}
+				if(i == j){continue;}
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+				const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+				const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+				const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+				Eigen::Matrix<double, 3, Eigen::Dynamic> tX	= rp*points[i];
+
+				unsigned long nr_data = nr_datas[i];
+				std::vector<long > & matchid = matchids[i][j];
+				matchid.resize(nr_data);
+				Tree3d * t3d = trees3d[j];
+
+				for(unsigned long k = 0; k < nr_data; ++k) {
+					long prev = matchid[k];
+					double * qp = tX.col(k).data();
+
+					size_t ret_index; double out_dist_sqr;
+					nanoflann::KNNResultSet<double> resultSet(1);
+					resultSet.init(&ret_index, &out_dist_sqr );
+					t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
+
+					long current = ret_index;
+					new_good_rematches += prev != current;
+					new_total_rematches++;
+					matchid[k] = current;
+				}
+				nr_matches[i] += matchid.size();
+			}
+		}
+	}
+	//	good_rematches += new_good_rematches;
+	//	total_rematches += new_total_rematches
+}
+
+
+Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector<Eigen::Matrix4d> poses){
+	unsigned long nr_frames = poses.size();
+	Eigen::MatrixXd all_residuals;
+
+	long total_matches = 0;
+	for(unsigned long i = 0; i < nr_frames; i++){
+		if(!is_ok[i]){continue;}
+		for(unsigned long j = 0; j < nr_frames; j++){
+			if(!is_ok[j]){continue;}
+			total_matches += matchids[i][j].size();
+		}
+	}
+
+	switch(type) {
+	case PointToPoint:	{all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break;
+	case PointToPlane:	{all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break;
+	default:			{printf("type not set\n");}					break;
+	}
+
+	long count = 0;
+	for(unsigned long i = 0; i < nr_frames; i++){
+		if(!is_ok[i]){continue;}
+
+		double * api = arraypoints[i];
+		double * ani = arraynormals[i];
+		double * aii = arrayinformations[i];
+		const unsigned long nr_api = nr_arraypoints[i];
+		for(unsigned long j = 0; j < nr_frames; j++){
+			if(!is_ok[j]){continue;}
+			if(i == j){continue;}
+
+			double * apj = arraypoints[j];
+			double * aij = arrayinformations[j];
+			const unsigned long nr_apj = nr_arraypoints[j];
+
+			std::vector<long > & matchidi = matchids[i][j];
+
+			Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+			const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+			const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+			const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+			if(type == PointToPlane){
+				for(unsigned long ki = 0; ki < nr_api; ++ki) {
+					long kj = matchidi[ki];
+					if( kj < 0 || kj >= nr_apj){continue;}
+					const double & src_x = api[3*ki+0];
+					const double & src_y = api[3*ki+1];
+					const double & src_z = api[3*ki+2];
+
+					const double & src_nx = ani[3*ki+0];
+					const double & src_ny = ani[3*ki+1];
+					const double & src_nz = ani[3*ki+2];
+
+					const double & info_i = aii[ki];
+
+					const double & dst_x = apj[3*kj+0];
+					const double & dst_y = apj[3*kj+1];
+					const double & dst_z = apj[3*kj+2];
+
+
+					const double & info_j = aij[kj];
+
+					float tx = m00*src_x + m01*src_y + m02*src_z + m03;
+					float ty = m10*src_x + m11*src_y + m12*src_z + m13;
+					float tz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+					float txn = m00*src_nx + m01*src_ny + m02*src_nz;
+					float tyn = m10*src_nx + m11*src_ny + m12*src_nz;
+					float tzn = m20*src_nx + m21*src_ny + m22*src_nz;
+
+					const double rw = 1.0/(1.0/info_i+1.0/info_j);
+					const double di = (txn*(tx-dst_x) + tyn*(ty-dst_y) + tzn*(tz-dst_z))*rw;
+					all_residuals(0,count) = di;
+					count++;
+				}
+			}
+
+			if(type == PointToPoint){
+				for(unsigned long ki = 0; ki < nr_api; ++ki) {
+					long kj = matchidi[ki];
+					if( kj < 0 ){continue;}
+					const double & src_x = api[3*ki+0];
+					const double & src_y = api[3*ki+1];
+					const double & src_z = api[3*ki+2];
+
+					const double & info_i = aii[ki];
+
+					const double & dst_x = apj[3*kj+0];
+					const double & dst_y = apj[3*kj+1];
+					const double & dst_z = apj[3*kj+2];
+
+					const double & info_j = aij[kj];
+
+					float tx = m00*src_x + m01*src_y + m02*src_z + m03;//        const unsigned long src_nr_ap = kp_nr_arraypoints[i];
+					//        if(src_nr_ap == 0){continue;}
+					float ty = m10*src_x + m11*src_y + m12*src_z + m13;
+					float tz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+					const double rw = 1.0/(1.0/info_i+1.0/info_j);
+					all_residuals(0,count) = (tx-dst_x)*rw;
+					all_residuals(1,count) = (ty-dst_y)*rw;
+					all_residuals(2,count) = (tz-dst_z)*rw;
+					count++;
+				}
+			}
+		}
+	}
+	return all_residuals;
+}
+
+Eigen::MatrixXd MassRegistrationPPR2::depthedge_getAllResiduals(std::vector<Eigen::Matrix4d> poses){
+	unsigned long nr_frames = poses.size();
+	Eigen::MatrixXd all_residuals;
+
+	long total_matches = 0;
+	for(unsigned long i = 0; i < nr_frames; i++){
+		if(!is_ok[i]){continue;}
+		for(unsigned long j = 0; j < nr_frames; j++){
+			if(!is_ok[j]){continue;}
+			total_matches += depthedge_matchids[i][j].size();
+		}
+	}
+
+
+	all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);
+
+
+	long count = 0;
+	for(unsigned long i = 0; i < nr_frames; i++){
+		if(!is_ok[i]){continue;}
+
+		double * api = depthedge_arraypoints[i];
+		double * aii = depthedge_arrayinformations[i];
+		const unsigned long nr_api = depthedge_nr_arraypoints[i];
+		for(unsigned long j = 0; j < nr_frames; j++){
+			if(!is_ok[j]){continue;}
+			if(i == j){continue;}
+
+			double * apj = depthedge_arraypoints[j];
+			double * aij = depthedge_arrayinformations[j];
+			const unsigned long nr_apj = depthedge_nr_arraypoints[j];
+
+			std::vector<long > & matchidi = depthedge_matchids[i][j];
+			unsigned long matchesi = matchidi.size();
+
+			if(matchesi != nr_api){continue;}
+
+			Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+			const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+			const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+			const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+
+			for(unsigned long ki = 0; ki < nr_api; ++ki) {
+				long kj = matchidi[ki];
+				if( kj < 0 ){continue;}
+				const double & src_x = api[3*ki+0];
+				const double & src_y = api[3*ki+1];
+				const double & src_z = api[3*ki+2];
+
+				const double & info_i = aii[ki];
+
+				const double & dst_x = apj[3*kj+0];
+				const double & dst_y = apj[3*kj+1];
+				const double & dst_z = apj[3*kj+2];
+
+				const double & info_j = aij[kj];
+
+				float tx = m00*src_x + m01*src_y + m02*src_z + m03;
+				float ty = m10*src_x + m11*src_y + m12*src_z + m13;
+				float tz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+				const double rw = 1.0/(1.0/info_i+1.0/info_j);
+				all_residuals(0,count) = (tx-dst_x)*rw;
+				all_residuals(1,count) = (ty-dst_y)*rw;
+				all_residuals(2,count) = (tz-dst_z)*rw;
+				count++;
+			}
+		}
+	}
+	return all_residuals;
+}
+
+Eigen::MatrixXd MassRegistrationPPR2::getAllKpResiduals(std::vector<Eigen::Matrix4d> poses){
+	unsigned long nr_frames = poses.size();
+
+
+	//    printf("is_ok size: %i\n",is_ok.size());
+	long total_matches = 0;
+	for(unsigned long i = 0; i < nr_frames; i++){
+		const unsigned long src_nr_ap = kp_nr_arraypoints[i];
+		if(src_nr_ap == 0){continue;}
+		for(unsigned long j = 0; j < nr_frames; j++){
+			const unsigned long dst_nr_ap = kp_nr_arraypoints[j];
+			if(dst_nr_ap == 0){continue;}
+			total_matches += kp_matches[i][j].size();
+		}
+	}
+
+
+
+	//    printf("total matches: %i\n",total_matches);
+
+
+	//    total_matches = 0;
+	//    for(unsigned long i = 0; i < nr_frames; i++){
+	//        for(unsigned long j = 0; j < nr_frames; j++){
+	//            total_matches += kp_matches[i][j].size();
+	//        }
+	//    }
+
+
+	//    printf("total matches: %i\n",total_matches);
+
+	//    exit(0);
+
+	long count = 0;
+	Eigen::MatrixXd all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);
+	//if(total_matches == 0){return all_residuals;}
+
+	long ignores = 0;
+	for(unsigned long i = 0; i < nr_frames; i++){
+		const unsigned long src_nr_ap = kp_nr_arraypoints[i];
+		if(src_nr_ap == 0){continue;}
+		double * src_ap = kp_arraypoints[i];
+		double * src_ai = kp_arrayinformations[i];
+
+		for(unsigned long j = 0; j < nr_frames; j++){
+			if(i == j){continue;}
+
+			const unsigned long dst_nr_ap = kp_nr_arraypoints[j];
+			if(dst_nr_ap == 0){continue;}
+
+			//            printf("nr kp(%i %i): %i %i\n",i,j,src_nr_ap,dst_nr_ap);
+			double * dst_ap = kp_arraypoints[j];
+			double * dst_ai = kp_arrayinformations[j];
+
+			Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+
+			const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+			const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+			const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+			std::vector< TestMatch > & matches = kp_matches[i][j];
+			unsigned long nr_matches = matches.size();
+
+			for(unsigned long k = 0; k < nr_matches; ++k) {
+				TestMatch & tm = matches[k];
+				unsigned long src_k = tm.src;
+				unsigned long dst_k = tm.dst;
+
+				const double & src_x = src_ap[3*src_k+0];
+				const double & src_y = src_ap[3*src_k+1];
+				const double & src_z = src_ap[3*src_k+2];
+				const double & src_info = src_ai[src_k];
+
+				double sx = m00*src_x + m01*src_y + m02*src_z + m03;
+				double sy = m10*src_x + m11*src_y + m12*src_z + m13;
+				double sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+				double dx = sx-dst_ap[3*dst_k+0];
+				double dy = sy-dst_ap[3*dst_k+1];
+				double dz = sz-dst_ap[3*dst_k+2];
+
+				const double & dst_info = dst_ai[dst_k];
+				const double rw = 1.0/(1.0/src_info+1.0/dst_info);
+				//                if(total_matches <= count){printf("strange\n");}
+				all_residuals(0,count) = dx*rw;
+				all_residuals(1,count) = dy*rw;
+				all_residuals(2,count) = dz*rw;
+				count++;
+			}
+		}
+	}
+	return all_residuals;
+}
+
+void MassRegistrationPPR2::clearData(){
+	rematch_time = 0;
+	residuals_time = 0;
+	opt_time = 0;
+	computeModel_time = 0;
+	setup_matches_time = 0;
+	setup_equation_time = 0;
+	setup_equation_time2 = 0;
+	solve_equation_time = 0;
+	total_time_start = 0;
+
+	nr_datas.clear();
+	is_ok.clear();
+
+	points.clear();
+	colors.clear();
+	normals.clear();
+	transformed_points.clear();
+	transformed_normals.clear();
+	informations.clear();
+
+	kp_nr_arraypoints.clear();
+
+	for(size_t i = 0; i < kp_arraypoints.size(); i++){
+		if(kp_arraypoints[i] != 0){
+			delete[] kp_arraypoints[i];
+			kp_arraypoints[i] = 0;
+		}
+	}
+	kp_arraypoints.clear();
+
+	for(size_t i = 0; i < kp_arraynormals.size(); i++){
+		if(kp_arraynormals[i] != 0){
+			delete[] kp_arraynormals[i];
+			kp_arraynormals[i] = 0;
+		}
+	}
+	kp_arraynormals.clear();
+
+	for(size_t i = 0; i < kp_arrayinformations.size(); i++){
+		if(kp_arrayinformations[i] != 0){
+			delete[] kp_arrayinformations[i];
+			kp_arrayinformations[i] = 0;
+		}
+	}
+	kp_arrayinformations.clear();
+
+
+	for(size_t i = 0; i < kp_arraydescriptors.size(); i++){
+		if(kp_arraydescriptors[i] != 0){
+			delete[] kp_arraydescriptors[i];
+			kp_arraydescriptors[i] = 0;
+		}
+	}
+	kp_arraydescriptors.clear();
+
+
+	kp_matches.clear();
+	//    double * kp_Qp_arr;
+	//    double * kp_Qn_arr;
+	//    double * kp_Xp_arr;
+	//    double * kp_Xn_arr;
+	//    double * kp_rangeW_arr;
+	//    DistanceWeightFunction2PPR2 * kpfunc;
+
+	frameid.clear();
+
+	nr_arraypoints.clear();
+
+	for(size_t i = 0; i < arraypoints.size(); i++){
+		if(arraypoints[i] != 0){
+			delete[] arraypoints[i];
+			arraypoints[i] = 0;
+		}
+	}
+	arraypoints.clear();
+
+	for(size_t i = 0; i < arraynormals.size(); i++){
+		if(arraynormals[i] != 0){
+			delete[] arraynormals[i];
+			arraynormals[i] = 0;
+		}
+	}
+	arraynormals.clear();
+
+	for(size_t i = 0; i < arraycolors.size(); i++){
+		if(arraycolors[i] != 0){
+			delete[] arraycolors[i];
+			arraycolors[i] = 0;
+		}
+	}
+	arraycolors.clear();
+
+	for(size_t i = 0; i < arrayinformations.size(); i++){
+		if(arrayinformations[i] != 0){
+			delete[] arrayinformations[i];
+			arrayinformations[i] = 0;
+		}
+	}
+	arrayinformations.clear();
+
+	for(size_t i = 0; i < trees3d.size(); i++){
+		if(trees3d[i] != 0){
+			delete trees3d[i];
+			trees3d[i] = 0;
+		}
+	}
+	trees3d.clear();
+
+	for(size_t i = 0; i < a3dv.size(); i++){
+		if(a3dv[i] != 0){
+			delete a3dv[i];
+			a3dv[i] = 0;
+		}
+	}
+	a3dv.clear();
+
+	nr_matches.clear();
+	matchids.clear();
+	matchdists.clear();
+	//    double * Qp_arr;
+	//    double * Qn_arr;
+	//    double * Xp_arr;
+	//    double * Xn_arr;
+	//    double * rangeW_arr;
+	//    DistanceWeightFunction2PPR2 * func;
+	matchscores.clear();
+
+	depthedge_nr_arraypoints.clear();
+
+	for(size_t i = 0; i < depthedge_arraypoints.size(); i++){
+		if(depthedge_arraypoints[i] != 0){
+			delete[] depthedge_arraypoints[i];
+			depthedge_arraypoints[i] = 0;
+		}
+	}
+	depthedge_arraypoints.clear();
+
+	for(size_t i = 0; i < depthedge_arrayinformations.size(); i++){
+		if(depthedge_arrayinformations[i] != 0){
+			delete[] depthedge_arrayinformations[i];
+			depthedge_arrayinformations[i] = 0;
+		}
+	}
+	depthedge_arrayinformations.clear();
+
+	for(size_t i = 0; i < depthedge_trees3d.size(); i++){
+		if(depthedge_trees3d[i] != 0){
+			delete depthedge_trees3d[i];
+			depthedge_trees3d[i] = 0;
+		}
+	}
+	depthedge_trees3d.clear();
+
+	for(size_t i = 0; i < depthedge_a3dv.size(); i++){
+		if(depthedge_a3dv[i] != 0){
+			delete depthedge_a3dv[i];
+			depthedge_a3dv[i] = 0;
+		}
+	}
+	depthedge_a3dv.clear();
+
+	depthedge_nr_matches.clear();
+	depthedge_matchids.clear();
+	depthedge_matchdists.clear();;
+	//    double * depthedge_Qp_arr;
+	//    double * depthedge_Xp_arr;
+	//    double * depthedge_rangeW_arr;
+	//    DistanceWeightFunction2PPR2 * depthedge_func;
+
+	sweepids.clear();;
+	background_nr_datas.clear();
+}
+
+void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){
+
+	double total_load_time_start = getTime();
+	frames.push_back(frame);
+	mmasks.push_back(mmask);
+
+	nr_datas.push_back(		0);
+	points.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	colors.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	normals.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_points.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_normals.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+
+	informations.push_back(			Eigen::VectorXd());
+
+	nr_matches.push_back(	0);
+	matchids.push_back(		std::vector< std::vector<long > >() );
+	matchdists.push_back(	std::vector< std::vector<double> >() );
+
+	nr_arraypoints.push_back(0);
+	arraypoints.push_back(0);
+	arraynormals.push_back(0);
+	arraycolors.push_back(0);
+	arrayinformations.push_back(0);
+	trees3d.push_back(0);
+	a3dv.push_back(0);
+
+	depthedge_nr_matches.push_back(	0);
+	depthedge_matchids.push_back(		std::vector< std::vector<long > >() );
+	depthedge_matchdists.push_back(	std::vector< std::vector<double> >() );
+
+	//depthedge_neighbours.push_back(0);
+	depthedge_nr_arraypoints.push_back(0);
+	depthedge_arraypoints.push_back(0);
+	depthedge_arrayinformations.push_back(0);
+	depthedge_trees3d.push_back(0);
+	depthedge_a3dv.push_back(0);
+
+
+	is_ok.push_back(false);
+
+	unsigned long i = points.size()-1;
+
+	bool * maskvec					= mmask->maskvec;
+	unsigned char *  edgedata		= (unsigned char *)(frame->depthedges.data);
+	unsigned char  * rgbdata		= (unsigned char	*)(frame->rgb.data);
+	unsigned short * depthdata		= (unsigned short	*)(frame->depth.data);
+	float		   * normalsdata	= (float			*)(frame->normals.data);
+
+	Camera * camera				= frame->camera;
+	const unsigned long width	= camera->width;
+	const unsigned long height	= camera->height;
+	const float idepth			= camera->idepth_scale;
+	const float cx				= camera->cx;
+	const float cy				= camera->cy;
+	const float ifx				= 1.0/camera->fx;
+	const float ify				= 1.0/camera->fy;
+
+	long count = 0;
+	for(unsigned long w = 0; w < width; w+=maskstep){
+		for(unsigned long h = 0; h < height; h+=maskstep){
+			long ind = h*width+w;
+			if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){
+				float z = idepth*float(depthdata[ind]);
+				float xn = normalsdata[3*ind+0];
+				if(z > 0.2 && xn != 2){count++;}
+			}
+		}
+	}
+
+
+	if(count < 10){
+		is_ok[i] = false;
+		return;
+	}else{
+		is_ok[i] = true;
+	}
+
+	//    printf("%i is ok %i\n",i,is_ok[i]);
+
+	nr_datas[i] = count;
+	points[i].resize(Eigen::NoChange,count);
+	colors[i].resize(Eigen::NoChange,count);
+	normals[i].resize(Eigen::NoChange,count);
+	transformed_points[i].resize(Eigen::NoChange,count);
+	transformed_normals[i].resize(Eigen::NoChange,count);
+
+	double * ap = new double[3*count];
+	double * an = new double[3*count];
+	double * ac = new double[3*count];
+	double * ai = new double[3*count];
+	nr_arraypoints[i] = count;
+	arraypoints[i] = ap;
+	arraynormals[i] = an;
+	arraycolors[i] = ac;
+	arrayinformations[i] = ai;
+
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & C	= colors[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & tX	= transformed_points[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & tXn	= transformed_normals[i];
+	Eigen::VectorXd information (count);
+
+	long c = 0;
+	for(unsigned long w = 0; w < width; w+=maskstep){
+		for(unsigned long h = 0; h < height;h+=maskstep){
+			if(c == count){continue;}
+			long ind = h*width+w;
+			//if(maskvec[ind] || nomask){
+			if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){
+				float z = idepth*float(depthdata[ind]);
+				float xn = normalsdata[3*ind+0];
+
+				if(z > 0.2 && xn != 2){
+					float yn = normalsdata[3*ind+1];
+					float zn = normalsdata[3*ind+2];
+
+					float x = (w - cx) * z * ifx;
+					float y = (h - cy) * z * ify;
+
+					ap[3*c+0] =x;
+					ap[3*c+1] =y;
+					ap[3*c+2] =z;
+
+					an[3*c+0] =xn;
+					an[3*c+1] =yn;
+					an[3*c+2] =zn;
+
+					ac[3*c+0] =rgbdata[3*ind+2];
+					ac[3*c+1] =rgbdata[3*ind+1];
+					ac[3*c+2] =rgbdata[3*ind+0];
+
+					ai[c] = pow(fabs(z),-2);//1.0/(z*z);
+
+					X(0,c)	= x;
+					X(1,c)	= y;
+					X(2,c)	= z;
+					Xn(0,c)	= xn;
+					Xn(1,c)	= yn;
+					Xn(2,c)	= zn;
+
+					information(c) = ai[c];//1.0/(z*z);
+					C(0,c) = rgbdata[3*ind+0];
+					C(1,c) = rgbdata[3*ind+1];
+					C(2,c) = rgbdata[3*ind+2];
+					c++;
+				}
+			}
+		}
+	}
+	informations[i] = information;
+
+	ArrayData3D<double> * a3d = new ArrayData3D<double>;
+	a3d->data	= ap;
+	a3d->rows	= count;
+	a3dv[i]		= a3d;
+	trees3d[i]	= new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+	trees3d[i]->buildIndex();
+
+
+	long depthedge_count = 0;
+	for(unsigned long w = 0; w < width; w++){
+		for(unsigned long h = 0; h < height; h++){
+			long ind = h*width+w;
+			if(maskvec[ind] && edgedata[ind] == 255){
+				float z = idepth*float(depthdata[ind]);
+				if(z > 0.2){depthedge_count++;}
+			}
+		}
+	}
+
+	//printf("depthedge_count: %i\n",depthedge_count);
+	if(depthedge_count > 10){
+		double * depthedge_ap = new double[3*depthedge_count];
+		double * depthedge_ai = new double[3*depthedge_count];
+		//long * dn = new long[depthedge_nr_neighbours*depthedge_count];
+		//depthedge_neighbours[i] = dn;
+		depthedge_nr_arraypoints[i] = depthedge_count;
+		depthedge_arraypoints[i] = depthedge_ap;
+		depthedge_arrayinformations[i] = depthedge_ai;
+
+		c = 0;
+		for(unsigned long w = 0; w < width; w+=1){
+			for(unsigned long h = 0; h < height; h+=1){
+				if(c == depthedge_count){continue;}
+				long ind = h*width+w;
+				if(maskvec[ind] && edgedata[ind] == 255){
+					float z = idepth*float(depthdata[ind]);
+					if(z > 0.2){
+						depthedge_ap[3*c+0] = (w - cx) * z * ifx;
+						depthedge_ap[3*c+1] = (h - cy) * z * ify;;
+						depthedge_ap[3*c+2] = z;
+						depthedge_ai[c] = pow(fabs(z),-2);//1.0/(z*z);
+						c++;
+					}
+				}
+			}
+		}
+
+		ArrayData3D<double> * depthedge_a3d = new ArrayData3D<double>;
+		depthedge_a3d->data					= depthedge_ap;
+		depthedge_a3d->rows					= depthedge_count;
+		depthedge_a3dv[i]					= depthedge_a3d;
+		depthedge_trees3d[i]				= new Tree3d(3, *depthedge_a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+		depthedge_trees3d[i]->buildIndex();
+		Tree3d * t3d                        = depthedge_trees3d[i];
+
+
+		//        const long nrdn = depthedge_nr_neighbours+1;
+		//
+
+		//#if defdomultithread
+		//	#pragma omp parallel for num_threads(8)
+		//#endif
+		//        for(long c = 0; c < depthedge_count; c++){
+		//            size_t ret_index[nrdn];
+		//            double out_dist_sqr[nrdn];
+		//            nanoflann::KNNResultSet<double> resultSet(nrdn);
+		//            resultSet.init(ret_index, out_dist_sqr );
+		//            t3d->findNeighbors(resultSet, depthedge_ap+3*c, nanoflann::SearchParams(10));
+		//            for(long k = 0; k < depthedge_nr_neighbours; k++){
+		//                dn[depthedge_nr_neighbours*c + k] = ret_index[k+1];
+		//            }
+		//        }
+	}
+
+	//printf("total load time:          %5.5f\n",getTime()-total_load_time_start);
+}
+
+void MassRegistrationPPR2::addData(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud){
+	double total_load_time_start = getTime();
+
+	nr_matches.push_back(	0);
+	matchids.push_back(		std::vector< std::vector<long > >() );
+	matchdists.push_back(	std::vector< std::vector<double> >() );
+	nr_datas.push_back(		0);
+
+	points.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	colors.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	normals.push_back(				Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_points.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+	transformed_normals.push_back(	Eigen::Matrix<double, 3, Eigen::Dynamic>());
+
+	informations.push_back(			Eigen::VectorXd());
+
+	nr_arraypoints.push_back(0);
+
+	arraypoints.push_back(0);
+	arraynormals.push_back(0);
+	arraycolors.push_back(0);
+	arrayinformations.push_back(0);
+
+	trees3d.push_back(0);
+	a3dv.push_back(0);
+	is_ok.push_back(false);
+
+	unsigned long i = points.size()-1;
+
+	long count = 0;
+	for(unsigned long j = 0; j < cloud->points.size(); j+=nomaskstep*nomaskstep){
+		if (isValidPoint(cloud->points[j])){count++;}
+	}
+
+	printf("count: %i\n",count);
+
+	if(count < 10){
+		is_ok[i] = false;
+		return;
+	}else{
+		is_ok[i] = true;
+	}
+
+
+
+	double * ap = new double[3*count];
+	double * an = new double[3*count];
+	double * ac = new double[3*count];
+	double * ai = new double[3*count];
+
+	nr_datas[i] = count;
+	points[i].resize(Eigen::NoChange,count);
+	colors[i].resize(Eigen::NoChange,count);
+	normals[i].resize(Eigen::NoChange,count);
+	transformed_points[i].resize(Eigen::NoChange,count);
+	transformed_normals[i].resize(Eigen::NoChange,count);
+
+	nr_arraypoints[i] = count;
+
+	arraypoints[i] = ap;
+	arraynormals[i] = an;
+	arraycolors[i] = ac;
+	arrayinformations[i] = ai;
+
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & C	= colors[i];
+	Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
+
+	Eigen::VectorXd information (count);
+
+	long c = 0;
+	for(unsigned long j = 0; j < cloud->points.size(); j+=nomaskstep*nomaskstep){
+		pcl::PointXYZRGBNormal p = cloud->points[j];
+		if (isValidPoint(p)){
+
+			float xn = p.normal_x;
+			float yn = p.normal_y;
+			float zn = p.normal_z;
+
+			float x = p.x;
+			float y = p.y;
+			float z = p.z;
+
+			//if(j % 1000 == 0){printf("%i -> %f %f %f\n",j,x,y,z);}
+
+			ap[3*c+0] =x;
+			ap[3*c+1] =y;
+			ap[3*c+2] =z;
+
+			an[3*c+0] =xn;
+			an[3*c+1] =yn;
+			an[3*c+2] =zn;
+
+			ac[3*c+0] =p.r;
+			ac[3*c+1] =p.g;
+			ac[3*c+2] =p.b;
+
+			ai[c] = 1/sqrt(x*x+y*y+z*z);
+
+			X(0,c)	= x;
+			X(1,c)	= y;
+			X(2,c)	= z;
+			Xn(0,c)	= xn;
+			Xn(1,c)	= yn;
+			Xn(2,c)	= zn;
+
+			information(c) = ai[c];//1/sqrt(2+x*x+y*y+z*z);//1.0/(z*z);
+			C(0,c) = p.r;
+			C(1,c) = p.g;
+			C(2,c) = p.b;
+			c++;
+
+		}
+	}
+
+	informations[i] = information;
+
+	kp_nr_arraypoints.push_back(0);
+	kp_arraypoints.push_back(0);
+	kp_arraynormals.push_back(0);
+	kp_arrayinformations.push_back(0);
+	kp_arraydescriptors.push_back(0);
+
+	ArrayData3D<double> * a3d = new ArrayData3D<double>;
+	a3d->data	= ap;
+	a3d->rows	= count;
+	a3dv[i]		= a3d;
+	trees3d[i]	= new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+	trees3d[i]->buildIndex();
+	//	if(visualizationLvl > 0){
+	//		printf("total load time:          %5.5f\n",getTime()-total_load_time_start);
+	//	}
+	printf("total load time:          %5.5f\n",getTime()-total_load_time_start);
+}
+
+std::vector<Eigen::Matrix4d> MassRegistrationPPR2::optimize(std::vector<Eigen::Matrix4d> poses){
+	bool onetoone = true;
+	unsigned long nr_frames = poses.size();
+	Eigen::MatrixXd Xo1;
+
+	long optt = 2;
+	for(long outer=0; outer < 60; ++outer) {
+		if(getTime()-total_time_start > timeout){break;}
+
+		//printf("outer: %i\n",outer);
+
+		std::vector<Eigen::Matrix4d> poses2 = poses;
+		if(type == PointToPlane && optt == 2){
+			for(unsigned long i = 0; i < nr_frames; i++){
+				if(getTime()-total_time_start > timeout){break;}
+				if(!is_ok[i]){continue;}
+
+				unsigned long count				= 0;
+				double * api					= arraypoints[i];
+				double * ani					= arraynormals[i];
+				double * aci					= arraycolors[i];
+				double * aii					= arrayinformations[i];
+				const unsigned long nr_api		= nr_arraypoints[i];
+
+				unsigned long kp_count			= 0;
+				//				double * kp_api					= kp_arraypoints[i];
+				//				double * kp_ani					= kp_arraynormals[i];
+				//				double * kp_aii					= kp_arrayinformations[i];
+				//				const unsigned long kp_nr_api	= kp_nr_arraypoints[i];
+
+				unsigned long depthedge_count			= 0;
+				double * depthedge_api					= depthedge_arraypoints[i];
+				double * depthedge_aii					= depthedge_arrayinformations[i];
+				const unsigned long depthedge_nr_api		= depthedge_nr_arraypoints[i];
+
+				Eigen::Affine3d rpi = Eigen::Affine3d(poses[i]);
+				const double & mi00 = rpi(0,0); const double & mi01 = rpi(0,1); const double & mi02 = rpi(0,2); const double & mi03 = rpi(0,3);
+				const double & mi10 = rpi(1,0); const double & mi11 = rpi(1,1); const double & mi12 = rpi(1,2); const double & mi13 = rpi(1,3);
+				const double & mi20 = rpi(2,0); const double & mi21 = rpi(2,1); const double & mi22 = rpi(2,2); const double & mi23 = rpi(2,3);
+
+				for(unsigned long j = 0; j < nr_frames; j++){
+					if(!is_ok[j]){continue;}
+					if(i == j){continue;}
+
+					double * apj					= arraypoints[j];
+					double * anj					= arraynormals[j];
+					double * acj					= arraycolors[j];
+					double * aij					= arrayinformations[j];
+					const unsigned long nr_apj		= nr_arraypoints[j];
+
+					double * depthedge_apj					= depthedge_arraypoints[j];
+					double * depthedge_aij					= depthedge_arrayinformations[j];
+					const unsigned long depthedge_nr_apj		= depthedge_nr_arraypoints[j];
+
+					//					double * kp_apj					= kp_arraypoints[j];
+					//					double * kp_anj					= kp_arraynormals[j];
+					//					double * kp_aij					= kp_arrayinformations[j];
+					//					const unsigned long kp_nr_apj	= kp_nr_arraypoints[j];
+
+					Eigen::Affine3d rpj = Eigen::Affine3d(poses[j]);
+					const double & mj00 = rpj(0,0); const double & mj01 = rpj(0,1); const double & mj02 = rpj(0,2); const double & mj03 = rpj(0,3);
+					const double & mj10 = rpj(1,0); const double & mj11 = rpj(1,1); const double & mj12 = rpj(1,2); const double & mj13 = rpj(1,3);
+					const double & mj20 = rpj(2,0); const double & mj21 = rpj(2,1); const double & mj22 = rpj(2,2); const double & mj23 = rpj(2,3);
+
+					if(use_surface){
+						std::vector<long > & matchidj = matchids[j][i];
+						unsigned long matchesj = matchidj.size();
+						std::vector<long > & matchidi = matchids[i][j];
+						unsigned long matchesi = matchidi.size();
+
+
+						std::vector<double> & matchdistj = matchdists[j][i];
+						std::vector<double> & matchdisti = matchdists[i][j];
+
+						for(unsigned long ki = 0; true && ki < matchesi; ki++){
+							long kj = matchidi[ki];
+							if( kj == -1 ){continue;}
+							if( kj >=  matchesj){continue;}
+							if(!onetoone || matchidj[kj] == ki){
+								Xp_arr[3*count+0] = api[3*ki+0];
+								Xp_arr[3*count+1] = api[3*ki+1];
+								Xp_arr[3*count+2] = api[3*ki+2];
+
+								Xn_arr[3*count+0] = ani[3*ki+0];
+								Xn_arr[3*count+1] = ani[3*ki+1];
+								Xn_arr[3*count+2] = ani[3*ki+2];
+
+								const double & info_i = aii[ki];
+
+								const double & dst_x = apj[3*kj+0];
+								const double & dst_y = apj[3*kj+1];
+								const double & dst_z = apj[3*kj+2];
+
+								const double & dst_nx = anj[3*kj+0];
+								const double & dst_ny = anj[3*kj+1];
+								const double & dst_nz = anj[3*kj+2];
+
+								const double & info_j = aij[kj];
+
+								Qp_arr[3*count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03;
+								Qp_arr[3*count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13;
+								Qp_arr[3*count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23;
+
+								Qn_arr[3*count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz;
+								Qn_arr[3*count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz;
+								Qn_arr[3*count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz;
+
+								rangeW_arr[count] = 1.0/(1.0/info_i+1.0/info_j);
+
+								count++;
+							}
+						}
+					}
+
+
+					if(use_depthedge){
+						std::vector<long > & matchidj = depthedge_matchids[j][i];
+						unsigned long matchesj = matchidj.size();
+						std::vector<long > & matchidi = depthedge_matchids[i][j];
+						unsigned long matchesi = matchidi.size();
+
+
+						std::vector<double> & matchdistj = depthedge_matchdists[j][i];
+						std::vector<double> & matchdisti = depthedge_matchdists[i][j];
+
+						for(unsigned long ki = 0; true && ki < matchesi; ki++){
+							long kj = matchidi[ki];
+							if( kj == -1 ){continue;}
+							if( kj >=  matchesj){continue;}
+							if(!onetoone || matchidj[kj] == ki){
+								depthedge_Xp_arr[3*depthedge_count+0] = depthedge_api[3*ki+0];
+								depthedge_Xp_arr[3*depthedge_count+1] = depthedge_api[3*ki+1];
+								depthedge_Xp_arr[3*depthedge_count+2] = depthedge_api[3*ki+2];
+
+								const double & info_i = depthedge_aii[ki];
+
+								const double & dst_x = depthedge_apj[3*kj+0];
+								const double & dst_y = depthedge_apj[3*kj+1];
+								const double & dst_z = depthedge_apj[3*kj+2];
+
+								const double & info_j = depthedge_aij[kj];
+
+								depthedge_Qp_arr[3*depthedge_count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03;
+								depthedge_Qp_arr[3*depthedge_count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13;
+								depthedge_Qp_arr[3*depthedge_count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23;
+
+								depthedge_rangeW_arr[depthedge_count] = 1.0/(1.0/info_i+1.0/info_j);
+
+								depthedge_count++;
+							}
+						}
+					}
+					/*
+					if(kp_matches.size() > 0 && kp_matches[i].size() > 0 && kp_matches[i][j].size() > 0){
+						std::vector< TestMatch > & matches = kp_matches[i][j];
+						unsigned long nr_matches = matches.size();
+						for(unsigned long k = 0; k < nr_matches; k++){
+							TestMatch & m = matches[k];
+							long ki = m.src;
+							long kj = m.dst;
+
+							kp_Xp_arr[3*kp_count+0] = kp_api[3*ki+0];
+							kp_Xp_arr[3*kp_count+1] = kp_api[3*ki+1];
+							kp_Xp_arr[3*kp_count+2] = kp_api[3*ki+2];
+
+							kp_Xn_arr[3*kp_count+0] = kp_ani[3*ki+0];
+							kp_Xn_arr[3*kp_count+1] = kp_ani[3*ki+1];
+							kp_Xn_arr[3*kp_count+2] = kp_ani[3*ki+2];
+							const double & info_i = kp_aii[ki];
+
+							const double & dst_x = kp_apj[3*kj+0];
+							const double & dst_y = kp_apj[3*kj+1];
+							const double & dst_z = kp_apj[3*kj+2];
+
+							const double & dst_nx = kp_anj[3*kj+0];
+							const double & dst_ny = kp_anj[3*kj+1];
+							const double & dst_nz = kp_anj[3*kj+2];
+
+							const double & info_j = kp_aij[kj];
+
+							kp_Qp_arr[3*kp_count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03;
+							kp_Qp_arr[3*kp_count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13;
+							kp_Qp_arr[3*kp_count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23;
+
+							kp_Qn_arr[3*kp_count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz;
+							kp_Qn_arr[3*kp_count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz;
+							kp_Qn_arr[3*kp_count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz;
+
+							kp_rangeW_arr[kp_count] = 1.0/(1.0/info_i+1.0/info_j);
+							kp_count++;
+						}
+					}
+*/
+				}
+
+				//printf("count %i depthedge_count %i\n",count,depthedge_count);
+				if(count == 0 && kp_count == 0 && depthedge_count == 0){break;}
+
+				typedef Eigen::Matrix<double, 6, 1> Vector6d;
+				typedef Eigen::Matrix<double, 6, 6> Matrix6d;
+
+				Matrix6d ATA;
+				Vector6d ATb;
+
+				//std::vector<Eigen::MatrixXd> Xv;
+				//if(visualizationLvl == 4){for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);}
+
+
+
+				Eigen::Affine3d p = rpi;
+				bool do_inner = true;
+				for(long inner=0; inner < 15 && do_inner; ++inner) {
+
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+					ATA.setZero ();
+					ATb.setZero ();
+					const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3);
+					const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3);
+					const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3);
+					double planeNoise = func->getNoise();
+					double planeInfo = 1.0/(planeNoise*planeNoise);
+
+
+					double depthedge_Noise = depthedge_func->getNoise();
+					double depthedge_kpInfo = 1.0/(depthedge_Noise*depthedge_Noise);
+
+					double kpNoise = kpfunc->getNoise();
+					double kpInfo = 1.0/(kpNoise*kpNoise);
+
+					if(use_surface){
+						for(unsigned long co = 0; co < count; co++){
+							const double & src_x = Xp_arr[3*co+0];
+							const double & src_y = Xp_arr[3*co+1];
+							const double & src_z = Xp_arr[3*co+2];
+							const double & src_nx = Xn_arr[3*co+0];
+							const double & src_ny = Xn_arr[3*co+1];
+							const double & src_nz = Xn_arr[3*co+2];
+
+							const double & dx = Qp_arr[3*co+0];
+							const double & dy = Qp_arr[3*co+1];
+							const double & dz = Qp_arr[3*co+2];
+							const double & dnx = Qn_arr[3*co+0];
+							const double & dny = Qn_arr[3*co+1];
+							const double & dnz = Qn_arr[3*co+2];
+
+							const double & rw = rangeW_arr[co];
+
+							const double & sx = m00*src_x + m01*src_y + m02*src_z + m03;
+							const double & sy = m10*src_x + m11*src_y + m12*src_z + m13;
+							const double & sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+							const double & nx = m00*src_nx + m01*src_ny + m02*src_nz;
+							const double & ny = m10*src_nx + m11*src_ny + m12*src_nz;
+							const double & nz = m20*src_nx + m21*src_ny + m22*src_nz;
+
+							const double & angle = nx*dnx+ny*dny+nz*dnz;
+							//printf("%f\n",angle);
+							//if(angle < 0){exit(0);continue;}
+
+							if(angle < 0){continue;}
+
+							double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz));
+							//double weight = angle*angle*angle*angle*func->getProb(di)*rw*rw;
+							double prob = func->getProb(di);
+							double weight = planeInfo*prob*rw*rw;
+
+							if(false && visualizationLvl == 5){
+								pcl::PointXYZRGBNormal p;
+								p.x = sx;
+								p.y = sy;
+								p.z = sz;
+								p.b = 0;
+								p.g = 255;
+								p.r = 0;
+								scloud->points.push_back(p);
+
+								pcl::PointXYZRGBNormal p1;
+								p1.x = dx;
+								p1.y = dy;
+								p1.z = dz;
+								p1.b = 255.0*prob;
+								p1.g = 255.0*prob;
+								p1.r = 255.0*prob;
+								dcloud->points.push_back(p1);
+							}
+
+							const double & a = nz*sy - ny*sz;
+							const double & b = nx*sz - nz*sx;
+							const double & c = ny*sx - nx*sy;
+
+							ATA.coeffRef (0) += weight * a * a;
+							ATA.coeffRef (1) += weight * a * b;
+							ATA.coeffRef (2) += weight * a * c;
+							ATA.coeffRef (3) += weight * a * nx;
+							ATA.coeffRef (4) += weight * a * ny;
+							ATA.coeffRef (5) += weight * a * nz;
+							ATA.coeffRef (7) += weight * b * b;
+							ATA.coeffRef (8) += weight * b * c;
+							ATA.coeffRef (9) += weight * b * nx;
+							ATA.coeffRef (10) += weight * b * ny;
+							ATA.coeffRef (11) += weight * b * nz;
+							ATA.coeffRef (14) += weight * c * c;
+							ATA.coeffRef (15) += weight * c * nx;
+							ATA.coeffRef (16) += weight * c * ny;
+							ATA.coeffRef (17) += weight * c * nz;
+							ATA.coeffRef (21) += weight * nx * nx;
+							ATA.coeffRef (22) += weight * nx * ny;
+							ATA.coeffRef (23) += weight * nx * nz;
+							ATA.coeffRef (28) += weight * ny * ny;
+							ATA.coeffRef (29) += weight * ny * nz;
+							ATA.coeffRef (35) += weight * nz * nz;
+
+							const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz);
+
+							ATb.coeffRef (0) += a * d;
+							ATb.coeffRef (1) += b * d;
+							ATb.coeffRef (2) += c * d;
+							ATb.coeffRef (3) += nx * d;
+							ATb.coeffRef (4) += ny * d;
+							ATb.coeffRef (5) += nz * d;
+						}
+					}
+
+					double wsum = 0;
+					double wsx = 0;
+					double wsy = 0;
+					double wsz = 0;
+					//printf("%s::%i\n",__FUNCTION__,__LINE__);
+					if(use_depthedge){
+						//printf("%s::%i\n",__FUNCTION__,__LINE__);
+						for(unsigned long co = 0; co < depthedge_count; co++){
+							//							printf("%s::%i\n",__FUNCTION__,__LINE__);
+							const double & src_x = depthedge_Xp_arr[3*co+0];
+							const double & src_y = depthedge_Xp_arr[3*co+1];
+							const double & src_z = depthedge_Xp_arr[3*co+2];
+
+							const double & dx = depthedge_Qp_arr[3*co+0];
+							const double & dy = depthedge_Qp_arr[3*co+1];
+							const double & dz = depthedge_Qp_arr[3*co+2];
+
+							const double & rw = depthedge_rangeW_arr[co];
+
+							const double & sx = m00*src_x + m01*src_y + m02*src_z + m03;
+							const double & sy = m10*src_x + m11*src_y + m12*src_z + m13;
+							const double & sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+
+							const double diffX = dx-sx;
+							const double diffY = dy-sy;
+							const double diffZ = dz-sz;
+
+							double probX = depthedge_func->getProb(rw*diffX);
+							double probY = depthedge_func->getProb(rw*diffY);
+							double probZ = depthedge_func->getProb(rw*diffZ);
+							double prob = probX*probY*probZ/(probX*probY*probZ + (1-probX)*(1-probY)*(1-probZ));
+
+							//printf("%5.5i -> %5.5f %5.5f %5.5f -> %5.5f %5.5f %5.5f , %5.5f %5.5f %5.5f -> prob: %5.5f\n",co,src_x,src_y,src_z,sx,sy,sz,dx,dy,dz,prob);
+
+							if(prob < 0.000001){continue;}
+
+							//							if(visualizationLvl == 5){
+							//								pcl::PointXYZRGBNormal p;
+							//								p.x = sx;
+							//								p.y = sy;
+							//								p.z = sz;
+							//								p.b = 0;
+							//								p.g = 255;
+							//								p.r = 0;
+
+							//								pcl::PointXYZRGBNormal p1;
+							//								p1.x = dx;
+							//								p1.y = dy;
+							//								p1.z = dz;
+							//								p1.b = 255.0;
+							//								p1.g = 0;
+							//								p1.r = 0;
+							//								dcloud->points.push_back(p1);
+
+							////								char buf [1024];
+							////								sprintf(buf,"line%i",scloud->points.size());
+							////								viewer->addLine<pcl::PointXYZRGBNormal>(p,p1,1.0-prob,prob,0,buf);
+							//							}
+
+							if(visualizationLvl == 5){
+
+								//								printf("%5.5i -> %5.5f %5.5f %5.5f , %5.5f %5.5f %5.5f \n",co,sx,sy,sz,dx,dy,dz);
+
+								pcl::PointXYZRGBNormal p;
+								p.x = sx;
+								p.y = sy;
+								p.z = sz;
+								p.b = 0;
+								p.g = 255;
+								p.r = 0;
+								scloud->points.push_back(p);
+
+								pcl::PointXYZRGBNormal p1;
+								p1.x = dx;
+								p1.y = dy;
+								p1.z = dz;
+								p1.b = 255.0*prob;
+								p1.g = 255.0*prob;
+								p1.r = 255.0*prob;
+								dcloud->points.push_back(p1);
+							}
+
+							//double weight = kpInfo*prob*rw*rw;
+
+							double weight = depthedge_kpInfo*prob*rw*rw;
+							wsum += weight;
+
+							wsx += weight * sx;
+							wsy += weight * sy;
+							wsz += weight * sz;
+
+							double wsxsx = weight * sx*sx;
+							double wsysy = weight * sy*sy;
+							double wszsz = weight * sz*sz;
+
+							ATA.coeffRef (0)  += wsysy + wszsz;//a0 * a0;
+							ATA.coeffRef (1)  -= weight * sx*sy;//a0 * a1;
+							ATA.coeffRef (2)  -= weight * sz*sx;//a0 * a2;
+
+
+							ATA.coeffRef (7)  += wsxsx + wszsz;//a1 * a1;
+							ATA.coeffRef (8)  -= weight * sy*sz;//a1 * a2;
+
+							ATA.coeffRef (14) += wsxsx + wsysy;//a2 * a2;
+
+							ATb.coeffRef (0) += weight * (sy*diffZ -sz*diffY);//a0 * b;
+							ATb.coeffRef (1) += weight * (-sx*diffZ + sz*diffX);//a1 * b;
+							ATb.coeffRef (2) += weight * (sx*diffY -sy*diffX);//a2 * b;
+							ATb.coeffRef (3) += weight * diffX;//a3 * b;
+							ATb.coeffRef (4) += weight * diffY;//a4 * b;
+							ATb.coeffRef (5) += weight * diffZ;//a5 * b;
+						}
+					}
+
+					for(unsigned long co = 0; co < kp_count; co++){
+						const double & src_x = kp_Xp_arr[3*co+0];
+						const double & src_y = kp_Xp_arr[3*co+1];
+						const double & src_z = kp_Xp_arr[3*co+2];
+
+						const double & src_nx = kp_Xn_arr[3*co+0];
+						const double & src_ny = kp_Xn_arr[3*co+1];
+						const double & src_nz = kp_Xn_arr[3*co+2];
+
+
+						const double & dx = kp_Qp_arr[3*co+0];
+						const double & dy = kp_Qp_arr[3*co+1];
+						const double & dz = kp_Qp_arr[3*co+2];
+
+						const double & dnx = kp_Qn_arr[3*co+0];
+						const double & dny = kp_Qn_arr[3*co+1];
+						const double & dnz = kp_Qn_arr[3*co+2];
+
+						const double & rw = kp_rangeW_arr[co];
+
+						const double & sx = m00*src_x + m01*src_y + m02*src_z + m03;
+						const double & sy = m10*src_x + m11*src_y + m12*src_z + m13;
+						const double & sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+						const double & nx = m00*src_nx + m01*src_ny + m02*src_nz;
+						const double & ny = m10*src_nx + m11*src_ny + m12*src_nz;
+						const double & nz = m20*src_nx + m21*src_ny + m22*src_nz;
+
+						const double & angle = nx*dnx+ny*dny+nz*dnz;
+
+
+
+						if(angle < 0){continue;}
+
+						const double diffX = dx-sx;
+						const double diffY = dy-sy;
+						const double diffZ = dz-sz;
+
+						double probX = kpfunc->getProb(rw*diffX);
+						double probY = kpfunc->getProb(rw*diffY);
+						double probZ = kpfunc->getProb(rw*diffZ);
+						double prob = probX*probY*probZ/(probX*probY*probZ + (1-probX)*(1-probY)*(1-probZ));
+
+						if(prob < 0.000001){continue;}
+
+						if(visualizationLvl == 5){
+							pcl::PointXYZRGBNormal p;
+							p.x = sx;
+							p.y = sy;
+							p.z = sz;
+							p.normal_x = nx;
+							p.normal_y = ny;
+							p.normal_z = nz;
+							p.b = 0;
+							p.g = 255;
+							p.r = 0;
+							scloud->points.push_back(p);
+
+							pcl::PointXYZRGBNormal pn = p;
+							pn.x += 0.01*nx;
+							pn.y += 0.01*ny;
+							pn.z += 0.01*nz;
+
+							pcl::PointXYZRGBNormal p1;
+							p1.x = dx;
+							p1.y = dy;
+							p1.z = dz;
+							p1.normal_x = dnx;
+							p1.normal_y = dny;
+							p1.normal_z = dnz;
+							p1.b = 255.0;
+							p1.g = 0;
+							p1.r = 0;
+							dcloud->points.push_back(p1);
+
+							pcl::PointXYZRGBNormal p1n = p1;
+							p1n.x += 0.01*dnx;
+							p1n.y += 0.01*dny;
+							p1n.z += 0.01*dnz;
+
+							char buf [1024];
+							sprintf(buf,"line%i",scloud->points.size());
+							viewer->addLine<pcl::PointXYZRGBNormal>(p,p1,1.0-prob,prob,0,buf);
+							//viewer->addLine<pcl::PointXYZRGBNormal>(p,p1, angle <= 0 , angle > 0 ,0,buf);
+
+							//							sprintf(buf,"slineN%i",scloud->points.size());
+							//							viewer->addLine<pcl::PointXYZRGBNormal>(p1,p1n,1,0,1,buf);
+
+							//							sprintf(buf,"dlineN%i",scloud->points.size());
+							//							viewer->addLine<pcl::PointXYZRGBNormal>(p,pn,1,1,0,buf);
+						}
+
+						//double weight = kpInfo*prob*rw*rw;
+						double weight = prob*rw*rw;
+						wsum += weight;
+
+						wsx += weight * sx;
+						wsy += weight * sy;
+						wsz += weight * sz;
+
+						double wsxsx = weight * sx*sx;
+						double wsysy = weight * sy*sy;
+						double wszsz = weight * sz*sz;
+
+						ATA.coeffRef (0)  += wsysy + wszsz;//a0 * a0;
+						ATA.coeffRef (1)  -= weight * sx*sy;//a0 * a1;
+						ATA.coeffRef (2)  -= weight * sz*sx;//a0 * a2;
+
+
+						ATA.coeffRef (7)  += wsxsx + wszsz;//a1 * a1;
+						ATA.coeffRef (8)  -= weight * sy*sz;//a1 * a2;
+
+						ATA.coeffRef (14) += wsxsx + wsysy;//a2 * a2;
+
+						ATb.coeffRef (0) += weight * (sy*diffZ -sz*diffY);//a0 * b;
+						ATb.coeffRef (1) += weight * (-sx*diffZ + sz*diffX);//a1 * b;
+						ATb.coeffRef (2) += weight * (sx*diffY -sy*diffX);//a2 * b;
+						ATb.coeffRef (3) += weight * diffX;//a3 * b;
+						ATb.coeffRef (4) += weight * diffY;//a4 * b;
+						ATb.coeffRef (5) += weight * diffZ;//a5 * b;
+					}
+
+
+					ATA.coeffRef (4)  -= wsz;//a0 * a4;
+					ATA.coeffRef (9)  += wsz;//a1 * a3;
+					ATA.coeffRef (5)  += wsy;//a0 * a5;
+					ATA.coeffRef (15) -= wsy;//a2 * a3;
+					ATA.coeffRef (11) -= wsx;//a1 * a5;
+					ATA.coeffRef (16) += wsx;//a2 * a4;
+					ATA.coeffRef (21) += wsum;//a3 * a3;
+					ATA.coeffRef (28) += wsum;//a4 * a4;
+					ATA.coeffRef (35) += wsum;//a5 * a5;
+
+					ATA.coeffRef (6) = ATA.coeff (1);
+					ATA.coeffRef (12) = ATA.coeff (2);
+					ATA.coeffRef (13) = ATA.coeff (8);
+					ATA.coeffRef (18) = ATA.coeff (3);
+					ATA.coeffRef (19) = ATA.coeff (9);
+					ATA.coeffRef (20) = ATA.coeff (15);
+					ATA.coeffRef (24) = ATA.coeff (4);
+					ATA.coeffRef (25) = ATA.coeff (10);
+					ATA.coeffRef (26) = ATA.coeff (16);
+					ATA.coeffRef (27) = ATA.coeff (22);
+					ATA.coeffRef (30) = ATA.coeff (5);
+					ATA.coeffRef (31) = ATA.coeff (11);
+					ATA.coeffRef (32) = ATA.coeff (17);
+					ATA.coeffRef (33) = ATA.coeff (23);
+					ATA.coeffRef (34) = ATA.coeff (29);
+
+					for(long d = 0; d < 6; d++){
+						ATA(d,d) += 1;
+					}
+
+					// Solve A*x = b
+					Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
+					Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)));
+					p = transformation*p;
+
+					double change_t = 0;
+					double change_r = 0;
+					for(unsigned long k = 0; k < 3; k++){
+						change_t += transformation(k,3)*transformation(k,3);
+						for(unsigned long l = 0; l < 3; l++){
+							if(k == l){ change_r += fabs(1-transformation(k,l));}
+							else{		change_r += fabs(transformation(k,l));}
+						}
+					}
+					change_t = sqrt(change_t);
+
+					if(visualizationLvl == 5){
+						printf("change_t: %10.10f change_r: %10.10f stopval: %10.10f\n",change_t,change_r,stopval);
+						viewer->removeAllPointClouds();
+						viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+						//viewer->addPointCloudNormals<pcl::PointXYZRGBNormal>(scloud,1,0.2,"scloudn");
+
+						viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+						//viewer->addPointCloudNormals<pcl::PointXYZRGBNormal>(scloud,1,0.2,"dcloudn");
+						viewer->spin();
+						viewer->removeAllPointClouds();
+						viewer->removeAllShapes();
+					}
+
+					if(change_t < stopval && change_r < stopval){do_inner = false;}
+				}
+
+
+				//new_opt_time += getTime()-new_opt_start;
+				//std::cout << p.matrix() << std::endl;
+				//Eigen::Matrix4d newpose = p.matrix()*poses[i];
+				poses[i] = p.matrix();//newpose;
+			}
+		}else if(type == PointToPlane && optt == 1){
+			for(unsigned long i = 0; i < nr_frames; i++){
+				if(getTime()-total_time_start > timeout){break;}
+				if(!is_ok[i]){continue;}
+				unsigned long count = 0;
+
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi    = transformed_points[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi     = points[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni   = transformed_normals[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni    = normals[i];
+				Eigen::VectorXd & informationi                    = informations[i];
+
+
+				for(unsigned long j = 0; j < nr_frames; j++){
+					if(!is_ok[j]){continue;}
+					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj    = transformed_points[j];
+					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj   = transformed_normals[j];
+					Eigen::VectorXd & informationj                    = informations[j];
+
+					std::vector<long > & matchidj = matchids[j][i];
+					unsigned long matchesj = matchidj.size();
+					std::vector<long > & matchidi = matchids[i][j];
+					unsigned long matchesi = matchidi.size();
+
+					std::vector<double> & matchdistj = matchdists[j][i];
+					std::vector<double> & matchdisti = matchdists[i][j];
+
+
+					for(unsigned long ki = 0; ki < matchesi; ki++){
+						long kj = matchidi[ki];
+						if( kj == -1 ){continue;}
+						if( kj >=  matchesj){continue;}
+						if(!onetoone || matchidj[kj] == ki){
+							Qp_arr[3*count+0] = tXj(0,kj);
+							Qp_arr[3*count+1] = tXj(1,kj);
+							Qp_arr[3*count+2] = tXj(2,kj);
+							Qn_arr[3*count+0] = tXnj(0,kj);
+							Qn_arr[3*count+1] = tXnj(1,kj);
+							Qn_arr[3*count+2] = tXnj(2,kj);
+
+							Xp_arr[3*count+0] = tXi(0,ki);
+							Xp_arr[3*count+1] = tXi(1,ki);
+							Xp_arr[3*count+2] = tXi(2,ki);
+							Xn_arr[3*count+0] = tXni(0,ki);
+							Xn_arr[3*count+1] = tXni(1,ki);
+							Xn_arr[3*count+2] = tXni(2,ki);
+
+							rangeW_arr[count] = 1.0/(1.0/informationi(ki)+1.0/informationj(kj));
+							count++;
+						}
+					}
+				}
+
+				if(count == 0){break;}
+
+				typedef Eigen::Matrix<double, 6, 1> Vector6d;
+				typedef Eigen::Matrix<double, 6, 6> Matrix6d;
+
+				Matrix6d ATA;
+				Vector6d ATb;
+
+
+				Eigen::Affine3d p = Eigen::Affine3d::Identity();
+				bool do_inner = true;
+				for(long inner=0; inner < 5 && do_inner; ++inner) {
+					ATA.setZero ();
+					ATb.setZero ();
+					const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3);
+					const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3);
+					const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3);
+					for(unsigned long co = 0; co < count; co++){
+						const double & src_x = Xp_arr[3*co+0];
+						const double & src_y = Xp_arr[3*co+1];
+						const double & src_z = Xp_arr[3*co+2];
+						const double & src_nx = Xn_arr[3*co+0];
+						const double & src_ny = Xn_arr[3*co+1];
+						const double & src_nz = Xn_arr[3*co+2];
+
+						const double & dx = Qp_arr[3*co+0];
+						const double & dy = Qp_arr[3*co+1];
+						const double & dz = Qp_arr[3*co+2];
+						const double & dnx = Qn_arr[3*co+0];
+						const double & dny = Qn_arr[3*co+1];
+						const double & dnz = Qn_arr[3*co+2];
+
+						const double & rw = rangeW_arr[co];
+
+						const double & sx = m00*src_x + m01*src_y + m02*src_z + m03;
+						const double & sy = m10*src_x + m11*src_y + m12*src_z + m13;
+						const double & sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+						const double & nx = m00*src_nx + m01*src_ny + m02*src_nz;
+						const double & ny = m10*src_nx + m11*src_ny + m12*src_nz;
+						const double & nz = m20*src_nx + m21*src_ny + m22*src_nz;
+
+						if(nx*dnx+ny*dny+nz*dnz < 0){continue;}
+
+						double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz));
+						double weight = func->getProb(di)*rw*rw;
+
+						const double & a = nz*sy - ny*sz;
+						const double & b = nx*sz - nz*sx;
+						const double & c = ny*sx - nx*sy;
+
+						ATA.coeffRef (0) += weight * a * a;
+						ATA.coeffRef (1) += weight * a * b;
+						ATA.coeffRef (2) += weight * a * c;
+						ATA.coeffRef (3) += weight * a * nx;
+						ATA.coeffRef (4) += weight * a * ny;
+						ATA.coeffRef (5) += weight * a * nz;
+						ATA.coeffRef (7) += weight * b * b;
+						ATA.coeffRef (8) += weight * b * c;
+						ATA.coeffRef (9) += weight * b * nx;
+						ATA.coeffRef (10) += weight * b * ny;
+						ATA.coeffRef (11) += weight * b * nz;
+						ATA.coeffRef (14) += weight * c * c;
+						ATA.coeffRef (15) += weight * c * nx;
+						ATA.coeffRef (16) += weight * c * ny;
+						ATA.coeffRef (17) += weight * c * nz;
+						ATA.coeffRef (21) += weight * nx * nx;
+						ATA.coeffRef (22) += weight * nx * ny;
+						ATA.coeffRef (23) += weight * nx * nz;
+						ATA.coeffRef (28) += weight * ny * ny;
+						ATA.coeffRef (29) += weight * ny * nz;
+						ATA.coeffRef (35) += weight * nz * nz;
+
+						const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz);
+
+						ATb.coeffRef (0) += a * d;
+						ATb.coeffRef (1) += b * d;
+						ATb.coeffRef (2) += c * d;
+						ATb.coeffRef (3) += nx * d;
+						ATb.coeffRef (4) += ny * d;
+						ATb.coeffRef (5) += nz * d;
+					}
+
+					ATA.coeffRef (6) = ATA.coeff (1);
+					ATA.coeffRef (12) = ATA.coeff (2);
+					ATA.coeffRef (13) = ATA.coeff (8);
+					ATA.coeffRef (18) = ATA.coeff (3);
+					ATA.coeffRef (19) = ATA.coeff (9);
+					ATA.coeffRef (20) = ATA.coeff (15);
+					ATA.coeffRef (24) = ATA.coeff (4);
+					ATA.coeffRef (25) = ATA.coeff (10);
+					ATA.coeffRef (26) = ATA.coeff (16);
+					ATA.coeffRef (27) = ATA.coeff (22);
+					ATA.coeffRef (30) = ATA.coeff (5);
+					ATA.coeffRef (31) = ATA.coeff (11);
+					ATA.coeffRef (32) = ATA.coeff (17);
+					ATA.coeffRef (33) = ATA.coeff (23);
+					ATA.coeffRef (34) = ATA.coeff (29);
+
+					// Solve A*x = b
+					Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
+					Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)));
+					p = transformation*p;
+
+					double change_t = 0;
+					double change_r = 0;
+					for(unsigned long k = 0; k < 3; k++){
+						change_t += transformation(k,3)*transformation(k,3);
+						for(unsigned long l = 0; l < 3; l++){
+							if(k == l){ change_r += fabs(1-transformation(k,l));}
+							else{		change_r += fabs(transformation(k,l));}
+						}
+					}
+					change_t = sqrt(change_t);
+					if(change_t < stopval && change_r < stopval){do_inner = false;}
+				}
+
+				//new_opt_time += getTime()-new_opt_start;
+				//std::cout << p.matrix() << std::endl;
+				Eigen::Matrix4d newpose = p.matrix()*poses[i];
+				poses[i] = newpose;
+			}
+		}else{
+
+			for(unsigned long i = 0; i < nr_frames; i++){
+				if(getTime()-total_time_start > timeout){break;}
+				if(!is_ok[i]){continue;}
+				unsigned long nr_match = 0;
+				{
+					for(unsigned long j = 0; j < nr_frames; j++){
+						if(!is_ok[j]){continue;}
+						std::vector<long > & matchidj = matchids[j][i];
+						unsigned long matchesj = matchidj.size();
+						std::vector<long > & matchidi = matchids[i][j];
+						unsigned long matchesi = matchidi.size();
+
+
+						std::vector<double> & matchdistj = matchdists[j][i];
+						std::vector<double> & matchdisti = matchdists[i][j];
+
+
+						for(unsigned long ki = 0; ki < matchesi; ki++){
+							long kj = matchidi[ki];
+							if( kj == -1 ){continue;}
+							if( kj >=  matchesj){continue;}
+							if(!onetoone || matchidj[kj] == ki){	nr_match++;}
+						}
+					}
+				}
+
+				Eigen::Matrix3Xd Xp		= Eigen::Matrix3Xd::Zero(3,	nr_match);
+				Eigen::Matrix3Xd Xp_ori	= Eigen::Matrix3Xd::Zero(3,	nr_match);
+				Eigen::Matrix3Xd Xn		= Eigen::Matrix3Xd::Zero(3,	nr_match);
+
+				Eigen::Matrix3Xd Qp		= Eigen::Matrix3Xd::Zero(3,	nr_match);
+				Eigen::Matrix3Xd Qn		= Eigen::Matrix3Xd::Zero(3,	nr_match);
+				Eigen::VectorXd  rangeW	= Eigen::VectorXd::Zero(	nr_match);
+
+				long count = 0;
+
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi	= points[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[i];
+				Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni	= normals[i];
+				Eigen::VectorXd & informationi					= informations[i];
+
+				for(unsigned long j = 0; j < nr_frames; j++){
+					if(!is_ok[j]){continue;}
+					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXj	= transformed_points[j];
+					Eigen::Matrix<double, 3, Eigen::Dynamic> & tXnj	= transformed_normals[j];
+					Eigen::VectorXd & informationj					= informations[j];
+
+					std::vector<long > & matchidj = matchids[j][i];
+					unsigned long matchesj = matchidj.size();
+					std::vector<long > & matchidi = matchids[i][j];
+					unsigned long matchesi = matchidi.size();
+
+
+					std::vector<double> & matchdistj = matchdists[j][i];
+					std::vector<double> & matchdisti = matchdists[i][j];
+
+					for(unsigned long ki = 0; ki < matchesi; ki++){
+						long kj = matchidi[ki];
+						if( kj == -1 ){continue;}
+						if( kj >=  matchesj){continue;}
+						if(!onetoone || matchidj[kj] == ki){
+							Qp.col(count) = tXj.col(kj);
+							Qn.col(count) = tXnj.col(kj);
+
+							Xp_ori.col(count) = Xi.col(ki);
+							Xp.col(count) = tXi.col(ki);
+
+							Xn.col(count) = tXni.col(ki);
+							rangeW(count) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj));
+							count++;
+						}
+					}
+				}
+
+				if(count == 0){break;}
+
+				//showMatches(Xp,Qp);
+				for(long inner=0; inner < 5; ++inner) {
+					Eigen::MatrixXd residuals;
+					switch(type) {
+					case PointToPoint:	{residuals = Xp-Qp;} 						break;
+					case PointToPlane:	{
+						residuals		= Eigen::MatrixXd::Zero(1,	Xp.cols());
+						for(long i=0; i<Xp.cols(); ++i) {
+							float dx = Xp(0,i)-Qp(0,i);
+							float dy = Xp(1,i)-Qp(1,i);
+							float dz = Xp(2,i)-Qp(2,i);
+							float qx = Qn(0,i);
+							float qy = Qn(1,i);
+							float qz = Qn(2,i);
+							float di = qx*dx + qy*dy + qz*dz;
+							residuals(0,i) = di;
+						}
+					}break;
+					default:			{printf("type not set\n");}					break;
+					}
+					for(unsigned long k=0; k < nr_match; ++k) {residuals.col(k) *= rangeW(k);}
+
+					Eigen::VectorXd  W;
+					switch(type) {
+					case PointToPoint:	{W = func->getProbs(residuals); } 					break;
+					case PointToPlane:	{
+						W = func->getProbs(residuals);
+						for(long k=0; k<nr_match; ++k) {W(k) = W(k)*float((Xn(0,k)*Qn(0,k) + Xn(1,k)*Qn(1,k) + Xn(2,k)*Qn(2,k)) > 0.0);}
+					}	break;
+					default:			{printf("type not set\n");} break;
+					}
+
+					W = W.array()*rangeW.array()*rangeW.array();
+					Xo1 = Xp;
+					switch(type) {
+					case PointToPoint:	{
+						//RigidMotionEstimator::point_to_point(Xp, Qp, W);
+						pcl::TransformationFromCorrespondences tfc1;
+						for(unsigned long c = 0; c < nr_match; c++){tfc1.add(Eigen::Vector3f(Xp(0,c), Xp(1,c),Xp(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));}
+						Eigen::Affine3d rot = tfc1.getTransformation().cast<double>();
+						Xp = rot*Xp;
+						Xn = rot.rotation()*Xn;
+					}		break;
+					case PointToPlane:	{
+						point_to_plane2(Xp, Xn, Qp, Qn, W);
+					}	break;
+					default:  			{printf("type not set\n"); } break;
+					}
+
+					double stop1 = (Xp-Xo1).colwise().norm().maxCoeff();
+					Xo1 = Xp;
+					if(stop1 < 0.001){break; }
+				}
+				//exit(0);
+				pcl::TransformationFromCorrespondences tfc;
+				for(unsigned long c = 0; c < nr_match; c++){tfc.add(Eigen::Vector3f(Xp_ori(0,c),Xp_ori(1,c),Xp_ori(2,c)),Eigen::Vector3f(Xp(0,c),Xp(1,c),Xp(2,c)));}
+				poses[i] = tfc.getTransformation().cast<double>().matrix();
+			}
+		}
+
+		Eigen::Matrix4d p0inv = poses[0].inverse();
+		for(unsigned long j = 0; j < nr_frames; j++){
+			if(!is_ok[j]){continue;}
+			poses[j] = p0inv*poses[j];
+
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & tXi	= transformed_points[j];
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & Xi	= points[j];
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & tXni	= transformed_normals[j];
+			Eigen::Matrix<double, 3, Eigen::Dynamic> & Xni	= normals[j];
+
+			Eigen::Matrix4d p = poses[j];
+			float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+			float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+			float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+			for(long c = 0; c < Xi.cols(); c++){
+				float x = Xi(0,c);
+				float y = Xi(1,c);
+				float z = Xi(2,c);
+
+				float nx = Xni(0,c);
+				float ny = Xni(1,c);
+				float nz = Xni(2,c);
+
+				tXi(0,c)		= m00*x + m01*y + m02*z + m03;
+				tXi(1,c)		= m10*x + m11*y + m12*z + m13;
+				tXi(2,c)		= m20*x + m21*y + m22*z + m23;
+
+				tXni(0,c)		= m00*nx + m01*ny + m02*nz;
+				tXni(1,c)		= m10*nx + m11*ny + m12*nz;
+				tXni(2,c)		= m20*nx + m21*ny + m22*nz;
+			}
+		}
+		if(isconverged(poses, poses2, stopval, stopval)){break;}
+	}
+
+	//	exit(0);
+	return poses;
+}
+
+long popcount_lauradoux(uint64_t *buf, uint32_t size) {
+	const uint64_t* data = (uint64_t*) buf;
+
+	const uint64_t m1	= UINT64_C(0x5555555555555555);
+	const uint64_t m2	= UINT64_C(0x3333333333333333);
+	const uint64_t m4	= UINT64_C(0x0F0F0F0F0F0F0F0F);
+	const uint64_t m8	= UINT64_C(0x00FF00FF00FF00FF);
+	const uint64_t m16 = UINT64_C(0x0000FFFF0000FFFF);
+	const uint64_t h01 = UINT64_C(0x0101010101010101);
+
+	uint32_t bitCount = 0;
+	uint32_t i, j;
+	uint64_t count1, count2, half1, half2, acc;
+	uint64_t x;
+	uint32_t limit30 = size - size % 30;
+
+	// 64-bit tree merging (merging3)
+	for (i = 0; i < limit30; i += 30, data += 30) {
+		acc = 0;
+		for (j = 0; j < 30; j += 3) {
+			count1	=	data[j];
+			count2	=	data[j+1];
+			half1	=	data[j+2];
+			half2	=	data[j+2];
+			half1	&=	m1;
+			half2	= (half2	>> 1) & m1;
+			count1 -= (count1 >> 1) & m1;
+			count2 -= (count2 >> 1) & m1;
+			count1 +=	half1;
+			count2 +=	half2;
+			count1	= (count1 & m2) + ((count1 >> 2) & m2);
+			count1 += (count2 & m2) + ((count2 >> 2) & m2);
+			acc		+= (count1 & m4) + ((count1 >> 4) & m4);
+		}
+		acc = (acc & m8) + ((acc >>	8)	& m8);
+		acc = (acc			 +	(acc >> 16)) & m16;
+		acc =	acc			 +	(acc >> 32);
+		bitCount += (uint32_t)acc;
+	}
+
+	for (i = 0; i < size - limit30; i++) {
+		x = data[i];
+		x =	x			 - ((x >> 1)	& m1);
+		x = (x & m2) + ((x >> 2)	& m2);
+		x = (x			 +	(x >> 4)) & m4;
+		bitCount += (uint32_t)((x * h01) >> 56);
+	}
+	return bitCount;
+}
+
+void MassRegistrationPPR2::rematchKeyPoints(std::vector<Eigen::Matrix4d> poses, std::vector<Eigen::Matrix4d> prev_poses, bool first){
+	printf("rematchKeyPoints\n");
+	double total_rematchKeyPoints_time_start = getTime();
+	unsigned long nr_frames = poses.size();
+
+	long max_kps = 0;
+	for(unsigned long i = 0; i < nr_frames; i++){
+		printf("max_kps: %i kp_nr_arraypoints: %i\n",max_kps, kp_nr_arraypoints[i]);
+		max_kps = std::max(max_kps,kp_nr_arraypoints[i]);
+	}
+	printf("max_kps: %i\n",max_kps);
+
+	if(max_kps == 0){return;}
+
+	double *	best_src	= new double[max_kps];
+	double *	best_src2	= new double[max_kps];
+	double *	best_src_e	= new double[max_kps];
+	double *	best_src_f	= new double[max_kps];
+	long *		best_src_id	= new long[max_kps];
+
+	double *	best_dst	= new double[max_kps];
+	double *	best_dst_e	= new double[max_kps];
+	double *	best_dst_f	= new double[max_kps];
+	long *		best_dst_id	= new long[max_kps];
+
+	for(unsigned long i = 0; i < nr_frames; i++){
+		kp_matches.resize(nr_frames);
+		for(unsigned long j = 0; j < nr_frames; j++){
+			kp_matches[i].resize(nr_frames);
+		}
+	}
+
+
+	long ignores = 0;
+	bool useOrb = true;
+	if(useOrb){
+		for(unsigned long i = 0; i < nr_frames; i++){
+			const unsigned long src_nr_ap = kp_nr_arraypoints[i];
+			if(src_nr_ap == 0){continue;}
+			double * src_ap = kp_arraypoints[i];
+			double * src_an = kp_arraynormals[i];
+			double * src_ai = kp_arrayinformations[i];
+			uint64_t * src_data = kp_arraydescriptors[i];
+
+			for(unsigned long j = 0; j < nr_frames; j++){
+				if(i == j){continue;}
+
+				const unsigned long dst_nr_ap = kp_nr_arraypoints[j];
+				if(dst_nr_ap == 0){continue;}
+
+				for(long k = 0; k < src_nr_ap; k++){
+					best_src[k]		= 999999999999;
+					best_src2[k]		= 999999999999;
+					best_src_id[k]	= -1;
+				}
+				for(long k = 0; k < dst_nr_ap; k++){
+					best_dst[k]		= 999999999999;
+					best_dst_id[k]	= -1;
+				}
+
+
+				double * dst_ap = kp_arraypoints[j];
+				double * dst_an = kp_arraynormals[j];
+				double * dst_ai = kp_arrayinformations[j];
+				uint64_t * dst_data = kp_arraydescriptors[j];
+
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+
+				if(!first){
+					Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]);
+
+					Eigen::Affine3d diff = prev_rp.inverse()*rp;
+
+					double change_trans = 0;
+					double change_rot = 0;
+					double dt = 0;
+					for(unsigned long k = 0; k < 3; k++){
+						dt += diff(k,3)*diff(k,3);
+						for(unsigned long l = 0; l < 3; l++){
+							if(k == l){ change_rot += fabs(1-diff(k,l));}
+							else{		change_rot += fabs(diff(k,l));}
+						}
+					}
+					change_trans += sqrt(dt);
+
+					if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;}
+				}
+
+				const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+				const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+				const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+				float di = 10.0;
+				float pi = 10.0;
+
+
+
+#if defdomultithread
+#pragma omp parallel for num_threads(8)
+#endif
+				for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) {
+					const double & src_x = src_ap[3*src_k+0];
+					const double & src_y = src_ap[3*src_k+1];
+					const double & src_z = src_ap[3*src_k+2];
+
+					double sx = m00*src_x + m01*src_y + m02*src_z + m03;
+					double sy = m10*src_x + m11*src_y + m12*src_z + m13;
+					double sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+					unsigned long src_k4 = src_k*4;
+					const uint64_t s1 = src_data[src_k4+0];
+					const uint64_t s2 = src_data[src_k4+1];
+					const uint64_t s3 = src_data[src_k4+2];
+					const uint64_t s4 = src_data[src_k4+3];
+
+					uint64_t xordata [4];
+					for(unsigned long dst_k = 0; dst_k < dst_nr_ap; ++dst_k) {
+						double dx = sx-dst_ap[3*dst_k+0];
+						double dy = sy-dst_ap[3*dst_k+1];
+						double dz = sz-dst_ap[3*dst_k+2];
+
+						double p_dist = sqrt(dx*dx+dy*dy+dz*dz);
+
+						unsigned long dst_k4 = dst_k*4;
+						const uint64_t d1 = dst_data[dst_k4+0];
+						const uint64_t d2 = dst_data[dst_k4+1];
+						const uint64_t d3 = dst_data[dst_k4+2];
+						const uint64_t d4 = dst_data[dst_k4+3];
+
+						xordata[0] = s1 ^ d1;
+						xordata[1] = s2 ^ d2;
+						xordata[2] = s3 ^ d3;
+						xordata[3] = s4 ^ d4;
+
+						long cnt = popcount_lauradoux(xordata, 4);
+						double f_dist = float(cnt)/256.0f;
+
+						float d = f_dist*di + p_dist*pi;
+
+						if(d < best_src[src_k]){
+
+							best_src2[src_k] = best_src[src_k];
+							best_src_id[src_k] = dst_k;
+							best_src[src_k] = d;
+						}else if(d < best_src2[src_k]){
+							best_src2[src_k] = d;
+						}
+
+
+
+						if(d < best_dst[dst_k]){
+							best_dst_id[dst_k] = src_k;
+							best_dst[dst_k] = d;
+						}
+
+					}
+				}
+
+				std::vector< TestMatch > matches;
+				double threshold = 2.0;
+				double ratiothreshold = 0.8;
+
+				long nr_matches = 0;
+				for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) {
+					long dst_k = best_src_id[src_k];
+
+					if(best_dst_id[dst_k] != src_k){continue;}//One to one
+					if(best_src[src_k] > threshold){continue;}//threshold
+					if(best_src[src_k]/best_src2[src_k] < ratiothreshold){continue;}//ratiothreshold
+
+					if(dst_an[3*dst_k+0] > 1 || src_an[3*src_k+0] > 1){continue;}
+
+					nr_matches++;
+
+					matches.push_back(TestMatch(src_k, dst_k,best_src[src_k]));
+				}
+				kp_matches[i][j] = matches;
+			}
+		}
+	}
+
+
+	bool useSurf = false;
+	if(useSurf){
+		for(unsigned long i = 0; i < nr_frames; i++){
+			const unsigned long src_nr_ap = kp_nr_arraypoints[i];
+			if(src_nr_ap == 0){continue;}
+			double * src_ap = kp_arraypoints[i];
+			double * src_an = kp_arraynormals[i];
+			double * src_ai = kp_arrayinformations[i];
+			float* src_data = (float*)(kp_arraydescriptors[i]);
+
+			for(unsigned long j = 0; j < nr_frames; j++){
+				if(i == j){continue;}
+
+				const unsigned long dst_nr_ap = kp_nr_arraypoints[j];
+				if(dst_nr_ap == 0){continue;}
+
+				for(long k = 0; k < src_nr_ap; k++){
+					best_src[k]		= 999999999999;
+					best_src_id[k]	= -1;
+				}
+				for(long k = 0; k < dst_nr_ap; k++){
+					best_dst[k]		= 999999999999;
+					best_dst_id[k]	= -1;
+				}
+
+
+				double * dst_ap = kp_arraypoints[j];
+				double * dst_an = kp_arraynormals[j];
+				double * dst_ai = kp_arrayinformations[j];
+				float* dst_data = (float*)(kp_arraydescriptors[j]);
+
+				Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]);
+
+				if(!first){
+					Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]);
+
+					Eigen::Affine3d diff = prev_rp.inverse()*rp;
+
+					double change_trans = 0;
+					double change_rot = 0;
+					double dt = 0;
+					for(unsigned long k = 0; k < 3; k++){
+						dt += diff(k,3)*diff(k,3);
+						for(unsigned long l = 0; l < 3; l++){
+							if(k == l){ change_rot += fabs(1-diff(k,l));}
+							else{		change_rot += fabs(diff(k,l));}
+						}
+					}
+					change_trans += sqrt(dt);
+
+					if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;}
+				}
+
+				const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3);
+				const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3);
+				const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3);
+
+
+
+#if defdomultithread
+#pragma omp parallel for num_threads(8)
+#endif
+				for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) {
+					const double & src_x = src_ap[3*src_k+0];
+					const double & src_y = src_ap[3*src_k+1];
+					const double & src_z = src_ap[3*src_k+2];
+
+					double sx = m00*src_x + m01*src_y + m02*src_z + m03;
+					double sy = m10*src_x + m11*src_y + m12*src_z + m13;
+					double sz = m20*src_x + m21*src_y + m22*src_z + m23;
+
+
+
+					for(unsigned long dst_k = 0; dst_k < dst_nr_ap; ++dst_k) {
+						double dx = sx-dst_ap[3*dst_k+0];
+						double dy = sy-dst_ap[3*dst_k+1];
+						double dz = sz-dst_ap[3*dst_k+2];
+
+
+						double f_dist = 0;
+
+						for(unsigned long f_k = 0; f_k < 128; ++f_k) {
+							double diff = src_data[src_k*128 + f_k] - dst_data[dst_k*128 + f_k];
+							f_dist += diff*diff;
+						}
+
+						float d = f_dist;//*di + p_dist*pi;
+
+						if(d < best_src[src_k]){
+							best_src_id[src_k] = dst_k;
+							best_src[src_k] = d;
+						}
+
+						if(d < best_dst[dst_k]){
+							best_dst_id[dst_k] = src_k;
+							best_dst[dst_k] = d;
+						}
+
+					}
+				}
+
+				std::vector< TestMatch > matches;
+				double threshold = 0.25;
+				long nr_matches = 0;
+				for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) {
+					long dst_k = best_src_id[src_k];
+
+					if(best_dst_id[dst_k] != src_k || best_src[src_k] > threshold){continue;}//One to one
+
+					if(dst_an[3*dst_k+0] > 1 || src_an[3*src_k+0] > 1){continue;}
+
+					nr_matches++;
+
+					matches.push_back(TestMatch(src_k, dst_k,best_src[src_k]));
+				}
+				kp_matches[i][j] = matches;
+			}
+		}
+	}
+	if(false){
+		for(unsigned long i = 0; i < nr_frames; i++){
+			if(frameid[i] == -1){continue;}
+			for(unsigned long j = 0; j < nr_frames; j++){
+				if(frameid[j] == -1){continue;}
+				if(kp_matches[i][j].size() == 0){continue;}
+				unsigned char * src_data = (unsigned char *)(model->frames[frameid[i]]->rgb.data);
+				unsigned char * dst_data = (unsigned char *)(model->frames[frameid[j]]->rgb.data);
+
+				cv::Mat img;
+				img.create(480,2*640,CV_8UC3);
+				unsigned char * imgdata = (unsigned char *)img.data;
+				for(long w = 0; w < 640; w++){
+					for(long h = 0; h < 480; h++){
+						long ind = h*640+w;
+
+						long ind2 = h*2*640+w;
+						long ind3 = h*2*640+w+640;
+
+						imgdata[3*ind2+0] = src_data[3*ind+0];
+						imgdata[3*ind2+1] = src_data[3*ind+1];
+						imgdata[3*ind2+2] = src_data[3*ind+2];
+
+						imgdata[3*ind3+0] = dst_data[3*ind+0];
+						imgdata[3*ind3+1] = dst_data[3*ind+1];
+						imgdata[3*ind3+2] = dst_data[3*ind+2];
+					}
+				}
+
+				std::vector<cv::KeyPoint> & src_keypoints = model->all_keypoints[i];
+				std::vector<cv::KeyPoint> & dst_keypoints = model->all_keypoints[j];
+
+				std::vector< TestMatch > matches = kp_matches[i][j];
+				for(unsigned long k = 0; k < matches.size(); ++k) {
+					cv::KeyPoint & spt = src_keypoints[matches[k].src];
+					cv::KeyPoint & dpt = dst_keypoints[matches[k].dst];
+					cv::line(img, spt.pt, cv::Point(dpt.pt.x+640,dpt.pt.y), cv::Scalar(255,0,255));
+				}
+
+				cv::imshow( "matches", img );
+				cv::waitKey(0);
+
+				//for(long j = 0; j < width*height; j++){maskdata[j] = 255*maskvec[j];}
+			}
+		}
+	}
+
+	delete[] best_src;
+	delete[] best_src_e;
+	delete[] best_src_f;
+	delete[] best_src_id;
+	delete[] best_dst;
+	delete[] best_dst_e;
+	delete[] best_dst_f;
+	delete[] best_dst_id;
+
+	if(visualizationLvl > 0){printf("total rematchKeyPoints time:          %5.5f\n",getTime()-total_rematchKeyPoints_time_start);}
+
+}
+
+void MassRegistrationPPR2::showEdges(std::vector<Eigen::Matrix4d> poses){
+	printf("showEdges\n");
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	for(unsigned long i = 0; i < poses.size(); i++){
+		printf("%i/%i -> %i\n",i,poses.size(),depthedge_nr_arraypoints[i]);
+		Eigen::Matrix4d p = poses[i];
+		float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+		float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+		float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+		long r,g,b;
+		r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1);
+		g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1);
+		b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1);
+
+		for(unsigned long c = 0; c < depthedge_nr_arraypoints[i]; c++){
+			pcl::PointXYZRGBNormal p;
+
+			float x = depthedge_arraypoints.at(i)[3*c+0];
+			float y = depthedge_arraypoints.at(i)[3*c+1];
+			float z = depthedge_arraypoints.at(i)[3*c+2];
+
+
+			p.x		= m00*x + m01*y + m02*z + m03;
+			p.y		= m10*x + m11*y + m12*z + m13;
+			p.z		= m20*x + m21*y + m22*z + m23;
+			p.b		= r;
+			p.g		= g;
+			p.r		= b;
+			cloud->points.push_back(p);
+		}
+		printf("showEdges %i -> %i\n",i,cloud->points.size());
+	}
+	if(cloud->points.size() > 0){
+		viewer->removeAllPointClouds();
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(cloud), "cloud");
+		viewer->spin();
+	}
+}
+
+MassFusionResults MassRegistrationPPR2::getTransforms(std::vector<Eigen::Matrix4d> poses){
+	if(visualizationLvl > 0){printf("start MassRegistrationPPR2::getTransforms(std::vector<Eigen::Matrix4d> poses)\n");}
+
+	unsigned long nr_frames = informations.size();
+	if(poses.size() != nr_frames){
+		printf("ERROR: poses.size()  = %i != informations.size() %i\n",poses.size(), informations.size());
+		return MassFusionResults();
+	}
+
+	fast_opt = false;
+	if(fast_opt){
+		printf("debugging... setting nr frames to 3: %s :: %i\n",__FILE__,__LINE__);
+		nr_frames = 3;
+	}
+
+	matchscores.resize(nr_frames);
+
+	for(unsigned long i = 0; i < nr_frames; i++){
+		matchscores[i].resize(nr_frames);
+		for(unsigned long j = 0; j < nr_frames; j++){
+			matchscores[i][j] = 1;
+		}
+
+		Eigen::Matrix4d p = poses[i];
+		float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+		float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+		float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+		if(!is_ok[i]){continue;}
+
+		matchids[i].resize(nr_frames);
+		matchdists[i].resize(nr_frames);
+
+		depthedge_matchids[i].resize(nr_frames);
+		depthedge_matchdists[i].resize(nr_frames);
+
+		Eigen::Matrix<double, 3, Eigen::Dynamic> & X	= points[i];
+		Eigen::Matrix<double, 3, Eigen::Dynamic> & Xn	= normals[i];
+		Eigen::Matrix<double, 3, Eigen::Dynamic> & tX	= transformed_points[i];
+		Eigen::Matrix<double, 3, Eigen::Dynamic> & tXn	= transformed_normals[i];
+		long count = nr_datas[i];
+		for(long c = 0; c < count; c++){
+			float x = X(0,c);
+			float y = X(1,c);
+			float z = X(2,c);
+			float xn = Xn(0,c);
+			float yn = Xn(1,c);
+			float zn = Xn(2,c);
+
+			tX(0,c)		= m00*x + m01*y + m02*z + m03;
+			tX(1,c)		= m10*x + m11*y + m12*z + m13;
+			tX(2,c)		= m20*x + m21*y + m22*z + m23;
+			tXn(0,c)	= m00*xn + m01*yn + m02*zn;
+			tXn(1,c)	= m10*xn + m11*yn + m12*zn;
+			tXn(2,c)	= m20*xn + m21*yn + m22*zn;
+		}
+	}
+
+	func->reset();
+	kpfunc->reset();
+	depthedge_func->reset();
+
+	long imgcount = 0;
+	char buf [1024];
+	if(visualizationLvl == 1){
+		if(use_depthedge){showEdges(poses);}
+		if(use_surface){
+			std::vector<Eigen::MatrixXd> Xv;
+			for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
+			sprintf(buf,"image%5.5i.png",imgcount++);
+			show(Xv,false,std::string(buf),imgcount);
+		}
+	}
+
+	if(savePath.size() != 0){
+		std::vector<Eigen::MatrixXd> Xv;
+		for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
+		savePCD(Xv,savePath+"_before.pcd");
+	}
+
+	rematch_time = 0;
+	residuals_time = 0;
+	opt_time = 0;
+	computeModel_time = 0;
+	setup_matches_time = 0;
+	setup_equation_time = 0;
+	setup_equation_time2 = 0;
+	solve_equation_time = 0;
+	total_time_start = getTime();
+
+	bool first = true;
+	std::vector<Eigen::Matrix4d> poses0 = poses;
+
+	for(long funcupdate=0; funcupdate < 100; ++funcupdate) {
+		double start_noise = func->getNoise();
+		if(getTime()-total_time_start > timeout){break;}
+		//if(visualizationLvl == 2){if(use_depthedge){showEdges(poses);}std::vector<Eigen::MatrixXd> Xv;for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount); showEdges(poses);}
+		if(visualizationLvl == 2){
+			if(use_depthedge){showEdges(poses);}
+			if(use_surface){
+				std::vector<Eigen::MatrixXd> Xv;
+				for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
+				sprintf(buf,"image%5.5i.png",imgcount++);
+				show(Xv,false,std::string(buf),imgcount);
+			}
+		}
+
+		for(long rematching=0; rematching < 10; ++rematching) {
+			//if(visualizationLvl == 3){if(use_depthedge){showEdges(poses);}std::vector<Eigen::MatrixXd> Xv;for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);}
+			if(visualizationLvl == 3){
+				if(use_depthedge){showEdges(poses);}
+				if(use_surface){
+					std::vector<Eigen::MatrixXd> Xv;
+					for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
+					sprintf(buf,"image%5.5i.png",imgcount++);
+					show(Xv,false,std::string(buf),imgcount);
+				}
+			}
+
+			std::vector<Eigen::Matrix4d> poses1 = poses;
+
+			double rematch_time_start = getTime();
+			//rematch(poses,poses0,use_surface,use_depthedge,first);
+			rematch(poses,poses0,use_surface,false,first);
+			first = false;
+			poses0 = poses;
+			rematch_time += getTime()-rematch_time_start;
+
+			for(long lala = 0; lala < 5; lala++){
+				if(visualizationLvl > 0){
+					printf("funcupdate: %i rematching: %i lala: %i\n",funcupdate,rematching,lala);
+					printf("total_time:          %5.5f\n",getTime()-total_time_start);
+					printf("rematch_time:        %5.5f\n",rematch_time);
+					printf("compM residuals_time:%5.5f\n",residuals_time);
+					printf("computeModel:        %5.5f\n",computeModel_time);
+					printf("opt_time:            %5.5f\n",opt_time);
+				}
+				//if(visualizationLvl == 4){if(use_depthedge){showEdges(poses);}std::vector<Eigen::MatrixXd> Xv;for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);}
+				if(visualizationLvl == 4){
+					if(use_depthedge){showEdges(poses);}
+					if(use_surface){
+						std::vector<Eigen::MatrixXd> Xv;
+						for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
+						sprintf(buf,"image%5.5i.png",imgcount++);
+						show(Xv,false,std::string(buf),imgcount);
+					}
+				}
+
+				rematch_time_start = getTime();
+				rematch(poses,poses0,false,use_depthedge,first);
+				rematch_time += getTime()-rematch_time_start;
+
+				std::vector<Eigen::Matrix4d> poses2b = poses;
+
+				double residuals_time_start = getTime();
+
+				Eigen::MatrixXd all_residuals;
+				Eigen::MatrixXd depthedge_all_residuals;
+				if(use_surface){	all_residuals			= getAllResiduals(poses);}
+				if(use_depthedge){	depthedge_all_residuals = depthedge_getAllResiduals(poses);}
+
+				residuals_time += getTime()-residuals_time_start;
+
+				double computeModel_time_start = getTime();
+
+				if(use_surface){	func->computeModel(all_residuals);}
+				if(use_depthedge){	depthedge_func->computeModel(depthedge_all_residuals);}
+
+				//printf("surface reg: %5.5f noise: %5.5f\n",func->regularization,func->noiseval);
+				//printf("edge:   reg: %5.5f noise: %5.5f\n",depthedge_func->regularization,depthedge_func->noiseval);
+
+				//				kpfunc->computeModel(all_KPresiduals);
+				//stopval = func->getNoise()*0.1;
+				//                printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval);
+
+				//                double stopval1 = func->getNoise()*0.1;
+				//                double stopval2 = kpfunc->getNoise()*0.1;
+				//                if(func->noiseval > 0 && kpfunc->noiseval > 0){ stopval = std::min(stopval1,stopval2);}
+				//                else if(func->noiseval > 0){                    stopval = func->getNoise()*0.1;}
+				//                else if(kpfunc->noiseval > 0){                  stopval = kpfunc->getNoise()*0.1;}
+				//                else{                                           break;}
+				//                printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval);
+				//                printf("kpreg: %f kpnoise:%f stopval: %f\n",kpfunc->regularization,kpfunc->noiseval,stopval);
+
+				computeModel_time += getTime()-computeModel_time_start;
+
+				double opt_time_start = getTime();
+				poses = optimize(poses);
+				opt_time += getTime()-opt_time_start;
+
+				if(isconverged(poses, poses2b, stopval, stopval)){break;}
+			}
+			if(isconverged(poses, poses1, stopval, stopval)){break;}
+		}
+
+		double noise_before = func->getNoise();
+		func->update();
+		double noise_after = func->getNoise();
+		double ratio = noise_after/noise_before;
+
+		double depthedge_noise_before = depthedge_func->getNoise();
+		depthedge_func->update();
+		double depthedge_noise_after = depthedge_func->getNoise();
+		double depthedge_ratio = depthedge_noise_after/depthedge_noise_before;
+
+		double kpnoise_before = kpfunc->getNoise();
+		kpfunc->update();
+		double kpnoise_after = kpfunc->getNoise();
+		double kpratio = kpnoise_after/kpnoise_before;
+
+		double reg1 = func->regularization;
+		double reg2 = kpfunc->regularization;
+		func->regularization = std::max(reg1,reg2);
+		kpfunc->regularization = std::max(reg1,reg2);
+
+		//		printf("func->noiseval: %f func->regularization: %f\n",func->noiseval,func->regularization);
+		//printf("ratio: %f kpratio: %f\n",ratio,kpratio);
+		//if(func->noiseval >= kpfunc->noiseval && 0.01*func->noiseval   < func->regularization){break;}
+		//if(func->noiseval < kpfunc->noiseval  && 0.01*kpfunc->noiseval < kpfunc->regularization){break;}
+		//if(ratio > 0.99){break;}
+
+		double change = 1-noise_after/start_noise;
+
+		if(visualizationLvl > 0){
+			printf("start_noise: %5.5f noise_before: %5.5f noise_after: %5.5f \n found ratio: %f/%f -> %f\n",start_noise, noise_before,noise_after,noise_after,start_noise,noise_after/start_noise);
+			printf("check1: %10.10f > 20.0 * %10.10f -> %i\n",func->noiseval,func->regularization, func->noiseval > 20.0*func->regularization);
+			printf("change: %f\n",change);
+		}
+
+
+		if(func->noiseval > 20.0*func->regularization && fabs(change) < 0.01){break;}// && ratio < 0.9){break;}
+	}
+
+	//	printf("total_time:          %5.5f\n",getTime()-total_time_start);
+	//	printf("rematch_time:        %5.5f\n",rematch_time);
+	//	printf("compM residuals_time:%5.5f\n",residuals_time);
+	//	printf("computeModel:        %5.5f\n",computeModel_time);
+	//	printf("opt_time:            %5.5f\n",opt_time);
+
+	//	printf("setup_matches_time:  %5.5f\n",setup_matches_time);
+	//	printf("setup_equation_time: %5.5f\n",setup_equation_time);
+	//	printf("solve_equation_time: %5.5f\n",solve_equation_time);
+
+	if(visualizationLvl > 0){
+		showEdges(poses);
+
+		std::vector<Eigen::MatrixXd> Xv;
+		for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
+		sprintf(buf,"image%5.5i.png",imgcount++);
+		show(Xv,false,std::string(buf),imgcount);
+	}
+
+	Eigen::Matrix4d firstinv = poses.front().inverse();
+	for(long i = 0; i < nr_frames; i++){poses[i] = firstinv*poses[i];}
+
+
+	if(savePath.size() != 0){
+		std::vector<Eigen::MatrixXd> Xv;
+		for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}
+		savePCD(Xv,savePath+"_after.pcd");
+	}
+
+	return MassFusionResults(poses,-1);
+}
+
+}
diff --git a/quasimodo/quasimodo_models/src/registration/Registration.cpp b/quasimodo/quasimodo_models/src/registration/Registration.cpp
index f650de37..80a7791c 100644
--- a/quasimodo/quasimodo_models/src/registration/Registration.cpp
+++ b/quasimodo/quasimodo_models/src/registration/Registration.cpp
@@ -19,7 +19,7 @@ FusionResults Registration::getTransform(Eigen::MatrixXd guess){
 	return FusionResults(guess,0);
 }
 
-void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){
+void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y, bool stop){
 
 	//printf("start show\n");
 	unsigned int s_nr_data = X.cols();
@@ -27,7 +27,7 @@ void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){
 	//printf("nr datas: %i %i\n",s_nr_data,d_nr_data);
 
 
-viewer->removeAllPointClouds();
+	viewer->removeAllPointClouds();
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 
@@ -40,7 +40,11 @@ viewer->removeAllPointClouds();
 	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud");
 	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud");
 	//printf("pre\n");
-    viewer->spin();
+	if(stop){
+		viewer->spin();
+	}else{
+		viewer->spinOnce();
+	}
 	//printf("post\n");
 	viewer->removeAllPointClouds();
 
diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp
index 89e8c4c1..df453e71 100644
--- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp
+++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp
@@ -1,6 +1,8 @@
 #include "registration/RegistrationRandom.h"
 #include <iostream>
 #include <fstream>
+#include <omp.h>
+#include <algorithm>
 
 namespace reglib
 {
@@ -36,11 +38,106 @@ double transformationdiff(Eigen::Affine3d & A, Eigen::Affine3d & B, double rotat
 	return r*rotationweight+t;
 }
 
+
+bool compareFusionResults (FusionResults i,FusionResults j) { return i.score > j.score; }
+
+bool RegistrationRandom::issame(FusionResults fr1, FusionResults fr2, int stepxsmall){
+	Eigen::Matrix4d current_guess = fr1.guess.inverse()*fr2.guess;
+	float m00 = current_guess(0,0); float m01 = current_guess(0,1); float m02 = current_guess(0,2); float m03 = current_guess(0,3);
+	float m10 = current_guess(1,0); float m11 = current_guess(1,1); float m12 = current_guess(1,2); float m13 = current_guess(1,3);
+	float m20 = current_guess(2,0); float m21 = current_guess(2,1); float m22 = current_guess(2,2); float m23 = current_guess(2,3);
+
+	double sum = 0;
+	double count = 0;
+	unsigned int s_nr_data = src->data.cols();
+	for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++){
+		float x		= src->data(0,i*stepxsmall);
+		float y		= src->data(1,i*stepxsmall);
+		float z		= src->data(2,i*stepxsmall);
+		float dx	= m00*x + m01*y + m02*z + m03 - x;
+		float dy	= m10*x + m11*y + m12*z + m13 - y;
+		float dz	= m20*x + m21*y + m22*z + m23 - z;
+		sum += sqrt(dx*dx+dy*dy+dz*dz);
+		count++;
+	}
+	double mean = sum/count;
+	//printf("mean: %f\n",mean);
+	return mean < 20*fr1.stop;
+}
+
+double getPscore(double p, double x, double y,double z, double r, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud){
+	double score = 0;
+	unsigned int nrp = cloud->points.size();
+	for(unsigned int i = 0; i < nrp; i++){
+		pcl::PointXYZRGBNormal po = cloud->points[i];
+		double dx = po.x-x;
+		double dy = po.y-y;
+		double dz = po.z-z;
+		double dist = sqrt(dx*dx+dy*dy+dz*dz);
+		score += pow(fabs(r-dist),p);
+	}
+	return score;
+}
+
+
+double getPsphere(double p, double & x, double & y,double & z, double & r, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud){
+	double h = 0.00001;
+
+
+	double score = getPscore(p,x,y,z,r,cloud);
+	double step = 0.01;
+	while(step > 0.0001){
+		double next_score = getPscore(p,x,y,z,r+step,cloud);
+		while(score > next_score){
+			score = next_score;
+			r = r+step;
+			next_score = getPscore(p,x,y,z,r+step,cloud);
+		}
+		step *= 0.1;
+	}
+
+
+
+	for(unsigned int it = 0; it < 1000; it++){
+		double score_start = score;
+		printf("it: %10.10i -> %10.10f pos: %5.5f %5.5f %5.5f %5.5f d: ",it,score,x,y,z,r);
+		double dx = -(getPscore(p,x+h,y,z,r,cloud) - getPscore(p,x-h,y,z,r,cloud))/(2*h);
+		double dy = -(getPscore(p,x,y+h,z,r,cloud) - getPscore(p,x,y-h,z,r,cloud))/(2*h);
+		double dz = -(getPscore(p,x,y,z+h,r,cloud) - getPscore(p,x,y,z-h,r,cloud))/(2*h);
+		double dr = -(getPscore(p,x,y,z,r+h,cloud) - getPscore(p,x,y,z,r-h,cloud))/(2*h);
+
+		printf("%5.5f %5.5f %5.5f %5.5f\n",dx,dy,dz,dr);
+
+		double step = 0.001;
+		while(step > 0.00000001){
+			double next_score = getPscore(p,x+step*dx,y+step*dy,z+step*dz,r+step*dr,cloud);
+			while(score > next_score){
+				score = next_score;
+				x = x+step*dx;
+				y = y+step*dy;
+				z = z+step*dz;
+				r = r+step*dr;
+				next_score = getPscore(p,x+step*dx,y+step*dy,z+step*dz,r+step*dr,cloud);
+			}
+			step *= 0.1;
+		}
+		double ratio = score/score_start;
+		if(ratio > 0.999){break;}
+	}
+
+	printf("final -> %10.10f pos: %5.5f %5.5f %5.5f %5.5f d: ",score,x,y,z,r);
+	return score;
+}
+
 FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){
+	//refinement->visualizationLvl = visualizationLvl;
+	//	refinement->viewer = viewer;
+	//	refinement->visualizationLvl = 2;
+
 
 	unsigned int s_nr_data = src->data.cols();//std::min(int(src->data.cols()),int(500000));
 	unsigned int d_nr_data = dst->data.cols();
-    refinement->allow_regularization = true;
+	refinement->allow_regularization = true;
 	//printf("s_nr_data: %i d_nr_data: %i\n",s_nr_data,d_nr_data);
 
 	int stepy = std::max(1,int(d_nr_data)/100000);
@@ -69,28 +166,177 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){
 	double s_mean_x = 0;
 	double s_mean_y = 0;
 	double s_mean_z = 0;
-	for(unsigned int i = 0; i < s_nr_data; i++){
-		s_mean_x += src->data(0,i);
-		s_mean_y += src->data(1,i);
-		s_mean_z += src->data(2,i);
-	}
-	s_mean_x /= double(s_nr_data);
-	s_mean_y /= double(s_nr_data);
-	s_mean_z /= double(s_nr_data);
 
 	double d_mean_x = 0;
 	double d_mean_y = 0;
 	double d_mean_z = 0;
-	for(unsigned int i = 0; i < d_nr_data; i++){
-        d_mean_x += dst->data(0,i);
-		d_mean_y += dst->data(1,i);
-		d_mean_z += dst->data(2,i);
+	int meantype = 2;
+	if(meantype == 0 || meantype == 2){
+		for(unsigned int i = 0; i < s_nr_data; i++){
+			s_mean_x += src->data(0,i);
+			s_mean_y += src->data(1,i);
+			s_mean_z += src->data(2,i);
+		}
+		s_mean_x /= double(s_nr_data);
+		s_mean_y /= double(s_nr_data);
+		s_mean_z /= double(s_nr_data);
+
+		for(unsigned int i = 0; i < d_nr_data; i++){
+			d_mean_x += dst->data(0,i);
+			d_mean_y += dst->data(1,i);
+			d_mean_z += dst->data(2,i);
+		}
+		d_mean_x /= double(d_nr_data);
+		d_mean_y /= double(d_nr_data);
+		d_mean_z /= double(d_nr_data);
+
+		if(meantype == 2){
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+			scloud->points.clear();
+			dcloud->points.clear();
+			for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = src->data(0,i)  ;p.y = src->data(1,i);p.z = src->data(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);}
+			for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = dst->data(0,i)  ;p.y = dst->data(1,i);p.z = dst->data(2,i);p.b = 0;p.g = 255;p.r = 0;dcloud->points.push_back(p);}
+
+
+//			double s_sphere_x = s_mean_x;
+//			double s_sphere_y = s_mean_y;
+//			double s_sphere_z = s_mean_z;
+			double s_sphere_r = 0;
+			double sscore = getPsphere(1.0, s_mean_x,s_mean_y,s_mean_z,s_sphere_r,scloud);
+
+
+//			double d_sphere_x = d_mean_x;
+//			double d_sphere_y = d_mean_y;
+//			double d_sphere_z = d_mean_z;
+			double d_sphere_r = 0;
+			double dscore = getPsphere(1.0, d_mean_x,d_mean_y,d_mean_z,d_sphere_r,dcloud);
+		}
+
+	}else if(meantype == 1){
+		std::vector<double> s_xvec;
+		std::vector<double> s_yvec;
+		std::vector<double> s_zvec;
+		s_xvec.resize(s_nr_data);
+		s_yvec.resize(s_nr_data);
+		s_zvec.resize(s_nr_data);
+		for(unsigned int i = 0; i < s_nr_data; i++){
+			s_xvec[i] = src->data(0,i);
+			s_yvec[i] = src->data(1,i);
+			s_zvec[i] = src->data(2,i);
+		}
+
+		std::sort (s_xvec.begin(), s_xvec.end());
+		std::sort (s_yvec.begin(), s_yvec.end());
+		std::sort (s_zvec.begin(), s_zvec.end());
+
+		std::vector<double> d_xvec;
+		std::vector<double> d_yvec;
+		std::vector<double> d_zvec;
+		d_xvec.resize(d_nr_data);
+		d_yvec.resize(d_nr_data);
+		d_zvec.resize(d_nr_data);
+		for(unsigned int i = 0; i < d_nr_data; i++){
+			d_xvec[i] = dst->data(0,i);
+			d_yvec[i] = dst->data(1,i);
+			d_zvec[i] = dst->data(2,i);
+		}
+
+		std::sort (d_xvec.begin(), d_xvec.end());
+		std::sort (d_yvec.begin(), d_yvec.end());
+		std::sort (d_zvec.begin(), d_zvec.end());
+
+		int src_mid_ind = s_nr_data/2;
+		int dst_mid_ind = d_nr_data/2;
+
+		s_mean_x = s_xvec[src_mid_ind];
+		s_mean_y = s_xvec[src_mid_ind];
+		s_mean_z = s_xvec[src_mid_ind];
+
+		d_mean_x = d_xvec[dst_mid_ind];
+		d_mean_y = d_xvec[dst_mid_ind];
+		d_mean_z = d_xvec[dst_mid_ind];
+
+//		printf("s mean:   %10.10f %10.10f %10.10f\n",s_mean_x,s_mean_y,s_mean_z);
+//		printf("s median: %10.10f %10.10f %10.10f\n",s_xvec[src_mid_ind],s_yvec[src_mid_ind],s_zvec[src_mid_ind]);
+//		printf("d mean:   %10.10f %10.10f %10.10f\n",d_mean_x,d_mean_y,d_mean_z);
+//		printf("d median: %10.10f %10.10f %10.10f\n",d_xvec[dst_mid_ind],d_yvec[dst_mid_ind],d_zvec[dst_mid_ind]);
 	}
-	d_mean_x /= double(d_nr_data);
-	d_mean_y /= double(d_nr_data);
-	d_mean_z /= double(d_nr_data);
 
-	double stop		= 0.00001;
+
+
+
+//		viewer->removeAllPointClouds();
+//		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+//		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+//		scloud->points.clear();
+//		dcloud->points.clear();
+//		for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = src->data(0,i)  ;p.y = src->data(1,i);p.z = src->data(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);}
+//		for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = dst->data(0,i)+2;p.y = dst->data(1,i);p.z = dst->data(2,i);p.b = 0;p.g = 255;p.r = 0;dcloud->points.push_back(p);}
+//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+//		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+//		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud");
+//		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud");
+
+//		double s_sphere_x = s_mean_x;
+//		double s_sphere_y = s_mean_y;
+//		double s_sphere_z = s_mean_z;
+//		double s_sphere_r = 0;
+//		double sscore = getPsphere(1.0, s_sphere_x,s_sphere_y,s_sphere_z,s_sphere_r,scloud);
+
+
+//		double d_sphere_x = d_mean_x+2;
+//		double d_sphere_y = d_mean_y;
+//		double d_sphere_z = d_mean_z;
+//		double d_sphere_r = 0;
+//		double dscore = getPsphere(1.0, d_sphere_x,d_sphere_y,d_sphere_z,d_sphere_r,dcloud);
+
+//		pcl::PointXYZRGB smin;
+//		smin.x = s_mean_x;
+//		smin.y = s_mean_y;
+//		smin.z = s_mean_z;
+
+//		pcl::PointXYZRGB dmin;
+//		dmin.x = d_mean_x+2;
+//		dmin.y = d_mean_y;
+//		dmin.z = d_mean_z;
+
+//		pcl::PointXYZRGB s_sphere_p;
+//		s_sphere_p.x = s_sphere_x;
+//		s_sphere_p.y = s_sphere_y;
+//		s_sphere_p.z = s_sphere_z;
+
+//		pcl::PointXYZRGB d_sphere_p;
+//		d_sphere_p.x = d_sphere_x;
+//		d_sphere_p.y = d_sphere_y;
+//		d_sphere_p.z = d_sphere_z;
+
+//		pcl::PointXYZRGB smed;
+//		smed.x = s_xvec[src_mid_ind];
+//		smed.y = s_yvec[src_mid_ind];
+//		smed.z = s_zvec[src_mid_ind];
+
+//		pcl::PointXYZRGB dmed;
+//		dmed.x = d_xvec[dst_mid_ind]+2;
+//		dmed.y = d_yvec[dst_mid_ind];
+//		dmed.z = d_zvec[dst_mid_ind];
+
+//		viewer->addSphere (smin, 0.01, 1, 0, 0, "smin");
+//		viewer->addSphere (dmin, 0.01, 1, 0, 0, "dmin");
+
+//		viewer->addSphere (smed, 0.01, 0, 0, 1, "smed");
+//		viewer->addSphere (dmed, 0.01, 0, 0, 1, "dmed");
+
+//		viewer->addSphere (s_sphere_p, s_sphere_r, 0, 1, 1, "s_sphere");
+//		viewer->addSphere (d_sphere_p, d_sphere_r, 0, 1, 1, "d_sphere");
+
+//		//printf("pre\n");
+//		viewer->spin();
+//		//printf("post\n");
+//		viewer->removeAllPointClouds();
+//	}
+
 
 	Eigen::Affine3d Ymean = Eigen::Affine3d::Identity();
 	Ymean(0,3) = d_mean_x;
@@ -102,11 +348,7 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){
 	Xmean(1,3) = s_mean_y;
 	Xmean(2,3) = s_mean_z;
 
-	std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > all_X;
-	std::vector< Eigen::Affine3d > all_res;
-	std::vector< int > count_X;
-	std::vector< float > score_X;
-    std::vector< std::vector< Eigen::VectorXd > > all_starts;
+
 	int stepxsmall = std::max(1,int(s_nr_data)/250);
 	Eigen::VectorXd Wsmall (s_nr_data/stepxsmall);
 	for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++){Wsmall(i) = src->information(0,i*stepxsmall);}
@@ -115,136 +357,151 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){
 	double sumtime = 0;
 	double sumtimeSum = 0;
 	double sumtimeOK = 0;
-	int r = 0;
 
-    refinement->viewer = viewer;
+	refinement->viewer = viewer;
 	refinement->visualizationLvl = 0;
-	//for(int r = 0; r < 1000; r++){
-//	while(true){
-//		double rx = 2.0*M_PI*0.0001*double(rand()%10000);
-//		double ry = 2.0*M_PI*0.0001*double(rand()%10000);
-//		double rz = 2.0*M_PI*0.0001*double(rand()%10000);
-    //double stop = 0;
-	double step = 0.1+2.0*M_PI/5;
-	for(double rx = 0; rx < 2.0*M_PI; rx += step){
-	for(double ry = 0; ry < 2.0*M_PI; ry += step)
-	for(double rz = 0; rz < 2.0*M_PI; rz += step){
-		printf("rx: %f ry: %f rz: %f\n",rx,ry,rz);
 
+	std::vector< double > rxs;
+	std::vector< double > rys;
+	std::vector< double > rzs;
+	double step = 0.1+2.0*M_PI/7;
+	//	for(double rx = 0; rx < 2.0*M_PI; rx += step){
+	//		for(double ry = 0; ry < 2.0*M_PI; ry += step){
+	//			for(double rz = 0; rz < 2.0*M_PI; rz += step){
+	//				rxs.push_back(rx);
+	//				rys.push_back(ry);
+	//				rzs.push_back(rz);
+	//			}
+	//		}
+	//	}
+	int steps = 6;
+	for(double rx = 0; rx < steps; rx ++){
+		for(double ry = 0; ry < steps; ry++){
+			for(double rz = 0; rz < steps; rz++){
+				rxs.push_back(2.0 * M_PI * (rx/double(steps+1)));
+				rys.push_back(2.0 * M_PI * (ry/double(steps+1)));
+				rzs.push_back(2.0 * M_PI * (rz/double(steps+1)));
+			}
+		}
+	}
+
+
+	refinement->target_points = 250;
+	unsigned int nr_r = rxs.size();
+
+	std::vector<FusionResults> fr_X;
+	fr_X.resize(nr_r);
+
+	//refinement->visualizationLvl = visualizationLvl;
+	#pragma omp parallel for num_threads(8)
+	for(unsigned int r = 0; r < nr_r; r++){
+		//printf("registering: %i / %i\n",r+1,nr_r);
 		double start = getTime();
 
 		double meantime = 999999999999;
-		if(r != 0){meantime = sumtimeSum/double(sumtimeOK+1.0);}
+		if(sumtimeOK != 0){meantime = sumtimeSum/double(sumtimeOK+1.0);}
 		refinement->maxtime = std::min(0.5,3*meantime);
 
-        Eigen::VectorXd startparam = Eigen::VectorXd(3);
-        startparam(0) = rx;
-        startparam(1) = ry;
-        startparam(2) = rz;
-
 		Eigen::Affine3d randomrot = Eigen::Affine3d::Identity();
-
-		randomrot =	Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) *
-					Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) *
-					Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());
+		randomrot =	Eigen::AngleAxisd(rxs[r], Eigen::Vector3d::UnitX()) *
+				Eigen::AngleAxisd(rys[r], Eigen::Vector3d::UnitY()) *
+				Eigen::AngleAxisd(rzs[r], Eigen::Vector3d::UnitZ());
 
 		Eigen::Affine3d current_guess = Ymean*randomrot*Xmean.inverse();//*Ymean;
-        refinement->target_points = 250;
-        FusionResults fr = refinement->getTransform(current_guess.matrix());
-//fr.timeout = timestopped;
-        double stoptime = getTime();
-        sumtime += stoptime-start;
-
-		if(!fr.timeout){
-			sumtimeSum += stoptime-start;
-			sumtimeOK++;
-		}
 
-		//printf("sumtime: %f\n",sumtime);
-        stop = fr.score;
-        Eigen::Matrix4d m = fr.guess;
-        current_guess = m;//fr.guess;
-        float m00 = current_guess(0,0); float m01 = current_guess(0,1); float m02 = current_guess(0,2); float m03 = current_guess(0,3);
-        float m10 = current_guess(1,0); float m11 = current_guess(1,1); float m12 = current_guess(1,2); float m13 = current_guess(1,3);
-        float m20 = current_guess(2,0); float m21 = current_guess(2,1); float m22 = current_guess(2,2); float m23 = current_guess(2,3);
-
-		Eigen::Matrix<double, 3, Eigen::Dynamic> Xsmall;
-		Xsmall.resize(Eigen::NoChange,s_nr_data/stepxsmall);
-		for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++){
-			float x		= src->data(0,i*stepxsmall);
-			float y		= src->data(1,i*stepxsmall);
-			float z		= src->data(2,i*stepxsmall);
-			Xsmall(0,i)	= m00*x + m01*y + m02*z + m03;
-			Xsmall(1,i)	= m10*x + m11*y + m12*z + m13;
-			Xsmall(2,i)	= m20*x + m21*y + m22*z + m23;
-		}
+		FusionResults fr = refinement->getTransform(current_guess.matrix());
+		//fr_X[r] = refinement->getTransform(current_guess.matrix());
 
-		bool exists = false;
-		for(unsigned int ax = 0; ax < all_X.size(); ax++){
-			Eigen::Matrix<double, 3, Eigen::Dynamic> axX = all_X[ax];
-			Eigen::Affine3d axT = all_res[ax];
-			double diff = (Xsmall-axX).colwise().norm().mean();
-			if(diff < 20*stop){
-				count_X[ax]++;
-                all_starts[ax].push_back(startparam);
-				int count = count_X[ax];
-				float score = score_X[ax];
-                std::vector< Eigen::VectorXd > starts = all_starts[ax];
-				for(int bx = ax-1; bx >= 0; bx--){
-					if(count_X[bx] < count_X[bx+1]){
-						count_X[bx+1]		= count_X[bx];
-						score_X[bx+1]		= score_X[bx];
-						all_X[bx+1]			= all_X[bx];
-						all_starts[bx+1]	= all_starts[bx];
-						all_res[bx+1]		= all_res[bx];
-
-
-						all_X[bx] = axX;
-						count_X[bx] = count;
-						score_X[bx] = score;
-                        all_starts[bx] = starts;
-						all_res[bx] = axT;
-					}else{break;}
-				}
-				exists = true;
-				break;
+#pragma omp critical
+		{
+			fr_X[r] = fr;
+
+			double stoptime = getTime();
+			sumtime += stoptime-start;
+			if(!fr_X[r].timeout){
+				sumtimeSum += stoptime-start;
+				sumtimeOK++;
 			}
+			//		bool exists2 = false;
+			//		for(unsigned int ax = 0; ax < fr_X.size(); ax++){
+			//			if(issame(fr, fr_X[ax],stepxsmall)){exists2 = true; break;}
+			//		}
+			//		if(!exists2){fr_X.push_back(fr);}
 		}
+	}
 
 
 
-		if(!exists){
-			all_X.push_back(Xsmall);
-			count_X.push_back(1);
-			score_X.push_back(stop);
-            all_starts.push_back(std::vector< Eigen::VectorXd >());
-            all_starts.back().push_back(startparam);
-			all_res.push_back(current_guess);
+	FusionResults fr = FusionResults();
+	refinement->allow_regularization = false;
+
+	int tpbef = refinement->target_points;
+
+	int mul = 2;
+	for(int tp = 500; tp <= 16000; tp *= 2){
+		std::sort (fr_X.begin(), fr_X.end(), compareFusionResults);
+		refinement->target_points = tp;
+
+		unsigned int nr_X = fr_X.size()/mul;
+#pragma omp parallel for num_threads(8)
+		for(unsigned int ax = 0; ax < nr_X; ax++){
+			fr_X[ax] = refinement->getTransform(fr_X[ax].guess);
 		}
-        r++;
+
+		for(unsigned int ax = 0; ax < fr_X.size(); ax++){
+			for(unsigned int bx = ax+1; bx < fr_X.size(); bx++){
+				if(issame(fr_X[bx], fr_X[ax],stepxsmall)){
+					fr_X[bx] = fr_X.back();
+					fr_X.pop_back();
+					bx--;
+				}
+			}
+		}
+		mul *= 2;
 	}
-}
 
+	//	for(int tp = 500; tp <= 1000; tp *= 2){
+	//		refinement->target_points = tp;
+
+	//		unsigned int nr_X = fr_X.size();
+	//		//#pragma omp parallel for num_threads(8)
+	//		for(unsigned int ax = 0; ax < nr_X; ax++){
+	//			fr_X[ax] = refinement->getTransform(fr_X[ax].guess);
+	//		}
+
+	//		for(unsigned int ax = 0; ax < fr_X.size(); ax++){
+	//			for(unsigned int bx = ax+1; bx < fr_X.size(); bx++){
+	//				if(issame(fr_X[bx], fr_X[ax],stepxsmall)){
+	//					fr_X[bx] = fr_X.back();
+	//					fr_X.pop_back();
+	//					bx--;
+	//				}
+	//			}
+	//		}
+	//	}
+
+	refinement->visualizationLvl = visualizationLvl;
+	std::sort (fr_X.begin(), fr_X.end(), compareFusionResults);
+	for(unsigned int ax = 0; ax < fr_X.size() && ax < 500; ax++){
+		fr.candidates.push_back(fr_X[ax].guess);
+		fr.counts.push_back(1);
+		fr.scores.push_back(fr_X[ax].score);
+	}
 
-	FusionResults fr = FusionResults();
-    refinement->allow_regularization = false;
 
-	int tpbef = refinement->target_points;
-	refinement->target_points = 2000;
-	for(unsigned int ax = 0; ax < all_X.size(); ax++){
-        Eigen::Matrix4d np = all_res[ax].matrix();
+	if(visualizationLvl > 0){
+		refinement->allow_regularization = true;
 		refinement->visualizationLvl = visualizationLvl;
-		if(ax < 25){
-			double start = getTime();
-			FusionResults fr1 = refinement->getTransform(np);
-			double stop = getTime();
-			np = fr1.guess;
+		refinement->target_points = 1000000;
+		refinement->maxtime = 10000;
+		for(unsigned int ax = 0; ax < fr_X.size() && ax < 20; ax++){
+			printf("%i -> %f\n",ax,fr_X[ax].score);
+			refinement->getTransform(fr_X[ax].guess);
 		}
 		refinement->visualizationLvl = 0;
-        fr.candidates.push_back(np);
-		fr.counts.push_back(count_X[ax]);
-		fr.scores.push_back(1.0/score_X[ax]);
 	}
+
+
 	refinement->target_points = tpbef;
 	return fr;
 }
diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp
index 9c918c85..78b78686 100644
--- a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp
+++ b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp
@@ -25,16 +25,16 @@ RegistrationRefinement::RegistrationRefinement(){
 
 	visualizationLvl = 1;
 
-    target_points = 250;
-    allow_regularization = true;
-    maxtime = 9999999;
+	target_points = 250;
+	allow_regularization = true;
+	maxtime = 9999999;
 
-	func = new DistanceWeightFunction2PPR2();
-	func->startreg			= 0.1;
-	func->debugg_print		= false;
+	//	func = new DistanceWeightFunction2PPR2();
+	//	func->startreg			= 0.1;
+	//	func->debugg_print		= false;
 }
 RegistrationRefinement::~RegistrationRefinement(){
-	if(func != 0){delete func; func = 0;}
+	//if(func != 0){delete func; func = 0;}
 	if(arraypoints != 0){delete arraypoints; arraypoints = 0;}
 	if(trees3d != 0){delete trees3d; trees3d = 0;}
 	if(a3d != 0){delete a3d; a3d = 0;}
@@ -47,7 +47,7 @@ void RegistrationRefinement::setDst(CloudData * dst_){
 	Y.resize(Eigen::NoChange,d_nr_data/stepy);
 	N.resize(Eigen::NoChange,d_nr_data/stepy);
 	ycols = Y.cols();
-	total_dweight.resize(ycols);
+
 
 	int count = 0;
 	for(unsigned int i = 0; i < d_nr_data/stepy; i++){
@@ -84,13 +84,20 @@ void RegistrationRefinement::setDst(CloudData * dst_){
 
 FusionResults RegistrationRefinement::getTransform(Eigen::MatrixXd guess){
 
-//	DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2();
-//	func->startreg			= 0.1;
-//	func->debugg_print		= false;
+	std::vector<double> total_dweight;
+	total_dweight.resize(ycols);
+
+	DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2();
+	if(allow_regularization){
+		func->startreg			= 0.1;
+	}else{
+		func->startreg			= 0.0;
+	}
+	func->debugg_print		= false;
 
 
 	unsigned int s_nr_data = src->data.cols();
-    int stepx = std::max(1,int(s_nr_data)/target_points);
+	int stepx = std::max(1,int(s_nr_data)/target_points);
 
 	double stop		= 0.00001;
 
@@ -142,16 +149,42 @@ FusionResults RegistrationRefinement::getTransform(Eigen::MatrixXd guess){
 	double score = 0;
 	stop = 99999;
 
-	double start = getTime();
+	if(visualizationLvl >= 3){show(X,Y,true);}
 
-bool timestopped = false;
+	//	//printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows());
+	//double startTest = getTime();
+	//std::vector<size_t>				ret_indexes1(1);
+	//std::vector<double>				out_dists_sqr1(1);
+	//nanoflann::KNNResultSet<double>	resultSet1(1);
+	//resultSet1.init(&ret_indexes1[0], &out_dists_sqr1[0] );
+	//double qp1 [3];
+	//nanoflann::SearchParams sp1 = nanoflann::SearchParams(10);
+
+	//for(unsigned long it = 0; true; it++){
+	//	qp1[0] = 0.001*(rand()%1000);
+	//	qp1[1] = 0.001*(rand()%1000);
+	//	qp1[2] = 0.001*(rand()%1000);
+	//	trees3d->findNeighbors(resultSet1, qp1, sp1);
+	//	int mid = ret_indexes1[0];
+	//	if(it % 1000){
+	//		printf("mps: %15.15f\n",double(it)/(getTime()-startTest));
+	//	}
+	//}
+
+
+
+
+	double start = getTime();
+	long matches = 0;
+	bool timestopped = false;
 	/// ICP
 	for(int funcupdate=0; funcupdate < 100; ++funcupdate) {
 		if( (getTime()-start) > maxtime ){timestopped = true; break;}
 		for(int rematching=0; rematching < 100; ++rematching) {
 			if( (getTime()-start) > maxtime ){timestopped = true; break;}
 
-#pragma omp parallel for
+
+			//#pragma omp parallel for
 			for(unsigned int i=0; i< xcols; ++i) {
 				std::vector<size_t>   ret_indexes(1);
 				std::vector<double> out_dists_sqr(1);
@@ -162,9 +195,10 @@ bool timestopped = false;
 				trees3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
 				matchid[i] = ret_indexes[0];
 			}
+			matches += xcols;
 
 			/// Find closest point
-#pragma omp parallel for
+			//#pragma omp parallel for
 			for(unsigned int i=0; i< xcols; ++i) {
 				int id = matchid[i];
 				Qn.col(i) = N.col(id);
@@ -176,33 +210,33 @@ bool timestopped = false;
 				if( (getTime()-start) > maxtime ){timestopped = true; break;}
 				/// Compute weights
 				switch(type) {
-					case PointToPoint:	{residuals = X-Qp;} 						break;
-					case PointToPlane:	{
-						residuals		= Eigen::MatrixXd::Zero(1,	xcols);
-                        for(unsigned int i=0; i<xcols; ++i) {
-							float dx = X(0,i)-Qp(0,i);
-							float dy = X(1,i)-Qp(1,i);
-							float dz = X(2,i)-Qp(2,i);
-							float qx = Qn(0,i);
-							float qy = Qn(1,i);
-							float qz = Qn(2,i);
-							float di = qx*dx + qy*dy + qz*dz;
-							residuals(0,i) = di;
-						}
-					}break;
-					default:			{printf("type not set\n");}					break;
+				case PointToPoint:	{residuals = X-Qp;} 						break;
+				case PointToPlane:	{
+					residuals		= Eigen::MatrixXd::Zero(1,	xcols);
+					for(unsigned int i=0; i<xcols; ++i) {
+						float dx = X(0,i)-Qp(0,i);
+						float dy = X(1,i)-Qp(1,i);
+						float dz = X(2,i)-Qp(2,i);
+						float qx = Qn(0,i);
+						float qy = Qn(1,i);
+						float qz = Qn(2,i);
+						float di = qx*dx + qy*dy + qz*dz;
+						residuals(0,i) = di;
+					}
+				}break;
+				default:			{printf("type not set\n");}					break;
 				}
-                for(unsigned int i=0; i<xcols; ++i) {residuals.col(i) *= rangeW(i);}
+				for(unsigned int i=0; i<xcols; ++i) {residuals.col(i) *= rangeW(i);}
 				switch(type) {
-					case PointToPoint:	{func->computeModel(residuals);} 	break;
-					case PointToPlane:	{func->computeModel(residuals);}	break;
-					default:  			{printf("type not set\n");} break;
+				case PointToPoint:	{func->computeModel(residuals);} 	break;
+				case PointToPlane:	{func->computeModel(residuals);}	break;
+				default:  			{printf("type not set\n");} break;
 				}
 
 				for(int rematching2=0; rematching2 < 3; ++rematching2) {
 					if( (getTime()-start) > maxtime ){timestopped = true; break;}
 					if(rematching2 != 0){
-#pragma omp parallel for
+						//#pragma omp parallel for
 						for(unsigned int i=0; i< xcols; ++i) {
 							std::vector<size_t>   ret_indexes(1);
 							std::vector<double> out_dists_sqr(1);
@@ -213,9 +247,10 @@ bool timestopped = false;
 							trees3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10));
 							matchid[i] = ret_indexes[0];
 						}
+						matches += xcols;
 
 						/// Find closest point
-#pragma omp parallel for
+						//#pragma omp parallel for
 						for(unsigned int i=0; i< xcols; ++i) {
 							int id = matchid[i];
 							Qn.col(i) = N.col(id);
@@ -228,41 +263,45 @@ bool timestopped = false;
 						if( (getTime()-start) > maxtime ){timestopped = true; break;}
 						if(inner != 0){
 							switch(type) {
-								case PointToPoint:	{residuals = X-Qp;} 						break;
-								case PointToPlane:	{
-									residuals		= Eigen::MatrixXd::Zero(1,	xcols);
-                                    for(unsigned int i=0; i< xcols; ++i) {
-										float dx = X(0,i)-Qp(0,i);
-										float dy = X(1,i)-Qp(1,i);
-										float dz = X(2,i)-Qp(2,i);
-										float qx = Qn(0,i);
-										float qy = Qn(1,i);
-										float qz = Qn(2,i);
-										float di = qx*dx + qy*dy + qz*dz;
-										residuals(0,i) = di;
-									}
-								}break;
-								default:			{printf("type not set\n");}					break;
+							case PointToPoint:	{residuals = X-Qp;} 						break;
+							case PointToPlane:	{
+								residuals		= Eigen::MatrixXd::Zero(1,	xcols);
+								for(unsigned int i=0; i< xcols; ++i) {
+									float dx = X(0,i)-Qp(0,i);
+									float dy = X(1,i)-Qp(1,i);
+									float dz = X(2,i)-Qp(2,i);
+									float qx = Qn(0,i);
+									float qy = Qn(1,i);
+									float qz = Qn(2,i);
+									float di = qx*dx + qy*dy + qz*dz;
+									residuals(0,i) = di;
+								}
+							}break;
+							default:			{printf("type not set\n");}					break;
 							}
-                            for(unsigned int i=0; i<xcols; ++i) {residuals.col(i) *= rangeW(i);}
+							for(unsigned int i=0; i<xcols; ++i) {residuals.col(i) *= rangeW(i);}
 						}
 
 						switch(type) {
-							case PointToPoint:	{W = func->getProbs(residuals); } 					break;
-							case PointToPlane:	{
-								W = func->getProbs(residuals);
-                                for(unsigned int i=0; i<xcols; ++i) {W(i) = W(i)*float((Xn(0,i)*Qn(0,i) + Xn(1,i)*Qn(1,i) + Xn(2,i)*Qn(2,i)) > 0.0);}
-							}	break;
-							default:			{printf("type not set\n");} break;
+						case PointToPoint:	{W = func->getProbs(residuals); } 					break;
+						case PointToPlane:	{
+							W = func->getProbs(residuals);
+							for(unsigned int i=0; i<xcols; ++i) {
+								//float angle = Xn(0,i)*Qn(0,i) + Xn(1,i)*Qn(1,i) + Xn(2,i)*Qn(2,i);
+								//W(i) = W(i)*exp(-0.5*2.0*fabs(1-angle));//float(angle > 0.0);
+								W(i) = W(i)*float((Xn(0,i)*Qn(0,i) + Xn(1,i)*Qn(1,i) + Xn(2,i)*Qn(2,i)) > 0.0);
+							}
+						}	break;
+						default:			{printf("type not set\n");} break;
 						}
 						Wold = W;
 
 						//Normalizing weights has an effect simmilar to one to one matching
 						//in that it reduces the effect of border points
 						if(normalize_matchweights){
-//							for(unsigned int i=0; i < ycols; ++i)	{	total_dweight[i] = 0.0000001;}//Reset to small number to avoid division by zero
-//							for(unsigned int i=0; i< xcols; ++i)	{	total_dweight[matchid[i]] += W(i);}
-//							for(unsigned int i=0; i< xcols; ++i)	{	W(i) = W(i)*(W(i)/total_dweight[matchid[i]]);}
+							//							for(unsigned int i=0; i < ycols; ++i)	{	total_dweight[i] = 0.0000001;}//Reset to small number to avoid division by zero
+							//							for(unsigned int i=0; i< xcols; ++i)	{	total_dweight[matchid[i]] += W(i);}
+							//							for(unsigned int i=0; i< xcols; ++i)	{	W(i) = W(i)*(W(i)/total_dweight[matchid[i]]);}
 
 							for(unsigned int i=0; i < ycols; ++i)	{	total_dweight[i] = 99990.0000001;}//Reset to small number to avoid division by zero
 							for(unsigned int i=0; i< xcols; ++i)	{	total_dweight[matchid[i]] = std::min(total_dweight[matchid[i]],residuals.col(i).norm());}
@@ -273,21 +312,59 @@ bool timestopped = false;
 
 						W = W.array()*rangeW.array()*rangeW.array();
 
+						if(visualizationLvl == 3){
+							show(X,Y,false);
+
+						}else if(visualizationLvl >= 4){
+							//printf("start show\n");
+							unsigned int s_nr_data = X.cols();
+							unsigned int d_nr_data = Y.cols();
+							//printf("nr datas: %i %i\n",s_nr_data,d_nr_data);
+
+
+							viewer->removeAllPointClouds();
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+							scloud->points.clear();
+							dcloud->points.clear();/*
+							for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);}
+							for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);}*/
+							for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 255.0*Wold(i);p.g = 255.0*Wold(i);p.r = 255.0*Wold(i);scloud->points.push_back(p);}
+							for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;			p.g = 0;			p.r = 255;			dcloud->points.push_back(p);}
+							viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+							viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+							viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud");
+							viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud");
+							//printf("pre\n");
+							viewer->spinOnce();
+							//printf("post\n");
+							viewer->removeAllPointClouds();
+						}
+
 						switch(type) {
-							case PointToPoint:	{
-								pcl::TransformationFromCorrespondences tfc1;
-								for(unsigned int c = 0; c < X.cols(); c++){tfc1.add(Eigen::Vector3f(X(0,c), X(1,c),X(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));}
-								Eigen::Affine3d rot = tfc1.getTransformation().cast<double>();
-								X = rot*X;
-								Xn = rot.rotation()*Xn;
-							}break;
-							case PointToPlane:	{point_to_plane2(X, Xn, Qp, Qn, W);}break;
-							default:  			{printf("type not set\n"); } break;
+						case PointToPoint:	{
+							pcl::TransformationFromCorrespondences tfc1;
+							for(unsigned int c = 0; c < X.cols(); c++){tfc1.add(Eigen::Vector3f(X(0,c), X(1,c),X(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));}
+							Eigen::Affine3d rot = tfc1.getTransformation().cast<double>();
+							X = rot*X;
+							Xn = rot.rotation()*Xn;
+						}break;
+						case PointToPlane:	{point_to_plane2(X, Xn, Qp, Qn, W);}break;
+						default:  			{printf("type not set\n"); } break;
 						}
 
-						stop = 100*func->getConvergenceThreshold();
-						score = Wold.sum()/(pow(func->getNoise(),2)*float(xcols));
 
+
+
+						stop = 0.1*func->getNoise();//100*func->getConvergenceThreshold();
+						//score = Wold.sum()/(pow(func->getNoise(),2)*float(xcols));
+						score = Wold.sum()/(func->getNoise()*float(xcols));
+
+
+						//						printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise());
+						//	printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows());
+						//						show(X,Y);
 						double stop1 = (X-Xo1).colwise().norm().mean();
 						Xo1 = X;
 						if(stop1 < stop) break;
@@ -310,7 +387,40 @@ bool timestopped = false;
 		if(fabs(1.0 - noise_after/noise_before) < 0.01){break;}
 	}
 
-    if(visualizationLvl >= 2){show(X,Y);}
+	//	printf("mps: %15.15f ratio: %15.15f\n",double(matches)/(getTime()-start),(double(matches)/(getTime()-start))/16000.0);
+	//exit(0);
+
+	//printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise());
+
+	if(visualizationLvl == 2){printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); show(X,Y);}
+	if(visualizationLvl >= 4){
+		printf("visualizationLvl: %i\n",visualizationLvl);
+		//printf("start show\n");
+		unsigned int s_nr_data = X.cols();
+		unsigned int d_nr_data = Y.cols();
+		//printf("nr datas: %i %i\n",s_nr_data,d_nr_data);
+
+
+		viewer->removeAllPointClouds();
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+		scloud->points.clear();
+		dcloud->points.clear();/*
+								for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);}
+								for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);}*/
+		for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 255.0*Wold(i);p.g = 255.0*Wold(i);p.r = 255.0*Wold(i);scloud->points.push_back(p);}
+		for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;			p.g = 0;			p.r = 255;			dcloud->points.push_back(p);}
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud");
+		viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud");
+		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud");
+		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud");
+		//printf("pre\n");
+		viewer->spin();
+		//printf("post\n");
+		viewer->removeAllPointClouds();
+	}
+
 
 	pcl::TransformationFromCorrespondences tfc;
 	tfc.reset();
@@ -322,8 +432,12 @@ bool timestopped = false;
 	//delete func;
 	//func = 0;
 
-    FusionResults fr = FusionResults(guess,stop);
+
+	if(func != 0){delete func; func = 0;}
+
+	FusionResults fr = FusionResults(guess,score);
 	fr.timeout = timestopped;
+	fr.stop = stop;
 	return fr;
 
 }
diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp
index aa5a9a3c..ef3732bc 100644
--- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp
+++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp
@@ -7,9 +7,27 @@ DistanceWeightFunction2::DistanceWeightFunction2(){
 	p = 0.5;
 	f = PNORM;
 	convergence_threshold = 0.0001;
+	savePath = "";
+	saveData.str("");
 }
 DistanceWeightFunction2::~DistanceWeightFunction2(){}
 
+MatrixXd DistanceWeightFunction2::getMat(std::vector<double> & vec){
+	unsigned long nr_data = vec.size();
+	Eigen::VectorXd v (nr_data);
+	for(unsigned long i = 0; i < nr_data; i++){v(i) = vec[i];}
+	return v;
+}
+
+void DistanceWeightFunction2::computeModel(std::vector<double> & vec){
+	computeModel(getMat(vec));
+}
+
+
+VectorXd DistanceWeightFunction2::getProbs(std::vector<double> & vec){
+	return getProbs(getMat(vec));
+}
+
 void tukey_weight(Eigen::VectorXd& r, double p) {
 	for(int i=0; i<r.rows(); ++i) {
 		if(r(i) > p) r(i) = 0.0;
@@ -52,12 +70,18 @@ VectorXd DistanceWeightFunction2::getProbs(MatrixXd mat){
 	return W;//VectorXf(mat.rows());
 }
 
-double DistanceWeightFunction2::getProb(double d){
+double DistanceWeightFunction2::getProb(double d, bool debugg){
 	printf("double DistanceWeightFunction2::getProbs(double d){ not implemented\n");
 	exit(0);
 	return 0;
 }
 
+double DistanceWeightFunction2::getProbInfront(double d, bool debugg){
+	printf("double DistanceWeightFunction2::getProbInfront(double d){ not implemented\n");
+	exit(0);
+	return 0;
+}
+
 double DistanceWeightFunction2::getNoise(){return 1;}
 bool DistanceWeightFunction2::update(){return true;}
 void DistanceWeightFunction2::reset(){}
diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR.cpp
index d847a6c9..24fdfe4d 100644
--- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR.cpp
+++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR.cpp
@@ -3,6 +3,9 @@
 namespace reglib{
 
 DistanceWeightFunction2PPR::DistanceWeightFunction2PPR(	double maxd_, int histogram_size_){
+	savePath = "";
+	saveData.str("");
+
 	update_size = true;
 
 	start_maxd = maxd_;
diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp
index a5aa5a2c..d52477d5 100644
--- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp
+++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp
@@ -3,11 +3,15 @@
 namespace reglib{
 
 DistanceWeightFunction2PPR2::DistanceWeightFunction2PPR2(	double maxd_, int histogram_size_){
+	savePath = "";
+	saveData.str("");
+
 
     fixed_histogram_size = false;
 
 	regularization		= 0.1;
 	maxd 				= maxd_;
+	mind				= 0;
 	histogram_size		= histogram_size_;
 	starthistogram_size = histogram_size;
 	blurval				= 5;
@@ -16,13 +20,17 @@ DistanceWeightFunction2PPR2::DistanceWeightFunction2PPR2(	double maxd_, int hist
 
     maxnoise = 99999999999999;
 	noiseval = 100.0;
-	
+
 	prob.resize(histogram_size+1);
 	prob[histogram_size] = 0;
+
+	infront.resize(histogram_size+1);
+	infront[histogram_size] = 1;
 	
 	histogram.resize(histogram_size+1);
 	blur_histogram.resize(histogram_size+1);
 	noise.resize(histogram_size+1);
+	noisecdf.resize(histogram_size+1);
 
 	refine_mean = false;
 	refine_mul = false;
@@ -83,7 +91,11 @@ DistanceWeightFunction2PPR2::DistanceWeightFunction2PPR2(	double maxd_, int hist
 	refine_std			= true;
 	nr_refineiters		= 1;
 	convergence_threshold = 0.05;
+
+	ggd = false;
+	compute_infront = false;
 }
+
 DistanceWeightFunction2PPR2::~DistanceWeightFunction2PPR2(){}
 
 class Gaussian3 {
@@ -105,6 +117,16 @@ class Gaussian3 {
 		double dx = mean-x;
 		return mul*exp(dx*dx*scaledinformation);
 	}
+
+	double getcdf(double x)
+	{
+	   return 0.5 * erfc(-((x-mean)/stdval) * M_SQRT1_2);
+	}
+
+
+    void print(){
+        printf("Gaussian3:: mul = %5.5f mean = %5.5f stdval = %5.5f\n",mul,mean,stdval);
+    }
 };
 
 class gaussian2 {
@@ -188,6 +210,77 @@ double fitCurrentHist(std::vector<float> & A, std::vector<float> & B){
 	return pbest;
 }
 
+void blurHistogram3(std::vector<float> & blur_hist,  std::vector<float> & hist, float stdval, int histogramsize, bool debugg_print = false){
+	int nr_data = histogramsize;//blur_hist.size();
+
+//if(debugg_print){
+//	for(int i = 0; i < nr_data; i++){
+//		printf("%i -> %f\n",i,hist[i]);
+//	}
+//}
+
+	int nr_data2 = 2*nr_data;
+
+	std::vector<float> tmphist;
+	tmphist.resize(nr_data2);
+
+	std::vector<float> tmphist_blur;
+	tmphist_blur.resize(nr_data2);
+
+	for(int i = 0; i < nr_data; i++){
+		tmphist[i]			= hist[nr_data-1-i];
+		tmphist[nr_data+i]	= hist[i];
+		tmphist_blur[i]			= 0;
+		tmphist_blur[nr_data+i]	= 0;
+	}
+
+	double info = -0.5/(stdval*stdval);
+	double weights[nr_data2];
+	for(int i = 0; i < nr_data2; i++){weights[i] = 0;}
+	for(int i = 0; i < nr_data2; i++){
+		double current = exp(i*i*info);
+		weights[i] = current;
+		if(current < 0.001){break;}
+	}
+
+	int offset = 4.0*stdval;
+	offset = std::max(4,offset);
+	for(int i = 0; i < nr_data2; i++){
+		double v = tmphist[i];
+		if(v == 0){continue;}
+
+		//if(debugg_print){printf("v: %5.5f\n",v);}
+
+		int start	= std::max(0,i-offset);
+		int stop	= std::min(nr_data2,i+offset+1);
+
+		for(int j = start; j < stop; j++){tmphist_blur[j] += v*weights[abs(i-j)];}
+		//if(debugg_print){exit(0);}
+	}
+	for(int i = 0; i < nr_data; i++){blur_hist[i] = tmphist_blur[i+nr_data];}
+
+	float bef = 0;
+	float aft = 0;
+	for(int i = 0; i < nr_data; i++){
+		bef += hist[i];
+		aft += blur_hist[i];
+	}
+
+	for(int i = 0; i < nr_data; i++){blur_hist[i] *= bef/aft;}
+
+//	printf("hist = [");				for(int k = 0; k < 300 && k < hist.size();		k++){printf("%i ",int(hist[k]));}		printf("];\n");
+//	printf("hist_smooth = [");		for(int k = 0; k < 300 && k < blur_hist.size();	k++){printf("%i ",int(blur_hist[k]));}	printf("];\n");
+//	double p = fitCurrentHist(hist, blur_hist);
+//	printf("p = %f;\n",p);
+//	double p2 = fitCurrentHist2(hist, blur_hist);
+//	printf("p2 = %f;\n",p2);
+//	for(double p = 1.0; p < 3; p+=0.001){
+//		printf("%3.3f -> %8.8f\n",p,scoreCurrentHist(p, hist, blur_hist));
+//	}
+//	exit(0);
+}
+
+
 void blurHistogram2(std::vector<float> & blur_hist,  std::vector<float> & hist, float stdval,bool debugg_print = false){
 	int nr_data = blur_hist.size();
 	int nr_data2 = 2*nr_data;
@@ -291,6 +384,74 @@ const double step_h = 0.00001;
 const unsigned int step_iter = 25;
 const double cutoff_exp = -14;
 
+double scoreCurrent3(double mul, double mean, double stddiv, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+	double sum = 0;
+	double invstd = 1.0/stddiv;
+	for(unsigned int i = 0; i < nr_data; i++){
+		double dx = fabs(X[i] - mean)*invstd;
+		double inp = -0.5*pow(dx,power);
+		if(inp < cutoff_exp){sum += Y[i];}
+		else{
+			//double diff = Y[i]*Y[i]*(mul*exp(inp) - Y[i]);
+			double diff = (mul*exp(inp) - Y[i]);
+			//			if(diff > 0){	sum += costpen*diff;}
+			//			else{			sum -= diff;}
+			if(diff > 0){	sum += diff*diff;}
+			else{			sum -= diff;}
+		}
+	}
+	return sum;
+}
+
+double fitStdval3(double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+	int iter = 25;
+	double h = 0.000000001;
+
+	double std_max = std_mid*2;
+	double std_min = 0;
+	for(int i = 0; i < iter; i++){
+		std_mid = (std_max+std_min)/2;
+		double std_neg = scoreCurrent3(mul,mean,std_mid-h,power,X,Y,nr_data,costpen);
+		double std_pos = scoreCurrent3(mul,mean,std_mid+h,power,X,Y,nr_data,costpen);
+		if(std_neg < std_pos){	std_max = std_mid;}
+		else{					std_min = std_mid;}
+	}
+	return std_mid;
+}
+
+double fitPower3(double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+	int iter = 25;
+	double h = 0.000000001;
+
+	double power_max = power*2;
+	double power_min = 0.001;
+	for(int i = 0; i < iter; i++){
+		power = (power_max+power_min)/2;
+		double std_neg = scoreCurrent3(mul,mean,std_mid,power-h,X,Y,nr_data,costpen);
+		double std_pos = scoreCurrent3(mul,mean,std_mid,power+h,X,Y,nr_data,costpen);
+		if(std_neg < std_pos){	power_max = power;}
+		else{					power_min = power;}
+	}
+	power = (power_max+power_min)/2;
+	return power;
+}
+
+double fitMean3(double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+	int iter = 10;
+	double h = 0.000000001;
+	double mean_max = mean+10;
+	double mean_min = mean-10;
+
+	for(int i = 0; i < iter; i++){
+		mean = (mean_max+mean_min)/2;
+		double std_neg = scoreCurrent3(mul,mean-h,std_mid,power,X,Y,nr_data,costpen);
+		double std_pos = scoreCurrent3(mul,mean+h,std_mid,power,X,Y,nr_data,costpen);
+		if(std_neg < std_pos){	mean_max = mean;}
+		else{					mean_min = mean;}
+	}
+	return mean;
+}
+
 
 double scoreCurrent2(double bias, double mul, double mean, double stddiv, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
 	double info = -0.5/(stddiv*stddiv);
@@ -301,7 +462,8 @@ double scoreCurrent2(double bias, double mul, double mean, double stddiv, std::v
 		if(inp < cutoff_exp){sum += Y[i];}
 		else{
 			double diff = mul*exp(info*dx*dx) - Y[i];
-			if(diff > 0){	sum += costpen*diff;}
+			//if(diff > 0){	sum += costpen*diff;}
+			if(diff > 0){	sum += diff*diff;}
 			else{			sum -= diff;}
 		}
 	}
@@ -342,10 +504,13 @@ double fitBias2(double bias, double mul, double mean, double std_mid, std::vecto
 }
 
 double fitMean2(double bias,double mul, double mean, double std_mid, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
-	int iter = 25;
+	int iter = 10;
 	double h = 0.000000001;
-	double mean_max = mean*2;
-	double mean_min = 0;
+	double mean_max = mean+1;
+	double mean_min = mean-1;
+
+	//	double mean_max = mean*2;
+	//	double mean_min = 0;
 
 	for(int i = 0; i < iter; i++){
 		mean = (mean_max+mean_min)/2;
@@ -419,181 +584,338 @@ Gaussian3 getModel(double & stdval,std::vector<float> & hist, bool uniform_bias,
 	}
 	return Gaussian3(mul,mean,stdval);
 }
+/*
+void getGGModel(std::vector<float> & hist, unsigned int nr_bins){
 
-double DistanceWeightFunction2PPR2::getNoise(){return regularization+noiseval;}// + stdval*double(histogram_size)/maxd;}
 
-void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){
-	//debugg_print = false;
+	double mul = hist[0];
+	double mean = 0;
+	//unsigned int nr_bins = hist.size();
 
-	const unsigned int nr_data = mat.cols();
-	const int nr_dim = mat.rows();
-if(!fixed_histogram_size){
-	if(first){
-		first = false;
-		double sum = 0;
-		for(unsigned int j = 0; j < nr_data; j++){
-			for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);}
+	for(unsigned int k = 1; k < nr_bins; k++){
+		if(hist[k] > mul){
+			mul = hist[k];
+			mean = k;
 		}
-		stdval2 = sqrt(sum / double(nr_data*nr_dim));
-		if(debugg_print){printf("stdval2: %f\n",stdval2);}
 	}
 
-    if(update_size){
-		if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);}
-		maxd  = fabs(meanval2) + (stdval2 + regularization)*target_length;
-		if(debugg_print){printf("maxd: %f\n",maxd);}
-	}
+	double maxhist = mul;
 
-	double nr_inside = 0;
-	for(unsigned int j = 0; j < nr_data; j++){
-		for(int k = 0; k < nr_dim; k++){
-			if(fabs(mat(k,j)) < maxd){nr_inside++;}
+
+	std::vector<float> X;
+	std::vector<float> Y;
+	for(unsigned int k = 0; k < nr_bins; k++){
+		if(hist[k]  > mul*0.01){
+			X.push_back(k);
+			Y.push_back(hist[k]);
 		}
 	}
 
-	if(update_size){
-		nr_inside = std::min(nr_inside,double(nr_dim)*nr_inliers);
-		histogram_size = std::min(int(prob.size()),std::max(50,std::min(1000,int(0.5 + nr_inside/data_per_bin))));
-		blurval = blur*double(histogram_size);
+	unsigned int nr_data_opt = X.size();
+
+	double ysum = 0;
+	for(unsigned int i = 0; i < nr_data_opt; i++){ysum += fabs(Y[i]);}
+
+	double std_mid = 0;
+	for(unsigned int i = 0; i < nr_data_opt; i++){std_mid += (X[i]-mean)*(X[i]-mean)*fabs(Y[i])/ysum;}
+	double stdval = sqrt(std_mid);
+
+	double power = 2;
+	//printf("mean: %10.10f mul: %10.10f\n",mean,mul,std_mid,power);
+
+	printf("hist = [");
+	for(unsigned int k = 0; k < nr_bins; k++){printf("%5.5f ",float(hist[k])/maxhist);}
+	printf("];\n");
 
-		if(debugg_print){printf("nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);}
+	char buf [1024*1024];
+	sprintf(buf,"ggd = [");
+	int nr_refineiters = 10;
+
+	double pre_mean = mean;
+	double pre_std = stdval;
+	double pre_power = power;
+
+	for(int i = 0; i < nr_refineiters; i++){
+		printf("ggd%i = [",i);
+		for(unsigned int k = 0; k < nr_bins; k++){
+			printf("%5.5f ",mul/maxhist * exp(-0.5*pow(fabs(double(k)-mean)/stdval,power)));
+		}
+		printf("];\n");
+
+		if(i != 0){sprintf(buf,";");}
+		sprintf(buf,"%10.10f, %10.10f, %10.10f, %10.10f",mul,mean,stdval,power);
+
+		stdval	= fitStdval3(	mul,mean,stdval,power,X,Y,nr_data_opt,3);
+		power	= fitPower3(	mul,mean,stdval,power,X,Y,nr_data_opt,3);
+		mean	= fitMean3(		mul,mean,stdval,power,X,Y,nr_data_opt,3);
+		if(fabs(mean - pre_mean) < 0.1 && fabs(stdval - pre_std) < 0.1 && fabs(power - pre_power) < 0.01){
+			break;
+		}
+		pre_mean = mean;
+		pre_std = stdval;
+		pre_power = power;
 	}
+	sprintf(buf,"];\n");
+	printf("%s\n",buf);
+
+	printf("mean: %10.10f mul: %10.10f std: %10.10f power: %10.10f\n",mean,mul,std_mid,power);
+	//return Gaussian3(mul,mean,stdval);
 }
+*/
+
+double DistanceWeightFunction2PPR2::getNoise(){return regularization+noiseval;}// + stdval*double(histogram_size)/maxd;}
+
+#include <stdio.h>
+#include <execinfo.h>
+#include <signal.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){
+	const unsigned int nr_data = mat.cols();
+	const int nr_dim = mat.rows();
+
+double startTime = getCurrentTime3();
+//printf("\n################################################### ITER:%i ############################################################\n",iter);
+	if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);}
+	if(!fixed_histogram_size){
+		if(first){
+			first = false;
+			double sum = 0;
+			for(unsigned int j = 0; j < nr_data; j++){
+				for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);}
+			}
+			stdval2 = sqrt(sum / double(nr_data*nr_dim));
+			if(debugg_print){printf("stdval2: %f\n",stdval2);}
+		}
+
+		if(update_size){
+			//maxd = fabs(meanval2) + (stdval2 + regularization)*target_length;
+			if(bidir){
+				maxd = meanval2 + 2.0*(stdval2 + regularization)*target_length;
+				mind = meanval2 - 2.0*(stdval2 + regularization)*target_length;
+			}else{
+				maxd = fabs(meanval2) + (stdval2 + regularization)*target_length;
+				mind = 0;
+			}
+			if(debugg_print){
+				printf("maxd: %f\n",maxd);
+				printf("mind: %f\n",mind);
+			}
+		}
+
+		double nr_inside = 0;
+		for(unsigned int j = 0; j < nr_data; j++){
+			for(int k = 0; k < nr_dim; k++){
+				//if(fabs(mat(k,j)) < maxd){nr_inside++;}
+				double d = mat(k,j);
+				if(mind < d && d < maxd){nr_inside++;}
+			}
+		}
 
-	const double histogram_mul = double(histogram_size)/maxd;
+		if(update_size){
+			nr_inside = std::min(nr_inside,double(nr_dim)*nr_inliers);
+			int new_histogram_size = std::min(int(prob.size()),std::max(50,int(0.5 + nr_inside/data_per_bin)));
+			histogram_size = std::min(1000,new_histogram_size);//std::min(int(prob.size()),std::max(50,std::min(1000,int(0.5 + nr_inside/data_per_bin))));
+			blurval = blur*double(histogram_size)*float(histogram_size)/float(new_histogram_size);
 
-	if(debugg_print){printf("histogram_mul: %f histogram_size: %f maxd: %f\n",histogram_mul,double(histogram_size),maxd);}
+			if(debugg_print){printf("nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);}
+		}
+	}
+//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();
+	histogram_mul	= double(histogram_size)/maxd;
+	histogram_mul2	= double(histogram_size)/(maxd-mind);
+//	if(bidir){	histogram_mul2 = double(histogram_size)/(2.0*maxd);}
+//	else{		histogram_mul2 = double(histogram_size)/maxd;}
+
+	if(debugg_print){printf("histogram_mul: %5.5f histogram_size: %5.5f maxd: %5.5f mind: %5.5f\n",histogram_mul,double(histogram_size),maxd,mind);}
 
 	double start_time = getCurrentTime3();
 
 	for(int j = 0; j < histogram_size; j++){histogram[j] = 0;}
 	for(unsigned int j = 0; j < nr_data; j++){
 		for(int k = 0; k < nr_dim; k++){
-
-			double ind = fabs(mat(k,j))*histogram_mul;
-			double w2 = ind-int(ind);
-			double w1 = 1-w2;
-			if(ind >= 0 && (ind+0.5) < histogram_size){
-				histogram[int(ind+0.5)]++;
+			double ind = getInd(mat(k,j));
+			if(ind >= 0 && (ind+0.00001) < histogram_size){
+				histogram[int(ind+0.00001)]++;
 			}
-
-			//printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);
 		}
 	}
+//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();
+	//double ind = getInd(d,debugg);
 
-if(!fixed_histogram_size){
-	histogram[0]*=2;
-}
 	start_time = getCurrentTime3();
-	blurHistogram2(blur_histogram,histogram,blurval,false);
-
-    Gaussian3 g = getModel(stdval,blur_histogram,uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean);
+	blurHistogram3(blur_histogram,histogram,blurval,histogram_size,debugg_print);
+//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();
+//	stdval2 = 0;
+//	Gaussian3 g  = getModel(stdval, blur_histogram,	uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean);
+//	//Gaussian3 g2 = getModel(stdval2,histogram,		uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean);
+//	stdval2 = std::max(1.0,stdval2);
+//	stdval2 = maxd*stdval2/float(histogram_size);
+
+//	g.stdval = std::max(0.0001*double(histogram_size)/(maxd),std::min(g.stdval,maxnoise*double(histogram_size)/maxd));
+
+//	noiseval = maxd*g.stdval/float(histogram_size);
+
+//	stdval	= g.stdval;
+
+//	double Ib = 1/(blurval*blurval);
+//	double Ic = 1/(stdval*stdval);
+//	double Ia = Ib*Ic/(Ib-Ic);
+//	double corrected = sqrt(1.0/Ia);
+//	double rescaleval = 1;
+
+//	if(rescaling){
+//		if(stdval > blurval){
+//			g.stdval = corrected;
+//			rescaleval = stdval/corrected;
+//		}else{g.stdval = 1;}
+//	}
 
-	//printf("found stdval: %f\n",stdval);
-	stdval2 = 0;
-    Gaussian3 g2 = getModel(stdval2,histogram,uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean);
-	stdval2 = std::max(1.0,stdval2);
-	stdval2 = maxd*stdval2/float(histogram_size);
+//	stdval = g.stdval;
+//	mulval	= g.mul;
+//	meanval	= g.mean;
 
-	g.stdval = std::max(0.0001*double(histogram_size)/maxd,std::min(g.stdval,maxnoise*double(histogram_size)/maxd));
+//	meanval2 = maxd*meanval/float(histogram_size);
 
-	noiseval = maxd*g.stdval/float(histogram_size);
+//	g.stdval += histogram_size*regularization/maxd;
+//	g.update();
 
 
-	stdval	= g.stdval;
+	Gaussian3 g  = getModel(stdval, blur_histogram,	uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean);
+//    g.print();
 
-	double Ib = 1/(blurval*blurval);
-	double Ic = 1/(stdval*stdval);
-	double Ia = Ib*Ic/(Ib-Ic);
-	double corrected = sqrt(1.0/Ia);
-	double rescaleval = 1;
+//    GaussianDistribution * gd = new GaussianDistribution(refine_std,zeromean,refine_mean,refine_mul,costpen,nr_refineiters);
+//    gd->train(blur_histogram,histogram_size);
+//    gd->print();
+//    delete gd;
+//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();
+	g.stdval = std::max(0.0001*double(histogram_size)/(maxd),std::min(g.stdval,maxnoise*double(histogram_size)/maxd));
 
-	if(rescaling){
-		if(stdval > blurval){
-			g.stdval = corrected;
-			rescaleval = stdval/corrected;
-		}else{g.stdval = 1;}
-	}
+	//noiseval = maxd*g.stdval/float(histogram_size);
+	noiseval = (maxd-mind)*g.stdval/float(histogram_size);
+	if(bidir){noiseval *= 0.5;}
 
 	stdval = g.stdval;
 	mulval	= g.mul;
 	meanval	= g.mean;
 
-	meanval2 = maxd*meanval/float(histogram_size);
+	meanval2 = (maxd-mind)*(meanval/float(histogram_size)) + mind;
+	stdval2 = noiseval;//getNoise();
+	//meanval2 = maxd*meanval/float(histogram_size);
 
 	g.stdval += histogram_size*regularization/maxd;
 	g.update();
 
-	for(int k = 0; k < histogram_size; k++){	noise[k] = g.getval(k);}
-
-	std::vector<float> new_histogram;
-	if(rescaling){
-		new_histogram.resize(blur_histogram.size());
-		double sum = 0;
-		for(int k = 0; k < histogram_size; k++){
-			double ks = double(k)*rescaleval;
-			double w2 = ks-int(ks);
-			double w1 = 1-w2;
-			double newh = blur_histogram[histogram_size-1];
-			if(ks < histogram_size){
-				newh = blur_histogram[int(ks)]*w1 + blur_histogram[int(ks+1)]*w2;
-				sum +=  newh;
-			}
-			new_histogram[k] = newh;
-		}
+	for(int k = 0; k < histogram_size; k++){noise[k] = g.getval(k);}
+	if(compute_infront){
+		for(int k = 0; k < histogram_size; k++){noisecdf[k] = g.getcdf(k);}
 	}
+//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();
+//	std::vector<float> new_histogram;
+//	if(rescaling){
+//		new_histogram.resize(blur_histogram.size());
+//		double sum = 0;
+//		for(int k = 0; k < histogram_size; k++){
+//			double ks = double(k)*rescaleval;
+//			double w2 = ks-int(ks);
+//			double w1 = 1-w2;
+//			double newh = blur_histogram[histogram_size-1];
+//			if(ks < histogram_size){
+//				newh = blur_histogram[int(ks)]*w1 + blur_histogram[int(ks+1)]*w2;
+//				sum +=  newh;
+//			}
+//			new_histogram[k] = newh;
+//		}
+//	}
 	
 	for(int k = 0; k < histogram_size; k++){
 		if(max_under_mean && k < g.mean){	prob[k] = maxp;	}
 		else{
 			double hs = blur_histogram[k]+1;
-			if(rescaling){hs = new_histogram[k]+1;}
 			prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data
+			infront[k] = (1-prob[k])*noisecdf[k];
 		}
 	}
-
+//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();
 	nr_inliers = 0;
 	for(unsigned int j = 0; j < nr_data; j++){
 		float inl  = 1;
 		float ninl = 1;
 		for(int k = 0; k < nr_dim; k++){
-			double ind = fabs(mat(k,j))*histogram_mul;
-			float p = 0;
-			if(ind >= 0 && (ind+0.5) < histogram_size){p = prob[int(ind+0.5)];}
+			float p = getProb(mat(k,j));
 			inl *= p;
 			ninl *= 1.0-p;
 		}
 		double d = inl / (inl+ninl);
 		nr_inliers += d;
 	}
+//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();//SLOW
+	if(debugg_print){printf("hist = [");			for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ",	int(histogram[k]));}		printf("];\n");}
+	if(debugg_print){printf("noise = [");			for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ",	int(noise[k]));}			printf("];\n");}
+	if(debugg_print){printf("noisecdf = [");		for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));}			printf("];\n");}
+	if(debugg_print){printf("hist_smooth = [");		for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ",	int(blur_histogram[k]));}	printf("];\n");}
+	if(debugg_print){printf("prob = [");			for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);}					printf("];\n");}
+	if(debugg_print){printf("infront = [");			for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);}				printf("];\n");}
+	if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");}
 
-	if(debugg_print){printf("hist = [");				for(int k = 0; k < 300 && k < histogram_size; k++){printf("%i ",int(histogram[k]));}		printf("];\n");}
-    if(debugg_print){printf("noise = [");				for(int k = 0; k < 300 && k < int(noise.size()); k++){printf("%i ",int(noise[k]));}			printf("];\n");}
-    if(debugg_print){printf("hist_smooth = [");			for(int k = 0; k < 300 && k < int(blur_histogram.size()); k++){printf("%i ",int(blur_histogram[k]));}	printf("];\n");}
-    if(debugg_print){printf("new_histogram = [");		for(int k = 0; k < 300 && k < int(new_histogram.size()); k++){printf("%i ",int(new_histogram[k]));}	printf("];\n");}
 	if(false){printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);}
 	if(!fixed_histogram_size && update_size ){
-		double next_maxd  = meanval2 + (stdval2 + regularization)*target_length;
-		//printf("mean %f stdval %f regularization: %f\n",meanval2,stdval2,regularization);
+
+		double next_maxd;
+		double next_mind;
+		if(bidir){
+			next_maxd = meanval2 + 2.0*(stdval2 + regularization)*target_length;
+			next_mind = meanval2 - 2.0*(stdval2 + regularization)*target_length;
+		}else{
+			next_maxd = fabs(meanval2) + (stdval2 + regularization)*target_length;
+			next_mind = 0;
+		}
+
 		double logdiff = log(next_maxd/maxd);
-		//if(debugg_print){printf("maxd: %f next_maxd: %f logdiff: %f \n",maxd,next_maxd,logdiff);}
-		//if(debugg_print){printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);}
-		//
-		if(fabs(logdiff) > 0.2 && iter < 30){
+
+		//Ensure overlap
+		if(next_maxd < mind){		return;}
+		if(next_mind > maxd){		return;}
+		if(next_maxd == next_mind){	return;}
+
+		double overlap = 0;
+		if(next_maxd <= maxd && next_mind >= mind){overlap = next_maxd	-next_mind;}
+		if(next_maxd >= maxd && next_mind <= mind){overlap = maxd		-	  mind;}
+		if(next_maxd <= maxd && next_mind <= mind){overlap = next_maxd	-	  mind;}
+		if(next_maxd >= maxd && next_mind >= mind){overlap =      maxd	-next_mind;}
+
+		double ratio = 2*overlap/(maxd+next_maxd-mind-next_mind);
+		double newlogdiff = log(ratio);
+
+		//if(bidir){
+//			printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);
+//			printf("mind: %5.5f maxd: %5.5f noise: %5.5f noise+reg: %5.5f\n",mind,maxd,noiseval,getNoise());
+//			printf("next_maxd: %5.5f maxd: %5.5f\n",next_maxd,maxd);
+//			printf("next_mind: %5.5f mind: %5.5f\n",next_mind,mind);
+//			printf("logdiff: %5.5f\n",logdiff);
+//			printf("newlogdiff: %5.5f\n",newlogdiff);
+//			printf("ratio: %5.5f = %5.5f / %5.5f\n",ratio,2*overlap,(maxd+next_maxd-mind-next_mind) );
+		//}
+
+
+		//double next_maxd  = meanval2 + (stdval2 + regularization)*target_length;
+
+
+		//if(fabs(logdiff) > 0.2 && iter < 30){
+		if(fabs(newlogdiff) > 0.2 && iter < 30){
 			iter++;
 			computeModel(mat);
 			return;
 		}else{iter = 0;}
 	}
-	if(debugg_print){printf("prob = [");			for(int k = 0; k < 300 && k < histogram_size; k++){printf("%2.2f ",prob[k]);}				printf("];\n");}
-
-	//debugg_print = false;
+//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();
+	//if(bidir){exit(0);}
 }
 
 VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){
 	const unsigned int nr_data = mat.cols();
 	const int nr_dim = mat.rows();
-	const float histogram_mul = float(histogram_size)/maxd;
 
 	nr_inliers = 0;
 	VectorXd weights = VectorXd(nr_data);
@@ -601,14 +923,7 @@ VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){
 		float inl  = 1;
 		float ninl = 1;
 		for(int k = 0; k < nr_dim; k++){
-			double ind = fabs(mat(k,j))*histogram_mul;
-			double w2 = ind-int(ind);
-			double w1 = 1-w2;
-			float p = 0;
-			if(ind >= 0 && (ind+0.5) < histogram_size){
-				if(interp){	p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2;
-				}else{		p = prob[int(ind+0.5)];}
-			}
+			float p = getProb(mat(k,j));
 			inl *= p;
 			ninl *= 1.0-p;
 		}
@@ -623,10 +938,65 @@ VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){
 	return weights;
 }
 
-double DistanceWeightFunction2PPR2::getProb(double d){
-	const float histogram_mul = float(histogram_size)/maxd;
+double DistanceWeightFunction2PPR2::getProb(double d, bool debugg){
+	double ind = getInd(d,debugg);
+	float p = 0;
+	if(interp){
+		double w2 = ind-int(ind);
+		double w1 = 1-w2;
+		if(ind >= 0 && (ind+1) < histogram_size){
+			p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2;
+		}
+	}else{
+		if(ind >= 0 && ind < histogram_size){
+			p = prob[int(ind)];
+		}
+	}
+
+
+	if(debugg){printf("d: %5.5f -> ind: %5.5f -> p: %5.5f histogramsize: %5.5i maxd: %5.5f\n",d,ind,p,histogram_size,maxd);}
+	return p;
+
+/*
+ * 	//const float histogram_mul = float(histogram_size)/maxd;
 	double ind = fabs(d)*histogram_mul;
+	double w2 = ind-int(ind);
+	double w1 = 1-w2;
+	float p = 0;
+	if(ind >= 0 && (ind+0.5) < histogram_size){
+		if(interp){	p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2;
+		}else{		p = prob[int(ind+0.5)];}
+	}
+	return p;
+	*/
+}
+
+
+double DistanceWeightFunction2PPR2::getProbInfront(double d, bool debugg){
+	double ind = getInd(d,debugg);
+	float p;
+	if(ind > meanval){	p = 1;}
+	else{				p = 0;}
+
+	if(interp){
+		double w2 = ind-int(ind);
+		double w1 = 1-w2;
+		if(ind >= 0 && (ind+1) < histogram_size){
+			p = infront[int(ind)]*w1 + infront[int(ind+1)]*w2;
+		}
+	}else{
+		if(ind >= 0 && ind < histogram_size){
+			p = infront[int(ind)];
+		}
+	}
+
+
+	if(debugg){printf("d: %5.5f -> ind: %5.5f -> infront: %5.5f histogramsize: %5.5i maxd: %5.5f\n",d,ind,p,histogram_size,maxd);}
+	return p;
 
+/*
+ * 	//const float histogram_mul = float(histogram_size)/maxd;
+	double ind = fabs(d)*histogram_mul;
 	double w2 = ind-int(ind);
 	double w1 = 1-w2;
 	float p = 0;
@@ -635,6 +1005,7 @@ double DistanceWeightFunction2PPR2::getProb(double d){
 		}else{		p = prob[int(ind+0.5)];}
 	}
 	return p;
+	*/
 }
 
 bool DistanceWeightFunction2PPR2::update(){
@@ -648,10 +1019,12 @@ bool DistanceWeightFunction2PPR2::update(){
 		Gaussian3 g = Gaussian3(mulval,meanval,stdval);//getModel(stdval,blur_histogram,uniform_bias);
 
 		int iteration = 0;
-        for(int i = 0; i < 100; i++){
+        double regularization_before = regularization;
+        for(int i = 0; i < 500; i++){
 			iteration++;
-			regularization *= 0.5;
+            regularization *= 0.5;
 			double change = histogram_size*regularization/maxd;
+            //printf("DistanceWeightFunction2PPR2::update() %i -> reg %f change %f\n",i,regularization,change);
 
 			if(change < 0.01*stdval){return true;}
 
@@ -664,7 +1037,8 @@ bool DistanceWeightFunction2PPR2::update(){
 				new_sum_prob += std::min(maxp , g.getval(k)/hs) * new_histogram[k];
 			}
 
-			if(new_sum_prob < 0.99*old_sum_prob){return false;}
+            //printf("new_sum_prob %f old_sum_prob %f ratio: %f\n",new_sum_prob,old_sum_prob,new_sum_prob/old_sum_prob);
+			if(new_sum_prob < 0.99*old_sum_prob || regularization < regularization_before/double(target_length*5)){return false;}
 			g.stdval -= change;
 			g.update();
 		}
@@ -678,6 +1052,8 @@ void DistanceWeightFunction2PPR2::reset(){
 	nr_inliers = 9999999;
 	regularization	= startreg;
 	maxd			= startmaxd;
+	if(bidir){mind = -maxd;}
+	else{mind = 0;}
 	histogram_size	= starthistogram_size;
 
 	stdval		= maxd/target_length;
@@ -686,6 +1062,7 @@ void DistanceWeightFunction2PPR2::reset(){
 	meanval2	= 0;
 	iter = 0;
 	first = true;
+
 }
 
 std::string DistanceWeightFunction2PPR2::getString(){
@@ -703,6 +1080,19 @@ double DistanceWeightFunction2PPR2::getConvergenceThreshold(){
 		return convergence_threshold;
 	}
 }
+
+inline double DistanceWeightFunction2PPR2::getInd(double d, bool debugg){
+//	if(bidir){
+//		return 0.00001+(d+maxd)*histogram_mul2;
+//	}else{
+//		return 0.00001+fabs(d)*histogram_mul2;
+//	}
+	if(bidir){
+		return 0.00001+(d-mind)*histogram_mul2;
+	}else{
+		return 0.00001+fabs(d)*histogram_mul2;
+	}
+}
 }
 
 
diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp
new file mode 100644
index 00000000..90da4362
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp
@@ -0,0 +1,552 @@
+#include "weightfunctions/DistanceWeightFunction2PPR3.h"
+
+#include <stdio.h>
+#include <execinfo.h>
+#include <signal.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+double getTime3(){
+	struct timeval start1;
+	gettimeofday(&start1, NULL);
+	return double(start1.tv_sec+(start1.tv_usec/1000000.0));
+}
+
+namespace reglib{
+
+DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3(Distribution * dist_,	double maxd_, int histogram_size_){
+	savePath = "";
+	saveData.str("");
+
+	fixed_histogram_size = false;
+
+	regularization		= 0.1;
+	maxd 				= maxd_;
+	mind				= 0;
+	histogram_size		= histogram_size_;
+	starthistogram_size = histogram_size;
+	stdval				= blurval;
+
+	maxnoise = 99999999999999;
+	noiseval = 100.0;
+
+	prob.resize(histogram_size+1);
+	prob[histogram_size] = 0;
+
+	infront.resize(histogram_size+1);
+	infront[histogram_size] = 1;
+
+	histogram.resize(histogram_size+1);
+	blur_histogram.resize(histogram_size+1);
+	noise.resize(histogram_size+1);
+	noisecdf.resize(histogram_size+1);
+
+	mulval = 1;
+	meanval = 0;
+
+	startmaxd = maxd_;
+
+
+	scale_convergence = true;
+	nr_inliers = 1;
+
+	target_length = 10.0;
+
+	bidir = false;
+	iter = 0;
+
+	maxp = 0.99;
+
+	first = true;
+
+
+	startreg			= 2;
+	blurval				= 4;
+	stdval				= 100;
+	noiseval			= 100.0;
+	debugg_print		= true;
+	threshold			= false;
+	blur				= 0.03;
+	data_per_bin		= 30;
+
+
+	update_size			= true;//(setting&2 != 1);
+	max_under_mean		= true;//(setting&1 != 0);
+	interp				= true;//(setting&1 != 0);
+
+	nr_refineiters		= 1;
+	convergence_threshold = 0.05;
+	compute_infront = false;
+
+	sp      = new SignalProcessing();
+	dist    = dist_;//new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution();
+}
+
+DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3(	double maxd_, int histogram_size_){
+
+	fixed_histogram_size = false;
+
+	regularization		= 0.1;
+	maxd 				= maxd_;
+	mind				= 0;
+	histogram_size		= histogram_size_;
+	starthistogram_size = histogram_size;
+	stdval				= blurval;
+
+	maxnoise = 99999999999999;
+	noiseval = 100.0;
+
+	prob.resize(histogram_size+1);
+	prob[histogram_size] = 0;
+
+	infront.resize(histogram_size+1);
+	infront[histogram_size] = 1;
+	
+	histogram.resize(histogram_size+1);
+	blur_histogram.resize(histogram_size+1);
+	noise.resize(histogram_size+1);
+	noisecdf.resize(histogram_size+1);
+
+	mulval = 1;
+	meanval = 0;
+
+	startmaxd = maxd_;
+
+
+	scale_convergence = true;
+	nr_inliers = 1;
+
+	target_length = 10.0;
+
+	bidir = false;
+	iter = 0;
+
+	maxp = 0.99;
+
+	first = true;
+
+
+	startreg			= 2;
+	blurval				= 4;
+	stdval				= 100;
+	noiseval			= 100.0;
+	debugg_print		= true;
+	threshold			= false;
+	blur				= 0.03;
+	data_per_bin		= 30;
+
+
+	update_size			= true;//(setting&2 != 1);
+	max_under_mean		= true;//(setting&1 != 0);
+	interp				= true;//(setting&1 != 0);
+
+	nr_refineiters		= 1;
+	convergence_threshold = 0.05;
+	compute_infront = false;
+
+	sp      = new SignalProcessing();
+	dist    = new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution();
+}
+
+DistanceWeightFunction2PPR3::~DistanceWeightFunction2PPR3(){
+	if(sp != 0){delete sp;}
+	if(dist != 0){delete dist;}
+}
+
+double DistanceWeightFunction2PPR3::getNoise(){return regularization+noiseval;}
+
+void DistanceWeightFunction2PPR3::recomputeHistogram(std::vector<float> & hist, MatrixXd & mat){
+	const unsigned int nr_data = mat.cols();
+	const int nr_dim = mat.rows();
+	histogram_mul2	= double(histogram_size)/(maxd-mind);
+	for(int j = 0; j < histogram_size; j++){hist[j] = 0;}
+	for(unsigned int j = 0; j < nr_data; j++){
+		for(int k = 0; k < nr_dim; k++){
+			double ind = getInd(mat(k,j));
+			if(ind >= 0 && (ind+0.00001) < histogram_size){
+				hist[int(ind+0.00001)]++;
+			}
+		}
+	}
+}
+
+void DistanceWeightFunction2PPR3::recomputeProbs(){
+	for(int k = 0; k < histogram_size; k++){noise[k] = dist->getval(k);}
+	if(compute_infront){
+		for(int k = 0; k < histogram_size; k++){noisecdf[k] = dist->getcdf(k);}
+	}
+
+	double maxhist = blur_histogram[0];
+	for(int k = 1; k < histogram_size; k++){maxhist = std::max(maxhist,double(blur_histogram[k]));}
+	double minhist = maxhist*0.01;
+
+	for(int k = 0; k < histogram_size; k++){
+		if(max_under_mean && k < dist->mean){	prob[k] = maxp;	}
+		else{
+			double hs = std::max(minhist,std::max(1.0,double(blur_histogram[k])));
+			prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data
+		}
+        infront[k] = (1-prob[k])*noisecdf[k];
+	}
+
+    if(false && debugg_print){
+        for(int k = 0; k < histogram_size; k+=10){
+            printf("%i -> hist: %f prob[k]: %f noisecdf[k]: %f infront[k]: %f\n",k,double(blur_histogram[k]),prob[k],noisecdf[k],infront[k]);
+        }
+    }
+    //exit(0);
+}
+
+void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){
+	const unsigned int nr_data = mat.cols();
+	const int nr_dim = mat.rows();
+	double start_time;
+
+	if(debugg_print){printf("\n%%################################################### ITER:%i ############################################################\n",iter);}
+
+	if(	savePath.size() != 0 && iter == 0){	saveData.str("");}
+
+	if(	savePath.size() != 0){
+		saveData << "\n%################################################### ITER:" << iter << " ############################################################\n";
+	}
+
+
+
+	if(!fixed_histogram_size){
+		if(first){
+
+			double sum = 0;
+			for(unsigned int j = 0; j < nr_data; j++){
+				for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);}
+			}
+			double stdval = sqrt(sum / double(nr_data*nr_dim));
+			if(debugg_print){printf("%%stdval: %f\n",stdval);}
+            histogram_mul2	= double(histogram_size)/(maxd-mind);
+            dist->setNoise(stdval*histogram_mul2);
+		}
+
+		if(update_size){
+
+            if(debugg_print){printf("%%update_size\n");dist->print();}
+			double next_maxd;
+			double next_mind;
+			start_time = getTime();
+            dist->getMaxdMind(next_maxd,next_mind,0.0000001);
+//			next_maxd *= (maxd-mind)/double(histogram_size);
+//			next_mind *= (maxd-mind)/double(histogram_size);
+            if(first){
+                next_maxd *= (maxd-mind)/double(histogram_size);
+                next_mind = -next_maxd;
+            }else {
+                next_maxd = getDfromInd(next_maxd);
+                next_mind = getDfromInd(next_mind);
+            }
+			if(debugg_print){printf("%%getMaxdMind time: %5.5fs\n",getTime()-start_time);}
+			if(!bidir){next_mind = 0;}
+			mind = next_mind;
+			maxd = next_maxd;
+
+			start_time = getTime();
+			double nr_inside = 0;
+			for(unsigned int j = 0; j < nr_data; j++){
+				for(int k = 0; k < nr_dim; k++){
+					double d = mat(k,j);
+					if(mind < d && d < maxd){nr_inside++;}
+				}
+			}
+			if(debugg_print){printf("%%nr_inside time: %5.5fs\n",getTime()-start_time);}
+
+			int new_histogram_size = std::min(int(histogram.size()),std::max(50,int(0.5 + nr_inside/data_per_bin)));
+			histogram_size = std::min(1000,new_histogram_size);
+			blurval = blur*double(histogram_size)*float(histogram_size)/float(new_histogram_size);
+			if(debugg_print){printf("%%nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);}
+		}
+        first = false;
+	}
+
+	if(debugg_print){printf("%%histogram_size: %5.5f maxd: %5.5f mind: %5.5f\n",double(histogram_size),maxd,mind);}
+	start_time = getTime();
+	recomputeHistogram(histogram,mat);
+	if(debugg_print){printf("%%computing histogram time: %5.5fs\n",getTime()-start_time);}
+
+	start_time = getTime();
+	sp->process(histogram,blur_histogram,blurval,histogram_size);
+	if(debugg_print){printf("%%SignalProcessing time: %5.5fs\n",getTime()-start_time);}
+
+	start_time = getTime();
+	dist->setRegularization(double(histogram_size)*regularization/(maxd-mind));
+	dist->train(blur_histogram,histogram_size);
+	dist->update();
+	if(debugg_print){printf("%%train time: %5.5fs\n",getTime()-start_time);dist->print();}
+
+	start_time = getTime();
+	recomputeProbs();
+	if(debugg_print){printf("%%recomputeProbs time: %5.5fs\n",getTime()-start_time);}
+
+	start_time = getTime();
+	getProbs(mat);
+	if(debugg_print){printf("%%get number of inliers time: %5.5fs nr_inliers: %f\n",getTime()-start_time,nr_inliers);}
+
+	if(debugg_print){printf("hist = [");			for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ",	int(histogram[k]));}		printf("];\n");}
+	if(debugg_print){printf("hist_smooth = [");		for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ",	int(blur_histogram[k]));}	printf("];\n");}
+	if(debugg_print){printf("noise = [");			for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ",	int(noise[k]));}			printf("];\n");}
+	if(debugg_print){printf("noisecdf = [");		for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));}			printf("];\n");}
+	if(debugg_print){printf("prob = [");			for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);}					printf("];\n");}
+	if(debugg_print){printf("infront = [");			for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);}				printf("];\n");}
+    if(debugg_print){printf("figure(%i);",iter+1);}
+    if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");}
+
+	if(	savePath.size() != 0){
+		saveData << "d = [";
+		for(int k = 0; k < histogram_size; k++){saveData << (maxd-mind)*double(k)/double(histogram_size-1)+mind << " ";}
+		saveData << "];\n";
+
+		saveData << "hist = [";
+		for(int k = 0; k < histogram_size; k++){saveData << histogram[k] << " ";}
+		saveData << "];\n";
+		saveData << "hist_smooth = [";
+		for(int k = 0; k < histogram_size; k++){saveData << blur_histogram[k] << " ";}
+		saveData << "];\n";
+		saveData << "noise = [";
+		for(int k = 0; k < histogram_size; k++){saveData << noise[k] << " ";}
+		saveData << "];\n";
+		saveData << "noisecdf = [";
+		for(int k = 0; k < histogram_size; k++){saveData << noisecdf[k] << " ";}
+		saveData << "];\n";
+		saveData << "prob = [";
+		for(int k = 0; k < histogram_size; k++){saveData << prob[k] << " ";}
+		saveData << "];\n";
+		saveData << "infront = [";
+		for(int k = 0; k < histogram_size; k++){saveData << infront[k] << " ";}
+		saveData << "];\n";
+		saveData << "figure(" << iter+1 <<");\n";
+		saveData << "clf; hold on;\n";
+		saveData << "title('Distribution of residuals and corresponding estimates')\n";
+		saveData << "xlabel('residual values')\n";
+		saveData << "ylabel('number of data')\n";
+
+		saveData << "plot(d,hist_smooth,'r','LineWidth',2);\n";
+		saveData << "plot(d,hist,'m','LineWidth',2);\n";
+		saveData << "plot(d,noise,'b','LineWidth',2);\n";
+		saveData << "plot(prob*max(noise),'g','LineWidth',2);\n";
+		if(compute_infront){saveData << "plot(d,infront*max(noise),'c','LineWidth',2);\n";}
+		saveData << "legend('histogram smoothed','histogram raw','noise estimate','P(Inlier)'";
+		if(compute_infront){saveData << ",'P(Infront)'";}
+		saveData << ");\n";
+	}
+
+	if(!fixed_histogram_size && update_size ){
+
+		double next_maxd;
+		double next_mind;
+		start_time = getTime();
+
+        dist->getMaxdMind(next_maxd,next_mind,0.0000001);
+
+		if(debugg_print){
+			printf("%%getMaxdMind time: %5.5fs\n",getTime()-start_time);
+			printf("%%current: %f %f\n",mind,maxd);
+			printf("%%next: %f %f\n",next_mind,next_maxd);
+		}
+
+        next_maxd = getDfromInd(next_maxd);
+        next_mind = getDfromInd(next_mind);
+        //next_maxd *= (maxd-mind)/double(histogram_size);
+        //next_mind *= (maxd-mind)/double(histogram_size);
+
+        if(debugg_print){
+            printf("%%next: %f %f\n",next_mind,next_maxd);
+        }
+
+		if(!bidir){next_mind = 0;}
+
+		//Ensure overlap
+		if(next_maxd < mind){		return;}
+		if(next_mind > maxd){		return;}
+		if(next_maxd == next_mind){	return;}
+
+		if(debugg_print){printf("%%next: %f %f\n",next_mind,next_maxd);}
+		double overlap = 0;
+		if(next_maxd <= maxd && next_mind >= mind){     overlap = next_maxd-next_mind;}
+		else if(next_maxd >= maxd && next_mind <= mind){overlap =      maxd-	 mind;}
+		else if(next_maxd <= maxd && next_mind <= mind){overlap = next_maxd-	 mind;}
+		else if(next_maxd >= maxd && next_mind >= mind){overlap =      maxd-next_mind;}
+
+		double ratio = 2*overlap/(maxd+next_maxd-mind-next_mind);
+		double newlogdiff = log(ratio);
+		if(debugg_print){printf("%%overlap: %5.5f ratio: %5.5f newlogdiff: %5.5f\n",overlap,ratio,newlogdiff);}
+
+        if(fabs(newlogdiff) > 0.05 && iter < 5){
+			iter++;
+			computeModel(mat);
+			return;
+		}else{
+			iter = 0;
+			if(	savePath.size() != 0){
+				std::ofstream myfile;
+				myfile.open (savePath);
+				myfile << saveData.str();
+				myfile.close();
+			}
+		}
+	}else{
+		if(	savePath.size() != 0){
+			std::ofstream myfile;
+			myfile.open (savePath);
+			myfile << saveData.str();
+			myfile.close();
+		}
+	}
+}
+
+VectorXd DistanceWeightFunction2PPR3::getProbs(MatrixXd mat){
+	const unsigned int nr_data = mat.cols();
+	const int nr_dim = mat.rows();
+
+	nr_inliers = 0;
+	VectorXd weights = VectorXd(nr_data);
+	for(unsigned int j = 0; j < nr_data; j++){
+		float inl  = 1;
+		float ninl = 1;
+		for(int k = 0; k < nr_dim; k++){
+			float p = getProb(mat(k,j));
+			inl *= p;
+			ninl *= 1.0-p;
+		}
+		double d = inl / (inl+ninl);
+		nr_inliers += d;
+		weights(j) = d;
+	}
+
+	if(threshold){
+		for(unsigned int j = 0; j < nr_data; j++){weights(j) = weights(j) > 0.5;}
+	}
+	return weights;
+}
+
+double DistanceWeightFunction2PPR3::getProb(double d, bool debugg){
+	double ind = getInd(d,debugg);
+	float p = 0;
+	if(interp){
+		double w2 = ind-int(ind);
+		double w1 = 1-w2;
+		if(ind >= 0 && (ind+1) < histogram_size){
+			p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2;
+		}
+	}else{
+		if(ind >= 0 && ind < histogram_size){
+			p = prob[int(ind)];
+		}
+	}
+
+
+	if(debugg){printf("d: %5.5f -> ind: %5.5f -> p: %5.5f histogramsize: %5.5i maxd: %5.5f\n",d,ind,p,histogram_size,maxd);}
+	return p;
+}
+
+
+double DistanceWeightFunction2PPR3::getProbInfront(double d, bool debugg){
+	double ind = getInd(d,debugg);
+	float p;
+	if(ind > meanval){	p = 1;}
+	else{				p = 0;}
+
+	if(interp){
+		double w2 = ind-int(ind);
+		double w1 = 1-w2;
+		if(ind >= 0 && (ind+1) < histogram_size){
+			p = infront[int(ind)]*w1 + infront[int(ind+1)]*w2;
+		}
+	}else{
+		if(ind >= 0 && ind < histogram_size){
+			p = infront[int(ind)];
+		}
+	}
+
+	if(debugg){printf("d: %5.5f -> ind: %5.5f -> infront: %5.5f histogramsize: %5.5i maxd: %5.5f\n",d,ind,p,histogram_size,maxd);}
+	return p;
+}
+
+bool DistanceWeightFunction2PPR3::update(){
+	double start_time;
+
+	double old_sum_prob = 0;
+	for(int k = 0; k < histogram_size; k++){old_sum_prob += prob[k] * histogram[k];}
+
+	int iteration = 0;
+	double regularization_before = regularization;
+	for(int i = 0; i < 500; i++){
+		iteration++;
+		regularization *= 0.5;
+
+		start_time = getTime();
+		double before_noise = dist->getNoise();
+		dist->setRegularization(double(histogram_size)*regularization/(maxd-mind));
+		dist->update();
+		double after_noise = dist->getNoise();
+		if(debugg_print){printf("%%train time: %5.5fs\n",getTime()-start_time);dist->print();}
+
+		start_time = getTime();
+		recomputeProbs();
+		if(debugg_print){printf("%%recomputeProbs time: %5.5fs\n",getTime()-start_time);}
+
+		if(0.99*before_noise < after_noise){return true;}
+
+		double new_sum_prob = 0;
+		for(int k = 0; k < histogram_size; k++){new_sum_prob += prob[k] * histogram[k];}
+
+		//printf("new_sum_prob %f old_sum_prob %f ratio: %f\n",new_sum_prob,old_sum_prob,new_sum_prob/old_sum_prob);
+		if(new_sum_prob < 0.99*old_sum_prob || regularization*10 < regularization_before){return false;}
+	}
+	return true;
+}
+
+void DistanceWeightFunction2PPR3::reset(){
+	nr_inliers = 9999999;
+	regularization	= startreg;
+	maxd			= startmaxd;
+	if(bidir){mind = -maxd;}
+	else{mind = 0;}
+	histogram_size	= starthistogram_size;
+
+	stdval		= maxd/target_length;
+	stdval2		= maxd/target_length;
+	meanval		= 0;
+	meanval2	= 0;
+	iter = 0;
+	first = true;
+
+}
+
+std::string DistanceWeightFunction2PPR3::getString(){
+	char buf [1024];
+	sprintf(buf,"PPR_%i_%i_%i_%i",int(1000.0*startreg),int(1000.0*blur),interp,int(data_per_bin));
+	return std::string(buf);
+}
+
+double DistanceWeightFunction2PPR3::getConvergenceThreshold(){
+	if(scale_convergence){
+		return convergence_threshold*getNoise()/sqrt(nr_inliers);
+	}else{
+		return convergence_threshold;
+	}
+}
+
+inline double DistanceWeightFunction2PPR3::getInd(double d, bool debugg){
+    if(bidir){
+        return 0.00001+(d-mind)*histogram_mul2;
+    }else{
+        return 0.00001+fabs(d)*histogram_mul2;
+    }
+}
+
+inline double DistanceWeightFunction2PPR3::getDfromInd(double ind, bool debugg){
+    if(bidir){
+        return ind/histogram_mul2+mind;
+    }else{
+        return ind/histogram_mul2;
+    }
+}
+}
+
+
diff --git a/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp
new file mode 100644
index 00000000..26be398b
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp
@@ -0,0 +1,54 @@
+#include "weightfunctions/Distribution.h"
+
+namespace reglib
+{
+Distribution::Distribution(){
+    debugg_print = false;
+}
+Distribution::~Distribution(){}
+void	Distribution::reset(){printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__);}
+void    Distribution::train(std::vector<float> & hist, unsigned int nr_bins){   printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);}
+void    Distribution::update(){                                                 printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);}
+double  Distribution::getval(double x){                                         printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);}
+double  Distribution::getcdf(double x){                                         printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);}
+void    Distribution::print(){                                                  printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);}
+void    Distribution::setRegularization(double x){regularization = x;update();}
+double  Distribution::getNoise(){return 1;}
+void    Distribution::setNoise(double x){                                       printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);}
+void    Distribution::getMaxdMind(double & maxd, double & mind, double prob){
+    if(debugg_print){printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__);}
+    double half = prob*0.5;
+    double minDist = 0;
+    double minDistScore = getcdf(mean+minDist);
+    double maxDist = 1;
+    double maxDistScore = getcdf(mean+maxDist);
+
+    double target = 1-half;
+    //Grow to find interval
+    while(maxDistScore < target){
+        minDist = maxDist;
+        minDistScore = maxDistScore;
+        maxDist *= 2;
+        maxDistScore = getcdf(mean+maxDist);
+    }
+
+    //bisect to opt
+    for(unsigned int it = 0; it < 40; it++){
+        double midDist = (minDist+maxDist)*0.5;
+        double midDistScore = getcdf(mean+midDist);
+        if(midDistScore > target){
+            maxDist = midDist;
+            maxDistScore = midDistScore;
+        }else{
+            minDist = midDist;
+            minDistScore = midDistScore;
+        }
+        if((maxDistScore-minDistScore) < 1e-15){break;}
+    }
+
+    double midDist = (minDist+maxDist)*0.5;
+    mind = mean-midDist;
+    maxd = mean+midDist;
+}
+
+}
diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp
new file mode 100644
index 00000000..40e99f91
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp
@@ -0,0 +1,164 @@
+#include "weightfunctions/GaussianDistribution.h"
+
+namespace reglib
+{
+
+
+GaussianDistribution::GaussianDistribution(bool refine_std_, bool zeromean_, bool refine_mean_, bool refine_mul_, double costpen_, int nr_refineiters_ ,double mul_, double mean_,double stdval_){
+    refine_std  = refine_std_;
+    zeromean    = zeromean_;
+    refine_mean = refine_mean_;
+    refine_mul  = refine_mul_;
+    costpen     = costpen_;
+    nr_refineiters = nr_refineiters_;
+
+    mul         = mul_;
+    mean        = mean_;
+    stdval      = stdval_;
+    update();
+    debugg_print = false;
+}
+//GaussianDistribution::GaussianDistribution(double mul_, double mean_,	double stdval_){
+//    mul = mul_;
+//    mean = mean_;
+//    stdval = stdval_;
+//    update();
+//}
+GaussianDistribution::~GaussianDistribution(){}
+
+double GaussianDistribution::scoreCurrent2(double mul, double mean, double stddiv, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    double info = -0.5/(stddiv*stddiv);
+    double sum = 0;
+    for(unsigned int i = 0; i < nr_data; i++){
+        double dx = X[i] - mean;
+        double inp = info*dx*dx;
+        if(inp < cutoff_exp){sum += Y[i];}
+        else{
+            double diff = mul*exp(info*dx*dx) - Y[i];
+            if(diff > 0){	sum += costpen*diff;}
+            else{			sum -= diff;}
+        }
+    }
+    return sum;
+}
+
+double GaussianDistribution::fitStdval2(double mul, double mean, double std_mid, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    int iter = 25;
+    double h = 0.000000001;
+
+    double std_max = std_mid*2;
+    double std_min = 0;
+    for(int i = 0; i < iter; i++){
+        std_mid = (std_max+std_min)/2;
+        double std_neg = scoreCurrent2(mul,mean,std_mid-h,X,Y,nr_data,costpen);
+        double std_pos = scoreCurrent2(mul,mean,std_mid+h,X,Y,nr_data,costpen);
+        if(std_neg < std_pos){	std_max = std_mid;}
+        else{					std_min = std_mid;}
+    }
+    return std_mid;
+}
+
+double GaussianDistribution::fitMean2(double mul, double mean, double std_mid, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    int iter = 10;
+    double h = 0.000000001;
+    double mean_max = mean+1;
+    double mean_min = mean-1;
+
+    //	double mean_max = mean*2;
+    //	double mean_min = 0;
+
+    for(int i = 0; i < iter; i++){
+        mean = (mean_max+mean_min)/2;
+        double std_neg = scoreCurrent2(mul,mean-h,std_mid,X,Y,nr_data,costpen);
+        double std_pos = scoreCurrent2(mul,mean+h,std_mid,X,Y,nr_data,costpen);
+        if(std_neg < std_pos){	mean_max = mean;}
+        else{					mean_min = mean;}
+    }
+    return mean;
+}
+
+double GaussianDistribution::fitMul2(double mul, double mean, double std_mid, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    int iter = 25;
+    double h = 0.000000001;
+    double mul_max = mul*2;
+    double mul_min = 0;
+
+    for(int i = 0; i < iter; i++){
+        mul = (mul_max+mul_min)/2;
+        double std_neg = scoreCurrent2(mul-h,mean,std_mid,X,Y,nr_data,costpen);
+        double std_pos = scoreCurrent2(mul+h,mean,std_mid,X,Y,nr_data,costpen);
+        if(std_neg < std_pos){	mul_max = mul;}
+        else{					mul_min = mul;}
+    }
+    return mul;
+}
+
+void GaussianDistribution::train(std::vector<float> & hist, unsigned int nr_bins){
+    if(nr_bins == 0){nr_bins = hist.size();}
+    mul = hist[0];
+    mean = 0;
+    if(!zeromean){
+        for(unsigned int k = 1; k < nr_bins; k++){
+            if(hist[k] > mul){
+                mul = hist[k];
+                mean = k;
+            }
+        }
+    }
+
+    std::vector<float> X;
+    std::vector<float> Y;
+    for(unsigned int k = 0; k < nr_bins; k++){
+        if(hist[k]  > mul*0.01){
+            X.push_back(k);
+            Y.push_back(hist[k]);
+        }
+    }
+
+    unsigned int nr_data_opt = X.size();
+
+
+    double ysum = 0;
+    for(unsigned int i = 0; i < nr_data_opt; i++){ysum += fabs(Y[i]);}
+
+    double std_mid = 0;
+    for(unsigned int i = 0; i < nr_data_opt; i++){std_mid += (X[i]-mean)*(X[i]-mean)*fabs(Y[i])/ysum;}
+    stdval = sqrt(std_mid);
+
+    for(int i = 0; i < nr_refineiters; i++){
+        if(refine_std){		stdval	= fitStdval2(	mul,mean,stdval,X,Y,nr_data_opt,costpen);}
+        if(refine_mean){	mean	= fitMean2(		mul,mean,stdval,X,Y,nr_data_opt,costpen);}
+        if(refine_mul){		mul		= fitMul2(		mul,mean,stdval,X,Y,nr_data_opt,costpen);}
+    }
+}
+
+void GaussianDistribution::update(){
+    scaledinformation = -0.5/((stdval+regularization)*(stdval+regularization));
+}
+
+double GaussianDistribution::getval(double x){
+    double dx = mean-x;
+    return mul*exp(dx*dx*scaledinformation);
+}
+double GaussianDistribution::getcdf(double x){
+    return 0.5 * erfc(-((x-mean)/(stdval+regularization)) * M_SQRT1_2);
+}
+
+void GaussianDistribution::setNoise(double x){
+    stdval = x;
+    update();
+}
+
+void GaussianDistribution::print(){
+    printf("GaussianDistribution:: mul = %5.5f mean = %5.5f stdval = %5.5f reg = %5.5f\n",mul,mean,stdval,regularization);
+}
+
+double GaussianDistribution::getNoise(){return stdval+regularization;}
+
+//void GaussianDistribution::getMaxdMind(double & maxd, double & mind, double prob){
+
+//}
+
+
+}
+
diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp
new file mode 100644
index 00000000..82a7ffe1
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp
@@ -0,0 +1,208 @@
+#include "weightfunctions/GeneralizedGaussianDistribution.h"
+#include <boost/math/special_functions/gamma.hpp>
+
+namespace reglib
+{
+
+GeneralizedGaussianDistribution::GeneralizedGaussianDistribution(bool refine_std_, bool refine_power_, bool zeromean_, bool refine_mean_, bool refine_mul_, double costpen_, int nr_refineiters_ ,double mul_, double mean_,double stdval_,double power_){
+    refine_std  = refine_std_;
+    zeromean    = zeromean_;
+    refine_mean = refine_mean_;
+    refine_mul  = refine_mul_;
+    refine_power = refine_power_;
+    costpen     = costpen_;
+    nr_refineiters = nr_refineiters_;
+
+    mul         = mul_;
+    mean        = mean_;
+    stdval      = stdval_;
+    power       = power_;
+    update();
+    debugg_print = false;
+	regularization = 0;
+}
+
+GeneralizedGaussianDistribution::~GeneralizedGaussianDistribution(){
+	numcdf_vec.clear();
+}
+
+double GeneralizedGaussianDistribution::fitMul3(double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    int iter = 25;
+    double h = 0.000000001;
+    double mul_max = mul*2;
+    double mul_min = 0;
+
+    for(int i = 0; i < iter; i++){
+        mul = (mul_max+mul_min)/2;
+        double std_neg = scoreCurrent3(mul-h,mean,std_mid,power,X,Y,nr_data,costpen);
+        double std_pos = scoreCurrent3(mul+h,mean,std_mid,power,X,Y,nr_data,costpen);
+        if(std_neg < std_pos){	mul_max = mul;}
+        else{					mul_min = mul;}
+    }
+    return mul;
+}
+
+
+double GeneralizedGaussianDistribution::scoreCurrent3(double mul, double mean, double stddiv, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    double sum = 0;
+    double invstd = 1.0/stddiv;
+    for(unsigned int i = 0; i < nr_data; i++){
+        double dx = fabs(X[i] - mean)*invstd;
+        double inp = -0.5*pow(dx,power);
+        if(inp < cutoff_exp){sum += Y[i];}
+        else{
+            double diff = Y[i]*Y[i]*(mul*exp(inp) - Y[i]);
+            if(diff > 0){	sum += costpen*diff;}
+            else{			sum -= diff;}
+        }
+    }
+    return sum;
+}
+
+double GeneralizedGaussianDistribution::fitStdval3(double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    int iter = 25;
+    double h = 0.000000001;
+
+    double std_max = std_mid*2;
+    double std_min = 0;
+    for(int i = 0; i < iter; i++){
+        std_mid = (std_max+std_min)/2;
+        double std_neg = scoreCurrent3(mul,mean,std_mid-h,power,X,Y,nr_data,costpen);
+        double std_pos = scoreCurrent3(mul,mean,std_mid+h,power,X,Y,nr_data,costpen);
+        if(std_neg < std_pos){	std_max = std_mid;}
+        else{					std_min = std_mid;}
+    }
+    return std_mid;
+}
+
+double GeneralizedGaussianDistribution::fitPower3(double current_mul, double current_mean, double current_std, double current_power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    int iter = 20;
+    double h = 0.000000001;
+
+    double power_max = current_power*2;
+    double power_min = 0.001;
+    for(int i = 0; i < iter; i++){
+        current_power = (power_max+power_min)/2;
+        double std_neg = scoreCurrent3(current_mul,current_mean,current_std,current_power-h,X,Y,nr_data,costpen);
+        double std_pos = scoreCurrent3(current_mul,current_mean,current_std,current_power+h,X,Y,nr_data,costpen);
+        if(std_neg < std_pos){	power_max = current_power;}
+        else{					power_min = current_power;}
+    }
+    current_power = (power_max+power_min)/2;
+	return current_power;
+}
+
+double GeneralizedGaussianDistribution::fitMean3(double mul, double mean, double std_mid, double power, std::vector<float> & X, std::vector<float> & Y, unsigned int nr_data, double costpen){
+    int iter = 10;
+    double h = 0.000000001;
+    double mean_max = mean+10;
+    double mean_min = mean-10;
+
+    for(int i = 0; i < iter; i++){
+        mean = (mean_max+mean_min)/2;
+        double std_neg = scoreCurrent3(mul,mean-h,std_mid,power,X,Y,nr_data,costpen);
+        double std_pos = scoreCurrent3(mul,mean+h,std_mid,power,X,Y,nr_data,costpen);
+        if(std_neg < std_pos){	mean_max = mean;}
+        else{					mean_min = mean;}
+    }
+    return mean;
+}
+
+void GeneralizedGaussianDistribution::train(std::vector<float> & hist, unsigned int nr_bins){
+    if(nr_bins == 0){nr_bins = hist.size();}
+    mul = hist[0];
+    mean = 0;
+    if(!zeromean){
+        for(unsigned int k = 1; k < nr_bins; k++){
+            if(hist[k] > mul){
+                mul = hist[k];
+                mean = k;
+            }
+        }
+    }
+
+    std::vector<float> X;
+    std::vector<float> Y;
+    for(unsigned int k = 0; k < nr_bins; k++){
+        if(hist[k]  > mul*0.001){
+            X.push_back(k);
+            Y.push_back(hist[k]);
+        }
+    }
+
+    unsigned int nr_data_opt = X.size();
+
+
+    double ysum = 0;
+    for(unsigned int i = 0; i < nr_data_opt; i++){ysum += fabs(Y[i]);}
+
+    double std_mid = 0;
+    for(unsigned int i = 0; i < nr_data_opt; i++){std_mid += (X[i]-mean)*(X[i]-mean)*fabs(Y[i])/ysum;}
+    stdval = sqrt(std_mid);
+
+    for(int i = 0; i < nr_refineiters; i++){
+        if(refine_std){		stdval	= fitStdval3(	mul,mean,stdval,power,X,Y,nr_data_opt,costpen);}
+        if(refine_mean){	mean	= fitMean3(		mul,mean,stdval,power,X,Y,nr_data_opt,costpen);}
+        if(refine_mul){		mul		= fitMul3(		mul,mean,stdval,power,X,Y,nr_data_opt,costpen);}
+        if(refine_power){	power	= fitPower3(	mul,mean,stdval,power,X,Y,nr_data_opt,costpen);}
+    }
+
+	//print();
+}
+
+void GeneralizedGaussianDistribution::update(){
+	//printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__);
+    update_numcdf_vec();
+}
+
+double GeneralizedGaussianDistribution::getInp(double x){
+	return -0.5*pow(fabs(mean-x)/(stdval+regularization),power);
+}
+
+double GeneralizedGaussianDistribution::getval(double x){
+    return mul*exp(getInp(x));
+}
+double GeneralizedGaussianDistribution::getcdf(double x){
+
+    double part = (x-start)/(stop-start);
+    if(part < 0){return 0;}
+   // if(part >= 1){return 1;}
+    part *= double(numcdf_vec.size()-1);
+    unsigned int id0 = part;
+    unsigned int id1 = id0+1;
+	if(id1 >= numcdf_vec.size()){return 1;}
+    double w0 = double(id1)-part;
+    double w1 = part-double(id0);
+    double cdfx = numcdf_vec[id0]*w0 + numcdf_vec[id1]*w1;
+    return cdfx;
+}
+
+void GeneralizedGaussianDistribution::setNoise(double x){
+    stdval = x;
+    update();
+}
+
+void GeneralizedGaussianDistribution::print(){
+    printf("%%GeneralizedGaussianDistribution:: mul = %5.5f mean = %5.5f stdval = %5.5f reg = %5.5f power = %5.5f\n",mul,mean,stdval,regularization,power);
+}
+
+double GeneralizedGaussianDistribution::getNoise(){return stdval+regularization;}
+
+void GeneralizedGaussianDistribution::update_numcdf_vec(unsigned int bins, double prob){
+	start = mean - pow(-2.0*log(prob),1.0/power)*stdval;
+	stop  = mean + pow(-2.0*log(prob),1.0/power)*stdval;
+
+    double step     = (stop-start)/(bins-1);
+    double sum      = 0;
+    numcdf_vec.resize(bins);
+    for(unsigned int i = 0; i < bins; i++){
+        double x = start+double(i)*step+0.5*step;
+        numcdf_vec[i] = sum;
+		if(i < bins-1){sum += getval(x);}
+    }
+    for(unsigned int i = 0; i < bins; i++){numcdf_vec[i] /= sum;}
+}
+
+
+}
+
diff --git a/quasimodo/quasimodo_models/src/weightfunctions/SignalProcessing.cpp b/quasimodo/quasimodo_models/src/weightfunctions/SignalProcessing.cpp
new file mode 100644
index 00000000..766d216f
--- /dev/null
+++ b/quasimodo/quasimodo_models/src/weightfunctions/SignalProcessing.cpp
@@ -0,0 +1,59 @@
+#include "weightfunctions/SignalProcessing.h"
+
+namespace reglib
+{
+
+SignalProcessing::SignalProcessing(){}
+SignalProcessing::~SignalProcessing(){}
+void SignalProcessing::process(std::vector<float> & in, std::vector<float> & out, double stdval, unsigned int nr_bins){
+    if(nr_bins == 0){nr_bins = in.size();}
+
+
+    int nr_bins2 = 2*nr_bins;
+
+    std::vector<float> tmphist;
+    tmphist.resize(nr_bins2);
+
+    std::vector<float> tmphist_blur;
+    tmphist_blur.resize(nr_bins2);
+
+    for(int i = 0; i < nr_bins; i++){
+        tmphist[i]			= in[nr_bins-1-i];
+        tmphist[nr_bins+i]	= in[i];
+        tmphist_blur[i]			= 0;
+        tmphist_blur[nr_bins+i]	= 0;
+    }
+
+    double info = -0.5/(stdval*stdval);
+    double weights[nr_bins2];
+    for(int i = 0; i < nr_bins2; i++){weights[i] = 0;}
+    for(int i = 0; i < nr_bins2; i++){
+        double current = exp(i*i*info);
+        weights[i] = current;
+        if(current < 0.001){break;}
+    }
+
+    int offset = 4.0*stdval;
+    offset = std::max(4,offset);
+    for(int i = 0; i < nr_bins2; i++){
+        double v = tmphist[i];
+        if(v == 0){continue;}
+
+        int start	= std::max(0,i-offset);
+        int stop	= std::min(nr_bins2,i+offset+1);
+
+        for(int j = start; j < stop; j++){tmphist_blur[j] += v*weights[abs(i-j)];}
+    }
+    for(int i = 0; i < nr_bins; i++){out[i] = tmphist_blur[i+nr_bins];}
+
+    float bef = 0;
+    float aft = 0;
+    for(int i = 0; i < nr_bins; i++){
+        bef += in[i];
+        aft += out[i];
+    }
+
+    for(int i = 0; i < nr_bins; i++){out[i] *= bef/aft;}
+}
+
+}
diff --git a/quasimodo/quasimodo_msgs/CMakeLists.txt b/quasimodo/quasimodo_msgs/CMakeLists.txt
index fc559d4e..eef27298 100644
--- a/quasimodo/quasimodo_msgs/CMakeLists.txt
+++ b/quasimodo/quasimodo_msgs/CMakeLists.txt
@@ -51,12 +51,16 @@ add_message_files(
   retrieval_query.msg
   retrieval_query_result.msg
   SOMA2Object.msg
+  fused_world_state_object.msg
+  model_array.msg
+  retrieval_query_array.msg
 )
 
 ## Generate services in the 'srv' folder
 add_service_files(
   FILES
   index_frame.srv
+  index_cloud.srv
   model_from_frame.srv
   cloud_from_model.srv
   fuse_models.srv
@@ -64,6 +68,13 @@ add_service_files(
   query_cloud.srv
   simple_query_cloud.srv
   visualize_query.srv
+  segment_model.srv
+  metaroom_pair.srv
+  transform_cloud.srv
+  mask_pointclouds.srv
+  insert_model.srv
+  model_to_retrieval_query.srv
+  recognize.srv
 )
 
 ## Generate actions in the 'action' folder
diff --git a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg
new file mode 100644
index 00000000..4ca4638a
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg
@@ -0,0 +1,12 @@
+sensor_msgs/PointCloud2 surfel_cloud
+sensor_msgs/PointCloud2 features
+sensor_msgs/PointCloud2 keypoints
+string object_id
+uint64 vocabulary_id
+sensor_msgs/Image[] images
+sensor_msgs/Image[] depths
+sensor_msgs/Image[] masks
+geometry_msgs/Pose[] transforms
+string inserted_at
+string removed_at
+string associated_mongodb_fields_map
diff --git a/quasimodo/quasimodo_msgs/msg/model.msg b/quasimodo/quasimodo_msgs/msg/model.msg
index c732cc22..28f30d60 100644
--- a/quasimodo/quasimodo_msgs/msg/model.msg
+++ b/quasimodo/quasimodo_msgs/msg/model.msg
@@ -2,3 +2,5 @@ uint64 model_id
 geometry_msgs/Pose[] local_poses
 rgbd_frame[] frames
 sensor_msgs/Image[] masks
+sensor_msgs/PointCloud2[] clouds
+geometry_msgs/Pose global_pose
diff --git a/quasimodo/quasimodo_msgs/msg/model_array.msg b/quasimodo/quasimodo_msgs/msg/model_array.msg
new file mode 100644
index 00000000..6c4d29ad
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/msg/model_array.msg
@@ -0,0 +1 @@
+quasimodo_msgs/model[] models
diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_query.msg b/quasimodo/quasimodo_msgs/msg/retrieval_query.msg
index ba94f84d..cff71091 100644
--- a/quasimodo/quasimodo_msgs/msg/retrieval_query.msg
+++ b/quasimodo/quasimodo_msgs/msg/retrieval_query.msg
@@ -1,7 +1,12 @@
+uint64 ALL_QUERY = 0
+uint64 MONGODB_QUERY = 1
+uint64 METAROOM_QUERY = 2
 sensor_msgs/PointCloud2 cloud
 sensor_msgs/Image image
 sensor_msgs/Image depth
 sensor_msgs/Image mask
+geometry_msgs/Pose global_pose
 sensor_msgs/CameraInfo camera
 int32 number_query
 geometry_msgs/Transform room_transform
+uint64 query_kind
diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_query_array.msg b/quasimodo/quasimodo_msgs/msg/retrieval_query_array.msg
new file mode 100644
index 00000000..4a112f10
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/msg/retrieval_query_array.msg
@@ -0,0 +1 @@
+quasimodo_msgs/retrieval_query[] queries
diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_result.msg b/quasimodo/quasimodo_msgs/msg/retrieval_result.msg
index ce37b62c..e8a41456 100644
--- a/quasimodo/quasimodo_msgs/msg/retrieval_result.msg
+++ b/quasimodo/quasimodo_msgs/msg/retrieval_result.msg
@@ -6,3 +6,5 @@ image_array[] retrieved_masks
 string_array[] retrieved_image_paths
 float64[] retrieved_distance_scores
 int_array[] segment_indices
+geometry_msgs/Pose[] global_poses
+string[] vocabulary_ids
diff --git a/quasimodo/quasimodo_msgs/srv/index_cloud.srv b/quasimodo/quasimodo_msgs/srv/index_cloud.srv
new file mode 100644
index 00000000..04b4e7cb
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/srv/index_cloud.srv
@@ -0,0 +1,4 @@
+sensor_msgs/PointCloud2 cloud
+string object_id
+---
+uint64 id
diff --git a/quasimodo/quasimodo_msgs/srv/insert_model.srv b/quasimodo/quasimodo_msgs/srv/insert_model.srv
new file mode 100644
index 00000000..46c3602f
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/srv/insert_model.srv
@@ -0,0 +1,8 @@
+uint64 INSERT=1
+uint64 REMOVE=2
+uint64 action
+quasimodo_msgs/model model
+string object_id
+---
+uint64 vocabulary_id
+string object_id
diff --git a/quasimodo/quasimodo_msgs/srv/mask_pointclouds.srv b/quasimodo/quasimodo_msgs/srv/mask_pointclouds.srv
new file mode 100644
index 00000000..33fb3c54
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/srv/mask_pointclouds.srv
@@ -0,0 +1,4 @@
+sensor_msgs/PointCloud2[] clouds
+sensor_msgs/Image[] masks
+---
+sensor_msgs/PointCloud2[] clouds
diff --git a/quasimodo/quasimodo_msgs/srv/metaroom_pair.srv b/quasimodo/quasimodo_msgs/srv/metaroom_pair.srv
new file mode 100644
index 00000000..9f3907d7
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/srv/metaroom_pair.srv
@@ -0,0 +1,5 @@
+string background
+string foreground
+---
+sensor_msgs/Image[] dynamicmasks
+sensor_msgs/Image[] movingmasks
diff --git a/quasimodo/quasimodo_msgs/srv/model_to_retrieval_query.srv b/quasimodo/quasimodo_msgs/srv/model_to_retrieval_query.srv
new file mode 100644
index 00000000..5e1de100
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/srv/model_to_retrieval_query.srv
@@ -0,0 +1,3 @@
+quasimodo_msgs/model model
+---
+quasimodo_msgs/retrieval_query query
diff --git a/quasimodo/quasimodo_msgs/srv/recognize.srv b/quasimodo/quasimodo_msgs/srv/recognize.srv
new file mode 100644
index 00000000..329bb9da
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/srv/recognize.srv
@@ -0,0 +1,3 @@
+string id
+---
+string id
diff --git a/quasimodo/quasimodo_msgs/srv/segment_model.srv b/quasimodo/quasimodo_msgs/srv/segment_model.srv
new file mode 100644
index 00000000..818264a3
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/srv/segment_model.srv
@@ -0,0 +1,10 @@
+model backgroundmodel
+model[] models
+uint64 background_refine
+uint64 background_update
+uint64 models_refine
+---
+model backgroundmodel
+model[] models
+image_array[] dynamicmasks
+image_array[] movingmasks
diff --git a/quasimodo/quasimodo_msgs/srv/transform_cloud.srv b/quasimodo/quasimodo_msgs/srv/transform_cloud.srv
new file mode 100644
index 00000000..8d505095
--- /dev/null
+++ b/quasimodo/quasimodo_msgs/srv/transform_cloud.srv
@@ -0,0 +1,4 @@
+sensor_msgs/PointCloud2 cloud
+---
+sensor_msgs/PointCloud2 cloud1
+sensor_msgs/PointCloud2 cloud2
diff --git a/quasimodo/quasimodo_msgs/srv/visualize_query.srv b/quasimodo/quasimodo_msgs/srv/visualize_query.srv
index 53cde70e..a10f907a 100644
--- a/quasimodo/quasimodo_msgs/srv/visualize_query.srv
+++ b/quasimodo/quasimodo_msgs/srv/visualize_query.srv
@@ -2,3 +2,4 @@ retrieval_query query
 retrieval_result result
 ---
 sensor_msgs/Image image
+quasimodo_msgs/image_array images
diff --git a/quasimodo/quasimodo_object_search/CMakeLists.txt b/quasimodo/quasimodo_object_search/CMakeLists.txt
new file mode 100644
index 00000000..f7935d57
--- /dev/null
+++ b/quasimodo/quasimodo_object_search/CMakeLists.txt
@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(quasimodo_object_search)
+
+add_definitions(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS quasimodo_msgs pcl_ros mongodb_store)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# Find PCL
+find_package(PCL 1.6 REQUIRED COMPONENTS common io search visualization surface kdtree features surface segmentation octree filter keypoints)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES quasimodo_object_search2
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(${catkin_INCLUDE_DIRS})
+
+## Declare a C++ library
+# add_library(quasimodo_object_search2
+#   src/${PROJECT_NAME}/quasimodo_object_search2.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(quasimodo_object_search2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+add_executable(visualize_database_node src/visualize_database_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(quasimodo_object_search2_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(visualize_database_node
+  ${PCL_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+install(DIRECTORY scripts/
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        USE_SOURCE_PERMISSIONS)
+
+## Mark executables and/or libraries for installation
+# install(TARGETS quasimodo_object_search2 quasimodo_object_search2_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_quasimodo_object_search2.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/quasimodo/quasimodo_object_search/launch/object_search.launch b/quasimodo/quasimodo_object_search/launch/object_search.launch
new file mode 100644
index 00000000..356ca69d
--- /dev/null
+++ b/quasimodo/quasimodo_object_search/launch/object_search.launch
@@ -0,0 +1,26 @@
+<launch>
+    <arg name="search_directly" default="false"/>
+    <arg name="data_path" default="/home/strands/.semanticMap/vocabulary"/>
+    <arg name="enable_incremental" default="false"/>
+    <arg name="add_previous_maps" default="false"/>
+    <arg name="start_add_map_ind" default="-1"/>
+    <arg name="min_match_depth" default="1"/>
+
+    <include file="$(find quasimodo_retrieval)/launch/retrieval.launch">
+        <arg name="search_directly" value="false"/>
+        <arg name="vocabulary_path" value="$(arg data_path)/vocabulary"/>
+        <arg name="enable_incremental" value="$(arg enable_incremental)"/>
+        <arg name="min_match_depth" value="$(arg min_match_depth)"/>
+    </include>
+
+    <include file="$(find retrieval_processing)/launch/processing.launch">
+        <arg name="data_path" value="$(arg data_path)"/>
+        <arg name="add_previous_maps" value="$(arg add_previous_maps)"/>
+        <arg name="start_add_map_ind" value="$(arg start_add_map_ind)"/>
+    </include>
+
+    <node pkg="quasimodo_object_search" type="insert_object_server.py" name="insert_object_server" output="screen" respawn="true"/>
+
+    <node pkg="surfelize_it" type="surfelize_server" name="surfelize_server" output="screen" respawn="true"/>
+
+</launch>
diff --git a/quasimodo/quasimodo_object_search/package.xml b/quasimodo/quasimodo_object_search/package.xml
new file mode 100644
index 00000000..421cac79
--- /dev/null
+++ b/quasimodo/quasimodo_object_search/package.xml
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<package>
+  <name>quasimodo_object_search</name>
+  <version>0.0.0</version>
+  <description>The quasimodo_object_search2 package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="nbore@todo.todo">nbore</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/quasimodo_object_search2</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>quasimodo_msgs</build_depend>
+  <build_depend>mongodb_store</build_depend>
+  <run_depend>quasimodo_msgs</run_depend>
+  <run_depend>mongodb_store</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py
new file mode 100755
index 00000000..1b2e5fca
--- /dev/null
+++ b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py
@@ -0,0 +1,176 @@
+#!/usr/bin/env python
+
+import roslib
+import rospy
+from sensor_msgs.msg import PointCloud2, PointField
+#from world_modeling.srv import *
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+
+# SOMA2 stuff
+from soma_msgs.msg import SOMAObject
+from mongodb_store.message_store import MessageStoreProxy
+from soma_manager.srv import *
+from geometry_msgs.msg import Pose, Transform
+from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse
+from soma_io.state import World, Object
+from datetime import datetime, timedelta
+from quasimodo_msgs.msg import fused_world_state_object
+from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse
+from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse
+from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse
+from geometry_msgs.msg import Pose
+import time
+
+UPDATE_INT_MINUTES = 10.0
+
+def chron_callback():
+
+    print("making query")
+    soma_query = rospy.ServiceProxy('soma/query_db',SOMAQueryObjs)
+    print("done query")
+
+    # Query all observations during the last 30 mins
+    query = SOMAQueryObjsRequest()
+    query.query_type = 0
+    query.objecttypes=['unknown']
+    query.uselowertime = True
+    query.usedates = True
+    dt_obj = datetime.now() - timedelta(minutes=UPDATE_INT_MINUTES) #fromtimestamp(query.lowerdate
+    query.lowerhour = dt_obj.hour
+    print "Hour %d" % query.lowerhour
+    query.lowerminutes = dt_obj.minute
+    print "Minute %d" % query.lowerminutes
+
+    response = soma_query(query)
+
+    if not response.objects:
+        print("No SOMA objects!")
+    else:
+        print("got " + str(len(response.objects)) + " SOMA objects")
+        msg_store = MessageStoreProxy(database='world_state', collection='quasimodo')
+        # This hardcoding is no good!
+        world_model = World(server_host='localhost',server_port=62345)
+        #print world_model
+        for x in response.objects:
+            print(x.id)
+            wo = world_model.get_object(x.id)
+
+            transforms = [] #wo._poses
+            req = mask_pointcloudsRequest()
+            for obs, pose in zip(wo._observations, wo._poses):
+
+                #rgb = fo.get_message("/head_xtion/rgb/image_rect_color")
+                req.clouds.append(obs.get_message("/head_xtion/depth_registered/points"))
+                req.masks.append(obs.get_message("rgb_mask"))
+
+                #pointclouds.append(obs.get_message("object_cloud_camframe"))
+                #could we set everything that's in the background to inf/0?
+                transform = Transform()
+                transform.rotation.x = pose.quaternion.x
+                transform.rotation.y = pose.quaternion.y
+                transform.rotation.z = pose.quaternion.z
+                transform.rotation.w = pose.quaternion.w
+                transform.translation.x = pose.position.x
+                transform.translation.y = pose.position.y
+                transform.translation.z = pose.position.z
+
+                transforms.append(transform)
+
+            print "Waiting for retrieval_segmentation_service..."
+            rospy.wait_for_service('retrieval_segmentation_service')
+            try:
+                surfelize_server = rospy.ServiceProxy('retrieval_segmentation_service', mask_pointclouds)
+                resp = surfelize_server(req)
+            except rospy.ServiceException, e:
+                print "Service call failed: %s"%e
+                return
+
+            #sensor_msgs/PointCloud2[] registered_views
+            #geometry_msgs/Transform[] registered_views_transforms
+            #std_msgs/Float32[]	  intrinsics # can be empty
+            #---
+            #sensor_msgs/PointCloud2 processed_cloud
+            req = ProcessRegisteredViewsRequest()
+            req.registered_views = resp.clouds
+            # This should possibly be transformed to the frame of the first
+            # observation to make visualization a bit easier
+            req.registered_views_transforms = transforms
+
+            print "Waiting for surfelize_server..."
+            rospy.wait_for_service('surfelize_server')
+            try:
+                surfelize_server = rospy.ServiceProxy('surfelize_server', ProcessRegisteredViews)
+                resp = surfelize_server(req)
+            except rospy.ServiceException, e:
+                print "Service call failed: %s"%e
+                return
+
+            new_obj = fused_world_state_object()
+            new_obj.surfel_cloud = resp.processed_cloud
+            new_obj.object_id = x.id
+            now = datetime.now()
+            new_obj.inserted_at = now.strftime("%Y-%m-%d %H:%M:%S")
+
+            for obs, pose in zip(wo._observations, wo._poses):
+                new_obj.transforms.append(Pose())
+                new_obj.transforms[-1].position.x = pose.position.x
+                new_obj.transforms[-1].position.y = pose.position.y
+                new_obj.transforms[-1].position.z = pose.position.z
+                new_obj.transforms[-1].orientation.x = pose.quaternion.x
+                new_obj.transforms[-1].orientation.y = pose.quaternion.y
+                new_obj.transforms[-1].orientation.z = pose.quaternion.z
+                new_obj.transforms[-1].orientation.w = pose.quaternion.w
+
+            print "Waiting for retrieval_features_service..."
+            rospy.wait_for_service('retrieval_features_service')
+            req = transform_cloudRequest()
+            req.cloud = resp.processed_cloud
+            try:
+                surfelize_server = rospy.ServiceProxy('retrieval_features_service', transform_cloud)
+                resp = surfelize_server(req)
+            except rospy.ServiceException, e:
+                print "Service call failed: %s"%e
+                return
+
+            new_obj.features = resp.cloud1
+            new_obj.keypoints = resp.cloud2
+
+            object_id = msg_store.insert(new_obj)
+
+            print "Waiting for retrieval_vocabulary_service..."
+            rospy.wait_for_service('retrieval_vocabulary_service')
+            req = index_cloudRequest()
+            req.cloud = new_obj.features
+            req.object_id = object_id
+            try:
+                surfelize_server = rospy.ServiceProxy('retrieval_vocabulary_service', index_cloud)
+                resp = surfelize_server(req)
+            except rospy.ServiceException, e:
+                print "Service call failed: %s"%e
+                return
+
+            new_obj.vocabulary_id = resp.id
+            msg_store.update_id(object_id, new_obj)
+            # Now, simply save this in mongodb, or maybe extract features first?
+
+            print object_id
+
+            #break # if you only want to add one object
+
+
+
+    print("done")
+
+
+if __name__ == '__main__':
+    rospy.init_node('create_object_search_observation', anonymous = False)
+    print("getting soma service")
+    rospy.wait_for_service('soma/query_db')
+    print("done initializing")
+
+    r = rospy.Rate(1.0/(60.0*UPDATE_INT_MINUTES)) # 10hz
+    while not rospy.is_shutdown():
+        chron_callback()
+        break
+        r.sleep()
diff --git a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py
new file mode 100755
index 00000000..ca468fde
--- /dev/null
+++ b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py
@@ -0,0 +1,187 @@
+#!/usr/bin/env python
+
+import roslib
+import rospy
+from sensor_msgs.msg import PointCloud2, PointField
+#from world_modeling.srv import *
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+
+# SOMA2 stuff
+from soma_msgs.msg import SOMAObject
+from mongodb_store.message_store import MessageStoreProxy
+from soma_manager.srv import *
+from geometry_msgs.msg import Pose, Transform
+from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse
+from soma_io.state import World, Object
+from datetime import datetime, timedelta
+from quasimodo_msgs.msg import fused_world_state_object, image_array
+from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse
+from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse
+from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse
+from quasimodo_msgs.srv import insert_model, insert_modelRequest, insert_modelResponse
+from geometry_msgs.msg import Pose
+from std_msgs.msg import Empty
+import time
+import json
+
+pub = ()
+
+def insert_model_cb(sreq):
+    msg_store = MessageStoreProxy(database='world_state', collection='quasimodo')
+
+    local_poses = sreq.model.local_poses
+
+    transforms = []
+    req = mask_pointcloudsRequest()
+    for cloud, mask, pose in zip(sreq.model.clouds, sreq.model.masks, sreq.model.local_poses):
+        req.clouds.append(cloud)
+        req.masks.append(mask)
+
+        transform = Transform()
+        transform.rotation.x = pose.orientation.x
+        transform.rotation.y = pose.orientation.y
+        transform.rotation.z = pose.orientation.z
+        transform.rotation.w = pose.orientation.w
+        transform.translation.x = pose.position.x
+        transform.translation.y = pose.position.y
+        transform.translation.z = pose.position.z
+        transforms.append(transform)
+
+    print "Waiting for retrieval_segmentation_service..."
+    rospy.wait_for_service('retrieval_segmentation_service')
+    try:
+        surfelize_server = rospy.ServiceProxy('retrieval_segmentation_service', mask_pointclouds)
+        resp = surfelize_server(req)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+        return False
+
+    req = ProcessRegisteredViewsRequest()
+    req.registered_views = resp.clouds
+    req.registered_views_transforms = transforms
+
+    print "Waiting for surfelize_server..."
+    rospy.wait_for_service('surfelize_server')
+    try:
+        surfelize_server = rospy.ServiceProxy('surfelize_server', ProcessRegisteredViews)
+        resp = surfelize_server(req)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+        return False
+
+    new_obj = fused_world_state_object()
+    new_obj.surfel_cloud = resp.processed_cloud
+    new_obj.object_id = "null"
+    now = datetime.now()
+    new_obj.inserted_at = now.strftime("%Y-%m-%d %H:%M:%S")
+    new_obj.transforms = local_poses
+
+    new_obj_depths = image_array()
+    new_obj_images = image_array()
+    new_obj_masks = image_array()
+    for rgbd_frame, mask in zip(sreq.model.frames, sreq.model.masks):
+        new_obj_depths.images.append(rgbd_frame.depth)
+        new_obj_images.images.append(rgbd_frame.rgb)
+        new_obj_masks.images.append(mask)
+
+    new_obj_associated_mongodb_fields = {}
+    new_obj_associated_mongodb_fields["depths"] = msg_store.insert(new_obj_depths)
+    new_obj_associated_mongodb_fields["images"] = msg_store.insert(new_obj_images)
+    new_obj_associated_mongodb_fields["masks"] = msg_store.insert(new_obj_masks)
+
+    new_obj.associated_mongodb_fields_map = json.dumps(new_obj_associated_mongodb_fields, ensure_ascii=True)
+
+    # TODO: mongodb does not support including all of this because of the size
+    # for rgbd_frame, mask in zip(sreq.model.frames, sreq.model.masks):
+    #     new_obj.depths.append(rgbd_frame.depth)
+    #     new_obj.images.append(rgbd_frame.rgb)
+    #     new_obj.masks.append(mask)
+
+    print "Waiting for retrieval_features_service..."
+    rospy.wait_for_service('retrieval_features_service')
+    req = transform_cloudRequest()
+    req.cloud = resp.processed_cloud
+    try:
+        surfelize_server = rospy.ServiceProxy('retrieval_features_service', transform_cloud)
+        resp = surfelize_server(req)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+        return False
+
+    new_obj.features = resp.cloud1
+    new_obj.keypoints = resp.cloud2
+
+    object_id = msg_store.insert(new_obj)
+
+    print "Waiting for retrieval_vocabulary_service..."
+    rospy.wait_for_service('retrieval_vocabulary_service')
+    req = index_cloudRequest()
+    req.cloud = new_obj.features
+    req.object_id = object_id
+    try:
+        surfelize_server = rospy.ServiceProxy('retrieval_vocabulary_service', index_cloud)
+        resp = surfelize_server(req)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+        return False
+
+    new_obj.vocabulary_id = resp.id
+    msg_store.update_id(object_id, new_obj)
+    # Now, simply save this in mongodb, or maybe extract features first?
+
+    print object_id
+
+    print("done")
+
+    resp = insert_modelResponse()
+    resp.vocabulary_id = new_obj.vocabulary_id
+    resp.object_id = object_id
+
+    return resp
+
+def remove_model_cb(req):
+
+    msg_store = MessageStoreProxy(database='world_state', collection='quasimodo')
+    # msg_store.delete(req.object_id)
+    # resp = insert_modelResponse()
+    # resp.object_id = req.object_id
+
+    new_obj = msg_store.query_id(req.object_id, 'quasimodo_msgs/fused_world_state_object')[0]
+    #print new_obj
+    now = datetime.now()
+    new_obj.removed_at = now.strftime("%Y-%m-%d %H:%M:%S")
+    msg_store.update_id(req.object_id, new_obj)
+
+    resp = insert_modelResponse()
+    resp.vocabulary_id = new_obj.vocabulary_id
+    resp.object_id = req.object_id
+
+    return resp
+
+def service_callback(req):
+
+    if req.action == insert_modelRequest.REMOVE:
+        resp = remove_model_cb(req)
+        if not resp:
+            print "Failed to remove model..."
+    elif req.action == insert_modelRequest.INSERT:
+        resp = insert_model_cb(req)
+        if not resp:
+            print "Failed to remove model..."
+    else:
+        resp = False
+        print "Service options are 1: INSERT and 2: REMOVE"
+
+    empty_msg = Empty()
+    pub.publish(empty_msg)
+
+    return resp
+
+if __name__ == '__main__':
+    rospy.init_node('insert_object_server', anonymous = False)
+
+    pub = rospy.Publisher("/model/added_to_db", data_class=Empty, queue_size=None)
+    service = rospy.Service('insert_model_service', insert_model, service_callback)
+
+    rospy.spin()
diff --git a/quasimodo/quasimodo_object_search/scripts/remove_all_models.py b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py
new file mode 100755
index 00000000..00aacc48
--- /dev/null
+++ b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py
@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+import roslib
+import rospy
+from sensor_msgs.msg import PointCloud2, PointField
+#from world_modeling.srv import *
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+
+# SOMA2 stuff
+from soma_msgs.msg import SOMAObject
+from mongodb_store.message_store import MessageStoreProxy
+from soma_manager.srv import *
+from geometry_msgs.msg import Pose, Transform
+from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse
+from soma_io.state import World, Object
+from datetime import datetime, timedelta
+from quasimodo_msgs.msg import fused_world_state_object, image_array
+from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse
+from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse
+from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse
+from quasimodo_msgs.srv import insert_model, insert_modelRequest, insert_modelResponse
+from geometry_msgs.msg import Pose
+from std_msgs.msg import Empty
+import time
+import json
+
+pub = ()
+
+def remove_models():
+
+    msg_store = MessageStoreProxy(database='world_state', collection='quasimodo')
+    # msg_store.delete(req.object_id)
+    # resp = insert_modelResponse()
+    # resp.object_id = req.object_id
+
+    results = msg_store.query(type='quasimodo_msgs/fused_world_state_object')
+
+    for message, metadata in results:
+        if len(message.removed_at) == 0:
+            message.removed_at = "manually removed"
+            msg_store.update_id(metadata['_id'], message)
+
+    empty_msg = Empty()
+    pub.publish(empty_msg)
+
+
+if __name__ == '__main__':
+    rospy.init_node('remove_all_models', anonymous = False)
+
+    pub = rospy.Publisher("/model/added_to_db", data_class=Empty, queue_size=None)
+
+    remove_models()
diff --git a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py
new file mode 100755
index 00000000..eaeda48f
--- /dev/null
+++ b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py
@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+import roslib
+import rospy
+from sensor_msgs.msg import PointCloud2, PointField
+#from world_modeling.srv import *
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+import numpy as np
+
+# SOMA2 stuff
+from soma_msgs.msg import SOMAObject
+from mongodb_store.message_store import MessageStoreProxy
+from soma_manager.srv import *
+from geometry_msgs.msg import Pose, Transform
+from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse
+from soma_io.state import World, Object
+from datetime import datetime, timedelta
+from quasimodo_msgs.msg import fused_world_state_object
+from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse
+from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse
+from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse
+from quasimodo_msgs.msg import retrieval_query
+from geometry_msgs.msg import Pose
+import time
+import std_msgs
+
+pub = ()
+cloud_pub = ()
+
+def retrieval_callback(object_id):
+
+    # This hardcoding is no good!
+    world_model = World(server_host='localhost',server_port=62345)
+    print(object_id)
+    wo = world_model.get_object(object_id.data)
+
+    transforms = [] #wo._poses
+    images = []
+    depths = []
+    masks = []
+    cameras = []
+    req = mask_pointcloudsRequest()
+    for obs, pose in zip(wo._observations, wo._poses):
+
+        images.append(obs.get_message("/head_xtion/rgb/image_rect_color"))
+        depthmat = np.zeros((480,640,1), np.uint16)
+        # have to be careful here, Johan has a different encoding
+        depths.append(CvBridge().cv2_to_imgmsg(depthmat, "mono16"))
+        masks.append(obs.get_message("rgb_mask"))
+        cameras.append(obs.get_message("/head_xtion/depth_registered/sw_registered/camera_info"))
+        req.clouds.append(obs.get_message("/head_xtion/depth_registered/points"))
+        req.masks.append(obs.get_message("rgb_mask"))
+
+        transform = Transform()
+        transform.rotation.x = pose.quaternion.x
+        transform.rotation.y = pose.quaternion.y
+        transform.rotation.z = pose.quaternion.z
+        transform.rotation.w = pose.quaternion.w
+        transform.translation.x = pose.position.x
+        transform.translation.y = pose.position.y
+        transform.translation.z = pose.position.z
+
+        transforms.append(transform)
+
+    print "Waiting for retrieval_segmentation_service..."
+    rospy.wait_for_service('retrieval_segmentation_service')
+    try:
+        surfelize_server = rospy.ServiceProxy('retrieval_segmentation_service', mask_pointclouds)
+        resp = surfelize_server(req)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+        return
+
+    req = ProcessRegisteredViewsRequest()
+    req.registered_views = resp.clouds
+    req.registered_views_transforms = transforms
+
+    print "Waiting for surfelize_server..."
+    rospy.wait_for_service('surfelize_server')
+    try:
+        surfelize_server = rospy.ServiceProxy('surfelize_server', ProcessRegisteredViews)
+        resp = surfelize_server(req)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+        return
+
+    query = retrieval_query()
+    query.camera = cameras[0]
+    query.cloud = resp.processed_cloud
+    query.depth = depths[0]
+    query.image = images[0]
+    cv_bgr_mask = cv_image = CvBridge().imgmsg_to_cv2(masks[0], "bgr8")
+    cv_mask = cv2.cvtColor(cv_bgr_mask, cv2.COLOR_BGR2GRAY)
+    query.mask = CvBridge().cv2_to_imgmsg(cv_mask, "mono8") #masks[0]
+    query.number_query = 10
+    query.room_transform = transforms[0]
+    #query.query_kind = retrieval_query.ALL_QUERY
+    query.query_kind = retrieval_query.MONGODB_QUERY
+
+    pub.publish(query)
+    resp.processed_cloud.header.frame_id = "/map"
+    cloud_pub.publish(resp.processed_cloud)
+
+    print("done")
+
+if __name__ == '__main__':
+    rospy.init_node('retrieve_object_search', anonymous = False)
+    pub = rospy.Publisher("/models/query", data_class=retrieval_query, queue_size=None)
+    cloud_pub = rospy.Publisher("/models/fused", data_class=PointCloud2, queue_size=None)
+    sub = rospy.Subscriber("/models/mongodb_query", std_msgs.msg.String, callback=retrieval_callback)
+    rospy.spin()
diff --git a/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp b/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp
new file mode 100644
index 00000000..8ff842aa
--- /dev/null
+++ b/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp
@@ -0,0 +1,90 @@
+#include <ros/ros.h>
+#include <mongodb_store/message_store.h>
+#include <quasimodo_msgs/fused_world_state_object.h>
+#include <std_msgs/Empty.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+using namespace std;
+using PointT = pcl::PointXYZRGB;
+using CloudT = pcl::PointCloud<PointT>;
+
+class visualization_server {
+public:
+
+    ros::NodeHandle n;
+    mongodb_store::MessageStoreProxy db_client;
+    ros::Publisher pub;
+    ros::Subscriber sub;
+    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+    size_t counter;
+
+    visualization_server() : db_client(n, "quasimodo", "world_state"), viewer(new pcl::visualization::PCLVisualizer("3D Viewer")), counter(0)
+    {
+        pub = n.advertise<sensor_msgs::PointCloud2>("/model/retrieval_db", 1);
+
+        sub = n.subscribe("/model/added_to_db", 1, &visualization_server::callback, this);
+
+        viewer->setBackgroundColor(1, 1, 1);
+
+        viewer->initCameraParameters();
+
+    }
+
+    void callback(const std_msgs::Empty& empty_msg)
+    {
+        std::vector<boost::shared_ptr<quasimodo_msgs::fused_world_state_object> > messages;
+        db_client.query(messages);
+
+        viewer->removeAllPointClouds();
+        size_t start_counter = counter;
+        CloudT::Ptr combined(new CloudT);
+        for (boost::shared_ptr<quasimodo_msgs::fused_world_state_object>& msg : messages) {
+            if (!msg->removed_at.empty()) {
+                continue;
+            }
+            if (msg->inserted_at.empty()) {
+                cout << "No insertion for msg with ID: " << msg->object_id << endl;
+            }
+            cout << "Adding " << counter << ":th point cloud..." << endl;
+            CloudT::Ptr cloud(new CloudT);
+            pcl::fromROSMsg(msg->surfel_cloud, *cloud);
+            *combined += *cloud;
+
+            float offset = 1.0f*float(counter - start_counter);
+            for (PointT& p : cloud->points) {
+                p.x += offset;
+            }
+
+            pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
+            string cloud_name = string("cloud") + std::to_string(counter);
+            viewer->addPointCloud<PointT>(cloud, rgb, cloud_name);
+            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloud_name);
+            ++counter;
+        }
+
+        sensor_msgs::PointCloud2 msg_cloud;
+        pcl::toROSMsg(*combined, msg_cloud);
+        pub.publish(msg_cloud);
+    }
+
+    void run()
+    {
+        while (!viewer->wasStopped()) {
+            ros::spinOnce();
+            viewer->spinOnce(100);
+        }
+    }
+
+};
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "visualize_database_node");
+
+    visualization_server vs;
+    vs.run();
+
+    return 0;
+}
diff --git a/quasimodo/quasimodo_retrieval/CMakeLists.txt b/quasimodo/quasimodo_retrieval/CMakeLists.txt
index e1b93d01..ace78e87 100644
--- a/quasimodo/quasimodo_retrieval/CMakeLists.txt
+++ b/quasimodo/quasimodo_retrieval/CMakeLists.txt
@@ -8,7 +8,7 @@ add_definitions(-std=c++11 -O3)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS roscpp tf sensor_msgs std_msgs image_geometry metaroom_xml_parser quasimodo_msgs message_runtime
                                         message_generation tf_conversions pcl_ros image_geometry cv_bridge tf_conversions eigen_conversions
-                                        k_means_tree dynamic_object_retrieval object_3d_benchmark dynamic_reconfigure soma2_msgs)
+                                        k_means_tree dynamic_object_retrieval object_3d_benchmark dynamic_reconfigure soma_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -175,17 +175,18 @@ add_executable(quasimodo_retrieval_server src/quasimodo_retrieval_server.cpp)
 add_executable(quasimodo_retrieval_publisher src/quasimodo_retrieval_publisher.cpp)
 add_executable(quasimodo_visualization_server src/quasimodo_visualization_server.cpp)
 add_executable(quasimodo_visualize_model src/quasimodo_visualize_model.cpp)
+add_executable(quasimodo_visualize_retrieval_cloud src/quasimodo_visualize_retrieval_cloud.cpp)
 add_executable(quasimodo_retrieval_node src/quasimodo_retrieval_node.cpp)
 add_executable(quasimodo_retrieve_observation src/quasimodo_retrieve_observation.cpp)
 add_executable(quasimodo_logging_server src/quasimodo_logging_server.cpp)
 
-add_dependencies(quasimodo_retrieval_server quasimodo_msgs_generate_messages_cpp)
-add_dependencies(quasimodo_retrieval_publisher quasimodo_msgs_generate_messages_cpp)
-add_dependencies(quasimodo_visualization_server quasimodo_msgs_generate_messages_cpp)
-add_dependencies(quasimodo_visualize_model quasimodo_msgs_generate_messages_cpp)
-add_dependencies(quasimodo_retrieval_node quasimodo_msgs_generate_messages_cpp)
-add_dependencies(quasimodo_retrieve_observation quasimodo_msgs_generate_messages_cpp)
-add_dependencies(quasimodo_logging_server quasimodo_msgs_generate_messages_cpp soma2_msgs_generate_messages_cpp)
+add_dependencies(quasimodo_retrieval_server mongodb_store quasimodo_msgs_generate_messages_cpp)
+add_dependencies(quasimodo_retrieval_publisher mongodb_store quasimodo_msgs_generate_messages_cpp)
+add_dependencies(quasimodo_visualization_server mongodb_store quasimodo_msgs_generate_messages_cpp)
+add_dependencies(quasimodo_visualize_model mongodb_store quasimodo_msgs_generate_messages_cpp)
+add_dependencies(quasimodo_retrieval_node mongodb_store quasimodo_msgs_generate_messages_cpp)
+add_dependencies(quasimodo_retrieve_observation mongodb_store quasimodo_msgs_generate_messages_cpp)
+add_dependencies(quasimodo_logging_server mongodb_store quasimodo_msgs_generate_messages_cpp soma_msgs_generate_messages_cpp)
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
@@ -222,6 +223,13 @@ target_link_libraries(quasimodo_visualize_model
   ${catkin_LIBRARIES}
 )
 
+target_link_libraries(quasimodo_visualize_retrieval_cloud
+  ${OpenCV_LIBS}
+  ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
 #target_link_libraries(quasimodo_retrieval_publisher
 #  pfhrgb_estimation extract_sift sift
 #  benchmark_result benchmark_retrieval benchmark_visualization benchmark_overlap
@@ -281,7 +289,7 @@ install(
 
 # Mark executables and/or libraries for installation
 install(TARGETS quasimodo_retrieval_server quasimodo_retrieval_publisher quasimodo_visualization_server
-                quasimodo_visualize_model quasimodo_retrieval_node quasimodo_retrieve_observation quasimodo_logging_server
+                quasimodo_visualize_model quasimodo_visualize_retrieval_cloud quasimodo_retrieval_node quasimodo_retrieve_observation quasimodo_logging_server
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/quasimodo/quasimodo_retrieval/launch/retrieval.launch b/quasimodo/quasimodo_retrieval/launch/retrieval.launch
index f00013ce..69b970b6 100644
--- a/quasimodo/quasimodo_retrieval/launch/retrieval.launch
+++ b/quasimodo/quasimodo_retrieval/launch/retrieval.launch
@@ -1,10 +1,14 @@
 <launch>
     <arg name="search_directly" default="false"/>
     <arg name="vocabulary_path" default="/home/strands/.semanticMap/vocabulary"/>
+    <arg name="enable_incremental" default="false"/>
+    <arg name="min_match_depth" default="3"/>
 
     <node pkg="surfelize_it" type="topic_processor" name="topic_processor" output="screen" respawn="true"/>
     <node pkg="quasimodo_retrieval" type="quasimodo_retrieval_node" name="quasimodo_retrieval_node" output="screen" respawn="true">
         <param name="vocabulary_path" value="$(arg vocabulary_path)"/>
+        <param name="enable_incremental" value="$(arg enable_incremental)"/>
+        <param name="min_match_depth" value="$(arg min_match_depth)"/>
     </node>
     <node pkg="quasimodo_retrieval" type="quasimodo_visualization_server" name="quasimodo_visualization_server" output="screen" respawn="true"/>
     <node pkg="quasimodo_retrieval" type="quasimodo_logging_server" name="quasimodo_logging_server" output="screen" respawn="true"/>
diff --git a/quasimodo/quasimodo_retrieval/package.xml b/quasimodo/quasimodo_retrieval/package.xml
index 967233e0..9b5915f1 100644
--- a/quasimodo/quasimodo_retrieval/package.xml
+++ b/quasimodo/quasimodo_retrieval/package.xml
@@ -58,7 +58,7 @@
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>libqt4-dev</build_depend>
   <build_depend>libqt4-opengl-dev</build_depend>
-  <build_depend>soma2_msgs</build_depend>
+  <build_depend>soma_msgs</build_depend>
   <run_depend>libpcl-all</run_depend>
   <run_depend>libopencv-dev</run_depend>
   <run_depend>cv_bridge</run_depend>
@@ -77,7 +77,7 @@
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>libqt4</run_depend>
   <run_depend>libqt4-opengl</run_depend>
-  <run_depend>soma2_msgs</run_depend>
+  <run_depend>soma_msgs</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp
index 31770f81..1e57f419 100644
--- a/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp
+++ b/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp
@@ -1,4 +1,5 @@
 #include "ros/ros.h"
+#include <exception>
 #include <dynamic_object_retrieval/dynamic_retrieval.h>
 #include <dynamic_object_retrieval/visualize.h>
 #include <object_3d_benchmark/benchmark_retrieval.h>
@@ -13,9 +14,9 @@
 #include <tf_conversions/tf_eigen.h>
 #include <eigen_conversions/eigen_msg.h>
 
-// for logging to soma2 in mongodb
-#include <soma2_msgs/SOMA2Object.h>
-#include <soma_manager/SOMA2InsertObjs.h>
+// for logging to soma in mongodb
+#include <soma_msgs/SOMAObject.h>
+#include <soma_manager/SOMAInsertObjs.h>
 #include <mongodb_store/MongoInsert.h>
 
 using namespace std;
@@ -39,26 +40,37 @@ class logging_server {
 
     void callback(const quasimodo_msgs::retrieval_query_result& res)
     {
-        soma_manager::SOMA2InsertObjs::Request soma_req;
+        soma_manager::SOMAInsertObjs::Request soma_req;
 
         size_t N = res.result.retrieved_clouds.size();
         soma_req.objects.resize(N);
         for (size_t i = 0; i < N; ++i) {
+            cout << "Retrieved image paths length: " << res.result.retrieved_image_paths.size() << endl;
             boost::filesystem::path sweep_xml = boost::filesystem::path(res.result.retrieved_image_paths[i].strings[0]).parent_path() / "room.xml";
-            auto data = SimpleXMLParser<PointT>::loadRoomFromXML(sweep_xml.string(), vector<string>(), false, false);
-            boost::posix_time::ptime start_time = data.roomLogStartTime;
-            boost::posix_time::ptime time_t_epoch(boost::gregorian::date(1970,1,1));
-            boost::posix_time::time_duration diff = start_time - time_t_epoch;
-            Eigen::Affine3d AT;
-            tf::transformTFToEigen(data.vIntermediateRoomCloudTransforms[0], AT);
-            pcl_ros::transformPointCloud(AT.matrix().cast<float>(), res.result.retrieved_clouds[i], soma_req.objects[i].cloud);
+            cout << "Sweep xml: " << sweep_xml.string() << endl;
+            boost::posix_time::time_duration diff;
+            if (sweep_xml.parent_path().parent_path().stem() == "temp") {
+                soma_req.objects[i].cloud = res.result.retrieved_clouds[i];
+                diff = boost::posix_time::time_duration(10, 10, 10); // just placeholder for now, should get real timestamp and transform from mongodb object
+            }
+            else {
+                auto data = SimpleXMLParser<PointT>::loadRoomFromXML(sweep_xml.string(), vector<string>({"RoomIntermediateCloud"}), false, false);
+                boost::posix_time::ptime start_time = data.roomLogStartTime;
+                boost::posix_time::ptime time_t_epoch(boost::gregorian::date(1970,1,1));
+                diff = start_time - time_t_epoch;
+                Eigen::Affine3d AT;
+                cout << "Intermediate room cloud transforms length: " << data.vIntermediateRoomCloudTransforms.size() << endl;
+                tf::transformTFToEigen(data.vIntermediateRoomCloudTransforms[0], AT);
+                pcl_ros::transformPointCloud(AT.matrix().cast<float>(), res.result.retrieved_clouds[i], soma_req.objects[i].cloud);
+            }
             //soma_req.objects[i].cloud = res.result.retrieved_clouds[i];
+            cout << "Retrieved initial poses length: " << res.result.retrieved_initial_poses.size() << endl;
             soma_req.objects[i].pose = res.result.retrieved_initial_poses[i].poses[0];
             soma_req.objects[i].logtimestamp = diff.total_seconds(); //   ros::Time::now().sec;
             soma_req.objects[i].id = res.result.retrieved_image_paths[i].strings[0];
         }
 
-        std::sort(soma_req.objects.begin(), soma_req.objects.end(), [](const soma2_msgs::SOMA2Object& o1, const soma2_msgs::SOMA2Object& o2) {
+        std::sort(soma_req.objects.begin(), soma_req.objects.end(), [](const soma_msgs::SOMAObject& o1, const soma_msgs::SOMAObject& o2) {
             return o1.logtimestamp < o2.logtimestamp;
         });
 
@@ -73,20 +85,25 @@ class logging_server {
         soma_req.objects[N].id = "retrieval_query";
         */
 
-        ros::ServiceClient soma_service = n.serviceClient<soma_manager::SOMA2InsertObjs>("/soma2/insert_objects");
-        soma_manager::SOMA2InsertObjs::Response soma_resp;
-        if (soma_service.call(soma_req, soma_resp)) {
-            cout << "Successfully inserted queries!" << endl;
+        ros::ServiceClient soma_service = n.serviceClient<soma_manager::SOMAInsertObjs>("/soma/insert_objects");
+        soma_manager::SOMAInsertObjs::Response soma_resp;
+        try {
+            if (soma_service.call(soma_req, soma_resp)) {
+                cout << "Successfully inserted queries!" << endl;
+            }
+            else {
+                cout << "Could not connect to SOMA!" << endl;
+            }
         }
-        else {
-            cout << "Could not connect to SOMA!" << endl;
+        catch (exception& e) {
+            cout << "Soma service called failed with: " << e.what() << endl;
         }
 
         /*
         mongodb_store::MongoInsert::Request db_req;
         //db_req.
 
-        ros::ServiceClient db_service = n.serviceClient<mongodb_store::MongoInsert>("/soma2/insert_objects");
+        ros::ServiceClient db_service = n.serviceClient<mongodb_store::MongoInsert>("/soma/insert_objects");
         mongodb_store::MongoInsert::Response db_resp;
         if (db_service.call(db_req, db_resp)) {
             cout << "Successfully inserted queries!" << endl;
diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp
index a6811366..dd34da72 100644
--- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp
+++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp
@@ -19,15 +19,24 @@
 #include "quasimodo_msgs/retrieval_query_result.h"
 #include "quasimodo_msgs/retrieval_query.h"
 #include "quasimodo_msgs/simple_query_cloud.h"
+#include "quasimodo_msgs/query_cloud.h"
 #include <pcl_ros/point_cloud.h>
 #include <cv_bridge/cv_bridge.h>
 #include <sensor_msgs/image_encodings.h>
 #include <tf_conversions/tf_eigen.h>
 #include <tf/transform_datatypes.h>
+#include <eigen_conversions/eigen_msg.h>
 
 #include <dynamic_reconfigure/server.h>
 #include <quasimodo_retrieval/parametersConfig.h>
 
+#define WITH_UNIVERSAL_URIS 1
+
+#if WITH_UNIVERSAL_URIS
+#include <mongodb_store/message_store.h>
+#include <quasimodo_msgs/fused_world_state_object.h>
+#endif
+
 using namespace std;
 
 using PointT = pcl::PointXYZRGB;
@@ -40,13 +49,54 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (HistT,
                                    (float[N], histogram, histogram)
 )
 
+namespace cereal
+{
+//! Loading for std::map<std::string, std::string> for text based archives
+template <class Archive, class C, class A>
+typename std::enable_if<traits::is_text_archive<Archive>::value, void>::type
+load(Archive& ar, std::map<std::string, std::string, C, A>& map)
+{
+    map.clear();
+
+    auto hint = map.begin();
+    while (true) {
+        const auto namePtr = ar.getNodeName();
+        if(!namePtr) {
+            break;
+        }
+        std::string key = namePtr;
+        std::string value; ar( value );
+        hint = map.emplace_hint( hint, std::move( key ), std::move( value ) );
+    }
+}
+} // namespace cereal
+
+template <typename Msg1, typename Msg2>
+Msg1 get_associated_mongodb_field(mongodb_store::MessageStoreProxy& message_store, const Msg2& message, const string& field)
+{
+    stringstream associated_mongodb_fields_map;
+    associated_mongodb_fields_map << "{\"value0\":  " << message.associated_mongodb_fields_map << "}";
+    map<string, string> associated_mongodb_fields;
+    {
+        cereal::JSONInputArchive archive_i(associated_mongodb_fields_map);
+        archive_i(associated_mongodb_fields);
+    }
+    string mongodb_id = associated_mongodb_fields.at(field);
+    pair<boost::shared_ptr<Msg1>, mongo::BSONObj> query = message_store.queryID<Msg1>(mongodb_id);
+    return *query.first;
+}
+
+
 template<typename VocabularyT>
 class retrieval_node {
 public:
     ros::NodeHandle n;
     ros::Publisher pub;
+    ros::Publisher img_pub;
+    ros::Publisher depth_pub;
     ros::ServiceServer service;
     ros::Publisher keypoint_pub;
+    ros::Publisher pubs[10];
     ros::Subscriber sub;
 
     dynamic_reconfigure::Server<quasimodo_retrieval::parametersConfig> server;
@@ -57,11 +107,15 @@ class retrieval_node {
     string retrieval_output;
     string retrieval_input;
 
-    VocabularyT vt;
+    //VocabularyT vt;
+    grouped_vocabulary_tree<HistT, 8> vt;
     dynamic_object_retrieval::vocabulary_summary summary;
 
     double iss_model_resolution; // 0.004
     double pfhrgb_radius_search; // 0.04
+    bool is_running;
+
+    int min_match_depth;
 
     void parameters_callback(quasimodo_retrieval::parametersConfig& config, uint32_t level) {
         iss_model_resolution = config.iss_model_resolution;
@@ -90,7 +144,11 @@ class retrieval_node {
         pn.param<std::string>("output", retrieval_output, std::string("retrieval_result"));
         pn.param<std::string>("input", retrieval_input, std::string("/models/query"));
 
+        pn.param<int>("min_match_depth", min_match_depth, 3);
+
         summary.load(vocabulary_path);
+        cout << "Loaded summary from: " << (vocabulary_path / "vocabulary_summary.json").string() << endl;
+        cout << "With data path: " << summary.noise_data_path << endl;
 
         // Load and save data in the relative format
         /*
@@ -106,19 +164,26 @@ class retrieval_node {
         if (vt.empty()) {
             dynamic_object_retrieval::load_vocabulary(vt, vocabulary_path);
             vt.set_cache_path(vocabulary_path.string());
-            vt.set_min_match_depth(3);
+            vt.set_min_match_depth(min_match_depth);
             vt.compute_normalizing_constants();
         }
 
+        is_running = false;
+
 
         //dynamic_reconfigure::Server<quasimodo_retrieval::parametersConfig>::CallbackType f;
         //f = boost::bind(&retrieval_node::parameters_callback, this, _1, _2);
         server.setCallback(boost::bind(&retrieval_node::parameters_callback, this, _1, _2));
 
         pub = n.advertise<quasimodo_msgs::retrieval_query_result>(retrieval_output, 1);
+        img_pub = n.advertise<sensor_msgs::Image>("/quasimodo_retrieval/raw_rgb_visualization", 1, true);
+        depth_pub = n.advertise<sensor_msgs::Image>("/quasimodo_retrieval/raw_depth_visualization", 1, true);
         keypoint_pub = n.advertise<sensor_msgs::PointCloud2>("/models/keypoints", 1);
+        for (size_t i = 0; i < 10; ++i) {
+            pubs[i] = n.advertise<sensor_msgs::PointCloud2>(string("/retrieval_cloud/") + to_string(i), 1, true);
+        }
         sub = n.subscribe(retrieval_input, 10, &retrieval_node::run_retrieval, this);
-        service = n.advertiseService("/query_normal_cloud", &retrieval_node::service_callback, this);
+        service = n.advertiseService("/quasimodo_retrieval_service", &retrieval_node::service_callback, this);
         /*
         vector<string> folder_xmls = semantic_map_load_utilties::getSweepXmls<PointT>(data_path.string(), true);
         for (const string& xml : folder_xmls) {
@@ -127,6 +192,27 @@ class retrieval_node {
         */
     }
 
+    bool maybe_reload()
+    {
+        dynamic_object_retrieval::vocabulary_summary temp_summary;
+        temp_summary.load(vocabulary_path);
+        if (summary.last_updated == temp_summary.last_updated || is_running) {
+            return false;
+        }
+
+        cout << "Re-loading new vocabulary with timestamp: " << temp_summary.last_updated << endl;
+        ros::Duration(3.0).sleep(); // sleep for a little bit to make sure the vocabulary is saved
+
+        summary = temp_summary;
+        vt = grouped_vocabulary_tree<HistT, 8>();
+        dynamic_object_retrieval::load_vocabulary(vt, vocabulary_path);
+        vt.set_cache_path(vocabulary_path.string());
+        vt.set_min_match_depth(min_match_depth);
+        vt.compute_normalizing_constants();
+
+        return true;
+    }
+
     pair<cv::Mat, cv::Mat> sweep_get_rgbd_at(const boost::filesystem::path& sweep_xml, int i)
     {
         stringstream ss;
@@ -151,6 +237,7 @@ class retrieval_node {
         for (int i = 0; i < transforms.size(); ++i) {
             cv::Mat mask = cv::Mat::zeros(height, width, CV_8UC1);
             int sum = 0;
+            pair<cv::Mat, cv::Mat> intermediate_images = sweep_get_rgbd_at(sweep_xml, i);
             for (const PointT& p : cloud->points) {
                 Eigen::Vector4f q = transforms[i]*p.getVector4fMap();
                 if (q(2)/q(3) < 0) {
@@ -162,6 +249,12 @@ class retrieval_node {
                 if (x >= width || x < 0 || y >= height || y < 0) {
                     continue;
                 }
+                float depth = 0.001f*float(intermediate_images.second.at<uint16_t>(y, x));
+                //cout << "Depth: " << depth << endl;
+                //cout << "Point depth: " << q(2)/q(3) << endl;
+                if (fabs(depth - q(2)) > 0.02f) {
+                    continue;
+                }
                 mask.at<uint8_t>(y, x) = 255;
                 ++sum;
             }
@@ -178,7 +271,6 @@ class retrieval_node {
 
 
             get<0>(images).push_back(mask);
-            pair<cv::Mat, cv::Mat> intermediate_images = sweep_get_rgbd_at(sweep_xml, i);
             get<1>(images).push_back(intermediate_images.first);
             get<2>(images).push_back(intermediate_images.second);
             //get<1>(images).push_back(benchmark_retrieval::sweep_get_rgb_at(sweep_xml, i));
@@ -195,6 +287,85 @@ class retrieval_node {
 
         // now probably do some dilation on the masks
 
+        if (get<0>(images).empty()) {
+            cv::Mat mask = cv::Mat::zeros(height, width, CV_8UC1);
+            cv::Mat depth = cv::Mat::zeros(height, width, CV_16UC1);
+            cv::Mat image = cv::Mat::zeros(height, width, CV_8UC3);
+            get<0>(images).push_back(mask);
+            get<1>(images).push_back(image);
+            get<2>(images).push_back(depth);
+            get<3>(images).push_back(-1);
+        }
+
+        return images;
+    }
+
+    tuple<vector<cv::Mat>, vector<cv::Mat>, vector<cv::Mat>, vector<int> >
+    generate_images_for_mongodb_object(const CloudT::Ptr& cloud, const Eigen::Matrix3f& K,
+        const boost::filesystem::path& sweep_xml,
+        const vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >& transforms)
+    {
+        tuple<vector<cv::Mat>, vector<cv::Mat>, vector<cv::Mat>, vector<int> > images;
+
+        //int height = 480;
+        //int width = 640;
+
+        string mongodb_id = sweep_xml.parent_path().parent_path().stem().string();
+        //string mongodb_uri = string("/world_state/quasimodo/") / mongodb_id;
+
+        mongodb_store::MessageStoreProxy message_store_quasimodo(n, "quasimodo", "world_state");
+        pair<boost::shared_ptr<quasimodo_msgs::fused_world_state_object>, mongo::BSONObj> query = message_store_quasimodo.queryID<quasimodo_msgs::fused_world_state_object>(mongodb_id);
+        query.first->depths = get_associated_mongodb_field<quasimodo_msgs::image_array>(message_store_quasimodo, *query.first, "depths").images;
+        query.first->images = get_associated_mongodb_field<quasimodo_msgs::image_array>(message_store_quasimodo, *query.first, "images").images;
+        query.first->masks = get_associated_mongodb_field<quasimodo_msgs::image_array>(message_store_quasimodo, *query.first, "masks").images;
+
+        for (int i = 0; i < query.first->images.size(); ++i) {
+            cv_bridge::CvImagePtr cv_mask_ptr;
+            try {
+                cv_mask_ptr = cv_bridge::toCvCopy(query.first->masks[i], sensor_msgs::image_encodings::BGR8);
+            }
+            catch (cv_bridge::Exception& e) {
+                ROS_ERROR("cv_bridge exception: %s", e.what());
+                exit(-1);
+            }
+            cv::Mat mask;
+            cv::cvtColor(cv_mask_ptr->image, mask, CV_BGR2GRAY);
+
+            cv_bridge::CvImagePtr cv_image_ptr;
+            try {
+                cv_image_ptr = cv_bridge::toCvCopy(query.first->images[i], sensor_msgs::image_encodings::BGR8);
+            }
+            catch (cv_bridge::Exception& e) {
+                ROS_ERROR("cv_bridge exception: %s", e.what());
+                exit(-1);
+            }
+
+            cv_bridge::CvImagePtr cv_depth_ptr;
+            try {
+                cv_depth_ptr = cv_bridge::toCvCopy(query.first->depths[i], sensor_msgs::image_encodings::MONO16);
+            }
+            catch (cv_bridge::Exception& e) {
+                ROS_ERROR("cv_bridge exception: %s", e.what());
+                exit(-1);
+            }
+
+            get<0>(images).push_back(mask);
+            get<1>(images).push_back(cv_image_ptr->image);
+            get<2>(images).push_back(cv_depth_ptr->image);
+            get<3>(images).push_back(i);
+
+        }
+
+        if (get<0>(images).empty()) {
+            cv::Mat mask = cv::Mat::zeros(480, 640, CV_8UC1);
+            cv::Mat depth = cv::Mat::zeros(480, 640, CV_16UC1);
+            cv::Mat image = cv::Mat::zeros(480, 640, CV_8UC3);
+            get<0>(images).push_back(mask);
+            get<1>(images).push_back(image);
+            get<2>(images).push_back(depth);
+            get<3>(images).push_back(-1);
+        }
+
         return images;
     }
 
@@ -308,41 +479,16 @@ class retrieval_node {
         std::cout << "Number of features: " << pfhrgb_cloud.size() << std::endl;
     }
 
-    bool service_callback(quasimodo_msgs::simple_query_cloud::Request& req,
-                          quasimodo_msgs::simple_query_cloud::Response& res)
+    // to have this templated is totally unnecessary but whatever
+    void prune_results(std::pair<std::vector<std::pair<boost::filesystem::path, vocabulary_tree<HistT, 8>::result_type> >,
+                       std::vector<std::pair<boost::filesystem::path, vocabulary_tree<HistT, 8>::result_type> > >& results)
     {
-        // we assume that req.query_cloud contains a cloud with PointXYZRGBNormal
-        cout << "Received query msg..." << endl;
-
-        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normal_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-        pcl::fromROSMsg(req.query_cloud, *normal_cloud);
-
-        CloudT::Ptr cloud(new CloudT);
-        NormalCloudT::Ptr normals(new NormalCloudT);
-        cloud->reserve(normal_cloud->size()); normals->reserve(normal_cloud->size());
-        for (const pcl::PointXYZRGBNormal& pn : normal_cloud->points) {
-            PointT p; p.getVector3fMap() = pn.getVector3fMap(); p.rgba = pn.rgba;
-            NormalT n; n.getNormalVector3fMap() = pn.getNormalVector3fMap();
-            cloud->push_back(p); normals->push_back(n);
-        }
-
-        HistCloudT::Ptr features(new HistCloudT);
-        CloudT::Ptr keypoints(new CloudT);
-        cout << "Computing features..." << endl;
-        test_compute_features(features, keypoints, cloud, normals, false, true);
-        cout << "Done computing features..." << endl;
-
-        vector<CloudT::Ptr> retrieved_clouds;
-        vector<boost::filesystem::path> sweep_paths;
-        // we should retrieve more than we want and only keep the results with valid files on the system
-        auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree<HistT, 8>&)vt, features, 200, vocabulary_path, summary);
-
-        // This is just to make sure that we have valid results even when some meta rooms have been deleted
         size_t counter = 0;
-        while (counter < number_query) {
+        while (counter < std::min(number_query, int32_t(results.first.size()))) {
+            // assume that everything in mongodb is kept
             if (!boost::filesystem::exists(results.first[counter].first)) {
-                auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair<boost::filesystem::path, vocabulary_tree<HistT, 8>::result_type>& res) {
-                    return boost::filesystem::exists(res.first);
+                auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair<boost::filesystem::path, typename VocabularyT::result_type>& v) {
+                    return boost::filesystem::exists(v.first);
                 });
                 if (iter == results.first.end()) {
                     break;
@@ -354,65 +500,74 @@ class retrieval_node {
             }
         }
         results.first.resize(counter);
+    }
 
-        tie(retrieved_clouds, sweep_paths) = benchmark_retrieval::load_retrieved_clouds(results.first);
-
-        cout << "Query cloud size: " << cloud->size() << endl;
-        for (CloudT::Ptr& c : retrieved_clouds) {
-            cout << "Retrieved cloud size: " << c->size() << endl;
-        }
-
-        vector<float> scores;
-        vector<int> indices;
-        for (auto s : results.first) {
-            boost::filesystem::path segment_path = s.first;
-            string name = segment_path.stem().string();
-            size_t last_index = name.find_last_not_of("0123456789");
-            int index = stoi(name.substr(last_index + 1));
-            indices.push_back(index);
-            scores.push_back(s.second.score);
-        }
-
-        vector<vector<Eigen::Matrix4f>, Eigen::aligned_allocator<Eigen::Matrix4f > > initial_poses;
-        vector<vector<cv::Mat> > masks(retrieved_clouds.size());
-        vector<vector<cv::Mat> > images(retrieved_clouds.size());
-        vector<vector<cv::Mat> > depths(retrieved_clouds.size());
-        vector<vector<string> > paths(retrieved_clouds.size());
-        Eigen::Matrix3f K;
-        // The new parameters in surfelize_it: 528, 525, 317, 245
-        // The old parameters in surfelize_it: 540, 540, 320, 240
-
-        // The old parameters in this node:
-        // K << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f;
-
-        // Just for johan to test:
-        K << 540.0f, 0.0f, 320.0f, 0.0f, 540.0f, 240.0f, 0.0f, 0.0f, 1.0f;
-        for (int i = 0; i < retrieved_clouds.size(); ++i) {
-            vector<int> inds;
-            auto sweep_data = SimpleXMLParser<PointT>::loadRoomFromXML(sweep_paths[i].string(), std::vector<std::string>{"RoomIntermediateCloud"}, false, false);
-            vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > sweep_transforms;
-            for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) {
-                Eigen::Affine3d e;
-                tf::transformTFToEigen(t, e);
-                sweep_transforms.push_back(e.inverse().matrix().cast<float>());
+    void prune_results(std::pair<std::vector<std::pair<std::vector<boost::filesystem::path>, grouped_vocabulary_tree<HistT, 8>::result_type> >,
+                       std::vector<std::pair<std::vector<boost::filesystem::path>, grouped_vocabulary_tree<HistT, 8>::result_type> > >& results)
+    {
+        size_t counter = 0;
+        while (counter < std::min(number_query, int32_t(results.first.size()))) {
+            // assume that everything in mongodb is kept
+            if (!boost::filesystem::exists(results.first[counter].first[0])) {
+                auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair<std::vector<boost::filesystem::path>, grouped_vocabulary_tree<HistT, 8>::result_type>& v) {
+                    return boost::filesystem::exists(v.first[0]);
+                });
+                if (iter == results.first.end()) {
+                    break;
+                }
+                std::swap(results.first[counter], *iter);
             }
-            tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms);
-            for (int j = 0; j < inds.size(); ++j) {
-                paths[i].push_back(sweep_paths[i].string() + " " + to_string(inds[j]));
+            else {
+                ++counter;
             }
         }
+        results.first.resize(counter);
+    }
 
-        res.result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices);
+    boost::filesystem::path base_path(const boost::filesystem::path& path)
+    {
+        return path;
+    }
 
-        cout << "Finished retrieval..." << endl;
+    boost::filesystem::path base_path(const vector<boost::filesystem::path>& path)
+    {
+        return path[0];
     }
 
-    void run_retrieval(const quasimodo_msgs::retrieval_query::ConstPtr& query_msg) //const string& sweep_xml)
+    vector<int> vocabulary_set(const grouped_vocabulary_tree<HistT, 8>::result_type& vec)
     {
-        cout << "Received query msg..." << endl;
+        return vec.subgroup_global_indices;
+    }
+
+    vector<int> vocabulary_set(const vocabulary_tree<HistT, 8>::result_type&  ind)
+    {
+        return vector<int> {ind.index};
+    }
+
+    bool retrieval_implementation(const quasimodo_msgs::retrieval_query& query, quasimodo_msgs::retrieval_result& result, geometry_msgs::Transform& query_room_transform)
+    {
+        cout << "Received query msg of kind " << query.query_kind << "..." << endl;
+        is_running = true;
+
+        dynamic_object_retrieval::uri_kind kind;
+        switch (query.query_kind) {
+        case quasimodo_msgs::retrieval_query::MONGODB_QUERY:
+            kind = dynamic_object_retrieval::uri_kind::mongodb;
+            break;
+        case quasimodo_msgs::retrieval_query::METAROOM_QUERY:
+            kind = dynamic_object_retrieval::uri_kind::metaroom;
+            break;
+        case quasimodo_msgs::retrieval_query::ALL_QUERY:
+            kind = dynamic_object_retrieval::uri_kind::all;
+            break;
+        default:
+            cout << "Invalid quasimodo_msgs::retrieval_query::query_kind option: " << query.query_kind << endl;
+            is_running = false;
+            return false;
+        }
 
         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normal_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-        pcl::fromROSMsg(query_msg->cloud, *normal_cloud);
+        pcl::fromROSMsg(query.cloud, *normal_cloud);
         CloudT::Ptr cloud(new CloudT);
         NormalCloudT::Ptr normals(new NormalCloudT);
         cloud->reserve(normal_cloud->size()); normals->reserve(normal_cloud->size());
@@ -423,10 +578,11 @@ class retrieval_node {
         }
 
         image_geometry::PinholeCameraModel cam_model;
-        cam_model.fromCameraInfo(query_msg->camera);
+        cam_model.fromCameraInfo(query.camera);
         cv::Matx33d cvK = cam_model.intrinsicMatrix();
         Eigen::Matrix3f K = Eigen::Map<Eigen::Matrix3d>(cvK.val).cast<float>();
-        K << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f; // should be removed if Johan returns K?
+        K << 528.0f, 0.0f, 317.0f, 0.0f, 525.0f, 245.0f, 0.0f, 0.0f, 1.0f; // surfelize_it intrinsics
+       // K << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f; // should be removed if Johan returns K?
 
         HistCloudT::Ptr features(new HistCloudT);
         CloudT::Ptr keypoints(new CloudT);
@@ -445,31 +601,39 @@ class retrieval_node {
         vector<boost::filesystem::path> sweep_paths;
         //auto results = dynamic_object_retrieval::query_reweight_vocabulary(vt, refined_query, query_image, query_depth,
         //                                                                   K, number_query, vocabulary_path, summary, false);
-        auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree<HistT, 8>&)vt, features, 200, vocabulary_path, summary);
+        /*
+         * This should be fine for grouped queries, what I was using in the paper
+         * The exists next will have to be changed somehow
+         * Then load_retrieved_clouds is exactly the same that I used in the paper
+         *
+         *
+         */
+        auto results = dynamic_object_retrieval::query_reweight_vocabulary((VocabularyT&)vt, features, 200, vocabulary_path, summary, false, kind);
 
         // This is just to make sure that we have valid results even when some meta rooms have been deleted
-        size_t counter = 0;
-        while (counter < number_query) {
-            if (!boost::filesystem::exists(results.first[counter].first)) {
-                auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair<boost::filesystem::path, vocabulary_tree<HistT, 8>::result_type>& v) {
-                    return boost::filesystem::exists(v.first);
-                });
-                if (iter == results.first.end()) {
-                    break;
-                }
-                std::swap(results.first[counter], *iter);
-            }
-            else {
-                ++counter;
-            }
-        }
-        results.first.resize(counter);
+        prune_results(results);
 
+        // this is OK, both return the clouds and paths
         tie(retrieved_clouds, sweep_paths) = benchmark_retrieval::load_retrieved_clouds(results.first);
 
-        auto data = SimpleXMLParser<PointT>::loadRoomFromXML(sweep_paths[0].string(), std::vector<std::string>{"RoomIntermediateCloud"}, false, false);
-        tf::StampedTransform room_transform = data.vIntermediateRoomCloudTransforms[0];
-        room_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
+        cout << "Sweep paths:" << endl;
+        for (boost::filesystem::path sweep_path : sweep_paths) {
+            cout << sweep_path.string() << endl;
+        }
+
+
+        // FIXME: This is a hack that requires that at least one is not from MongoDB, needs fixing!
+        tf::StampedTransform room_transform;
+        for (int i = 0; i < retrieved_clouds.size(); ++i) {
+            string name = base_path(results.first[i].first).stem().string();
+            cout << name << endl;
+            if (name != "segment") {
+                auto data = SimpleXMLParser<PointT>::loadRoomFromXML(sweep_paths[i].string(), std::vector<std::string>{"RoomIntermediateCloud"}, false, false);
+                room_transform = data.vIntermediateRoomCloudTransforms[0];
+                room_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
+                break;
+            }
+        }
 
         cout << "Query cloud size: " << cloud->size() << endl;
         for (CloudT::Ptr& c : retrieved_clouds) {
@@ -478,49 +642,107 @@ class retrieval_node {
 
         vector<float> scores;
         vector<int> indices;
+        vector<vector<int> > vocabulary_ids;
         for (auto s : results.first) {
-            boost::filesystem::path segment_path = s.first;
+            boost::filesystem::path segment_path = base_path(s.first);
             string name = segment_path.stem().string();
-            size_t last_index = name.find_last_not_of("0123456789");
-            int index = stoi(name.substr(last_index + 1));
-            indices.push_back(index);
-            // indices.push_back(s.second.index); // global index in the vocabulary
+            cout << name << endl;
+            if (name == "segment") { // for the mongodb results
+                indices.push_back(-1);
+            }
+            else { // for the metric map results
+                size_t last_index = name.find_last_not_of("0123456789");
+                int index = stoi(name.substr(last_index + 1));
+                indices.push_back(index);
+            }
             scores.push_back(s.second.score);
+            //vocabulary_ids.push_back(s.second.index);
+            vocabulary_ids.push_back(vocabulary_set(s.second));
         }
 
         vector<vector<Eigen::Matrix4f>, Eigen::aligned_allocator<Eigen::Matrix4f > > initial_poses;
+        vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > global_transforms;
         vector<vector<cv::Mat> > masks(retrieved_clouds.size());
         vector<vector<cv::Mat> > images(retrieved_clouds.size());
         vector<vector<cv::Mat> > depths(retrieved_clouds.size());
         vector<vector<string> > paths(retrieved_clouds.size());
         for (int i = 0; i < retrieved_clouds.size(); ++i) {
             vector<int> inds;
-            auto sweep_data = SimpleXMLParser<PointT>::loadRoomFromXML(sweep_paths[i].string(), std::vector<std::string>{"RoomIntermediateCloud"}, false, false);
             vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > sweep_transforms;
-            for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) {
+            if (indices[i] == -1) { // special case for mongodb result
+                global_transforms.push_back(Eigen::Matrix4f());
+                global_transforms.back().setIdentity();
+                tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, base_path(results.first[i].first), sweep_transforms);
+            }
+            else {
+                auto sweep_data = SimpleXMLParser<PointT>::loadRoomFromXML(sweep_paths[i].string(), std::vector<std::string>{"RoomIntermediateCloud"}, false, false);    
+                for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) {
+                    Eigen::Affine3d e;
+                    tf::transformTFToEigen(t, e);
+                    sweep_transforms.push_back(e.inverse().matrix().cast<float>());
+                }
                 Eigen::Affine3d e;
-                tf::transformTFToEigen(t, e);
-                sweep_transforms.push_back(e.inverse().matrix().cast<float>());
+                tf::transformTFToEigen(sweep_data.vIntermediateRoomCloudTransforms[0], e);
+                global_transforms.push_back(e.matrix().cast<float>());
+                tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms);
+
             }
-            tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms);
             for (int j = 0; j < inds.size(); ++j) {
                 paths[i].push_back(sweep_paths[i].string() + " " + to_string(inds[j]));
             }
+        }
+
+        tf::transformTFToMsg(room_transform, query_room_transform);
+        result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices, vocabulary_ids, global_transforms);
 
-            //Eigen::Affine3d AT;
-            //tf::transformTFToEigen(sweep_data.vIntermediateRoomCloudTransforms[0], AT);
-            //pcl::transformPointCloud(*retrieved_clouds[i], *retrieved_clouds[i], AT);
+        cout << "============ Returning filtered results: ============" << endl;
+        for (int i = 0; i < retrieved_clouds.size(); ++i) {
+            sensor_msgs::PointCloud2 cloud_msg = result.retrieved_clouds[i];
+            cloud_msg.header.stamp = ros::Time::now();
+            cloud_msg.header.frame_id = "/map";
+            pubs[i].publish(cloud_msg);
+
+            cout << "Path: " << result.retrieved_image_paths[i].strings[0] << ", Voc. ind: " << result.vocabulary_ids[i] << endl;
         }
+        cout << "=====================================================" << endl;
 
-        //cv::Mat full_query_image = benchmark_retrieval::sweep_get_rgb_at(sweep_xml, scan_index);
-        quasimodo_msgs::retrieval_query_result result;
-        result.query = *query_msg;
+        sensor_msgs::Image img_msg;
+        sensor_msgs::Image depth_msg;
+        tie(img_msg, depth_msg) = construct_results_image(images, depths);
+        img_pub.publish(img_msg);
+        depth_pub.publish(depth_msg);
+
+        cout << "Finished retrieval..." << endl;
+
+        is_running = false;
 
-        tf::transformTFToMsg(room_transform, result.query.room_transform);
-        result.result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices);
+        return true;
+    }
+
+    bool service_callback(quasimodo_msgs::query_cloud::Request& req,
+                          quasimodo_msgs::query_cloud::Response& res)
+    {
+        geometry_msgs::Transform query_room_transform;
+
+        bool success = retrieval_implementation(req.query, res.result, query_room_transform);
+
+        quasimodo_msgs::retrieval_query_result result;
+        result.query = req.query;
+        result.result = res.result;
+        result.query.room_transform = query_room_transform;
         pub.publish(result);
 
-        cout << "Finished retrieval..." << endl;
+        return success;
+    }
+
+    void run_retrieval(const quasimodo_msgs::retrieval_query::ConstPtr& query_msg) //const string& sweep_xml)
+    {
+        quasimodo_msgs::retrieval_query_result result;
+        geometry_msgs::Transform query_room_transform;
+        retrieval_implementation(*query_msg, result.result, query_room_transform);
+        result.query = *query_msg;
+        result.query.room_transform = query_room_transform;
+        pub.publish(result);
     }
 
     void convert_to_img_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image)
@@ -547,6 +769,37 @@ class retrieval_node {
         ros_image = *cv_pub_ptr->toImageMsg();
     }
 
+    pair<sensor_msgs::Image, sensor_msgs::Image> construct_results_image(const vector<vector<cv::Mat> >& images,
+                                                                         const vector<vector<cv::Mat> >& depths)
+    {
+        int width = 640;
+        int height = 480;
+
+        pair<int, int> sizes = make_pair(2, 5);
+
+        cout << "Images size: " << images.size() << endl;
+
+        cv::Mat visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_8UC3);
+        cv::Mat depth_visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_16UC1);
+        for (size_t i = 0; i < images.size(); ++i) {
+            cout << "Image " << i << " size: " << images[i].size() << endl;
+            cout << "Image " << i << " dimensions: " << images[i][0].rows << " x " << images[i][0].cols << endl;
+            size_t offset_height = i / sizes.second;
+            size_t offset_width = i % sizes.second;
+            cv::Rect image_rect(offset_width*width, offset_height*height, width, height);
+            images[i][0].copyTo(visualization(image_rect));
+            depths[i][0].copyTo(depth_visualization(image_rect));
+        }
+
+        sensor_msgs::Image img_msg;
+        convert_to_img_msg(visualization, img_msg);
+
+        sensor_msgs::Image depth_msg;
+        convert_to_depth_msg(depth_visualization, depth_msg);
+
+        return make_pair(img_msg, depth_msg);
+    }
+
     quasimodo_msgs::retrieval_result construct_msgs(const vector<CloudT::Ptr>& clouds,
                                                     const vector<vector<Eigen::Matrix4f>, Eigen::aligned_allocator<Eigen::Matrix4f > >& initial_poses,
                                                     const vector<vector<cv::Mat> >& images,
@@ -554,7 +807,9 @@ class retrieval_node {
                                                     const vector<vector<cv::Mat> >& masks,
                                                     const vector<vector<string> >& paths,
                                                     const vector<float>& scores,
-                                                    const vector<int>& indices)
+                                                    const vector<int>& indices,
+                                                    const vector<vector<int> >& vocabulary_ids,
+                                                    const vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >& global_poses)
     {
         quasimodo_msgs::retrieval_result res;
 
@@ -580,6 +835,8 @@ class retrieval_node {
         res.retrieved_image_paths.resize(number_retrieved);
         res.retrieved_distance_scores.resize(number_retrieved);
         res.segment_indices.resize(number_retrieved);
+        res.vocabulary_ids.resize(number_retrieved);
+        res.global_poses.resize(number_retrieved);
 
         for (int i = 0; i < number_retrieved; ++i) {
             pcl::toROSMsg(*clouds[i], res.retrieved_clouds[i]);
@@ -600,6 +857,14 @@ class retrieval_node {
             }
             res.retrieved_distance_scores[i] = scores[i];
             res.segment_indices[i].ints.push_back(indices[i]);
+
+            stringstream ss;
+            for (int ind : vocabulary_ids[i]) {
+                cout << "Writing retrieval ind: " << ind << endl;
+                ss << ind << " ";
+            }
+            res.vocabulary_ids[i] = ss.str();
+            tf::poseEigenToMsg(Eigen::Affine3d(global_poses[i].cast<double>()), res.global_poses[i]);
         }
 
         return res;
@@ -610,9 +875,36 @@ int main(int argc, char** argv)
 {
     ros::init(argc, argv, "quasimodo_retrieval_node");
 
-    retrieval_node<grouped_vocabulary_tree<HistT, 8> > rs(ros::this_node::getName());
+    ros::NodeHandle pn("~");
+    bool enable_incremental;
+    pn.param<bool>("enable_incremental", enable_incremental, false);
+
+    ros::Rate loop_rate(10);
+
+    if (enable_incremental) {
+
+        retrieval_node<grouped_vocabulary_tree<HistT, 8> > rs(ros::this_node::getName());
+
+        while (true)
+        {
+            ros::spinOnce();
+            loop_rate.sleep();
+            rs.maybe_reload();
+        }
 
-    ros::spin();
+    }
+    else {
+
+        retrieval_node<vocabulary_tree<HistT, 8> > rs(ros::this_node::getName());
+
+        while (true)
+        {
+            ros::spinOnce();
+            loop_rate.sleep();
+            rs.maybe_reload();
+        }
+
+    }
 
     return 0;
 }
diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_visualization_server.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_visualization_server.cpp
index 84596088..fb9c2c75 100644
--- a/quasimodo/quasimodo_retrieval/src/quasimodo_visualization_server.cpp
+++ b/quasimodo/quasimodo_retrieval/src/quasimodo_visualization_server.cpp
@@ -54,8 +54,8 @@ class visualization_server {
     //sensor_msgs/Image image
     //sensor_msgs/CameraInfo camera
     //geometry_msgs/Transform room_transform
-    sensor_msgs::Image vis_img_from_msgs(const quasimodo_msgs::retrieval_query& query,
-                                         const quasimodo_msgs::retrieval_result& result)
+    quasimodo_msgs::visualize_query::Response vis_img_from_msgs(const quasimodo_msgs::retrieval_query& query,
+                                                                const quasimodo_msgs::retrieval_result& result)
     {
         cv_bridge::CvImagePtr cv_ptr;
         try {
@@ -125,19 +125,33 @@ class visualization_server {
 
         label_pub.publish(label_msg);
 
-        cv::Mat visualization = benchmark_retrieval::make_visualization_image(cv_ptr->image, query_label, retrieved_clouds, sweep_paths, dummy_labels, T);
+        vector<cv::Mat> individual_images;
+        cv::Mat visualization = benchmark_retrieval::make_visualization_image(cv_ptr->image, query_label, retrieved_clouds,
+                                                                              sweep_paths, dummy_labels, T, individual_images);
 
-        cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage);
-        cv_pub_ptr->image = visualization;
-        cv_pub_ptr->encoding = "bgr8";
 
-        return *cv_pub_ptr->toImageMsg();
+
+        quasimodo_msgs::visualize_query::Response resp;
+        {
+            cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage);
+            cv_pub_ptr->image = visualization;
+            cv_pub_ptr->encoding = "bgr8";
+            resp.image = *cv_pub_ptr->toImageMsg();
+        }
+        for (cv::Mat& im : individual_images) {
+            cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage);
+            cv_pub_ptr->image = im;
+            cv_pub_ptr->encoding = "bgr8";
+            resp.images.images.push_back(*cv_pub_ptr->toImageMsg());
+        }
+
+        return resp;
     }
 
     bool service_callback(quasimodo_msgs::visualize_query::Request& req,
                           quasimodo_msgs::visualize_query::Response& res)
     {
-        res.image = vis_img_from_msgs(req.query, req.result);
+        res = vis_img_from_msgs(req.query, req.result);
 
         image_pub.publish(res.image);
 
@@ -146,9 +160,9 @@ class visualization_server {
 
     void callback(const quasimodo_msgs::retrieval_query_result& res)
     {
-        sensor_msgs::Image image = vis_img_from_msgs(res.query, res.result);
+        quasimodo_msgs::visualize_query::Response resp = vis_img_from_msgs(res.query, res.result);
 
-        image_pub.publish(image);
+        image_pub.publish(resp.image);
     }
 };
 
diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp
new file mode 100644
index 00000000..627dbeb9
--- /dev/null
+++ b/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp
@@ -0,0 +1,151 @@
+#include <dynamic_object_retrieval/summary_iterators.h>
+#include <dynamic_object_retrieval/dynamic_retrieval.h>
+#include <object_3d_benchmark/benchmark_retrieval.h>
+
+#include <metaroom_xml_parser/load_utilities.h>
+#include <dynamic_object_retrieval/definitions.h>
+#include <object_3d_retrieval/pfhrgb_estimation.h>
+
+#include <k_means_tree/k_means_tree.h>
+#include <vocabulary_tree/vocabulary_tree.h>
+#include <grouped_vocabulary_tree/grouped_vocabulary_tree.h>
+
+#include "ros/ros.h"
+#include "quasimodo_msgs/retrieval_query_result.h"
+#include "quasimodo_msgs/model.h"
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <tf_conversions/tf_eigen.h>
+#include <eigen_conversions/eigen_msg.h>
+
+using namespace std;
+
+using PointT = pcl::PointXYZRGB;
+using CloudT = pcl::PointCloud<PointT>;
+using HistT = pcl::Histogram<250>;
+using HistCloudT = pcl::PointCloud<HistT>;
+using LabelT = semantic_map_load_utilties::LabelledData<PointT>;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (HistT,
+                                   (float[N], histogram, histogram)
+)
+
+class retrieval_cloud_visualizer {
+public:
+    ros::NodeHandle n;
+    ros::Subscriber sub;
+    ros::Publisher pubs[10];
+
+    string input;
+
+    retrieval_cloud_visualizer(const std::string& name)
+    {
+        ros::NodeHandle pn("~");
+
+        pn.param<std::string>("input", input, std::string("/retrieval_result"));
+
+        sub = n.subscribe(input, 1, &retrieval_cloud_visualizer::callback, this);
+
+        for (size_t i = 0; i < 10; ++i) {
+            pubs[i] = n.advertise<sensor_msgs::PointCloud2>(string("/retrieval_raw_cloud/") + to_string(i), 1, true);
+        }
+    }
+
+    CloudT::Ptr construct_cloud(cv::Mat& rgb, cv::Mat& depth, cv::Mat& mask, const Eigen::Matrix3f& K, const Eigen::Matrix4f& T)
+    {
+        int height = rgb.rows;
+        int width = rgb.cols;
+
+        CloudT::Ptr cloud(new CloudT);
+        for (int i = 0; i < height; ++i) {
+            for (int j = 0; j < width; ++j) {
+                if (mask.at<uchar>(i, j) == 0) {
+                    continue;
+                }
+                float d = 0.001f*float(depth.at<uint16_t>(i, j));
+                Eigen::Vector3f ep(float(j), float(i), 1.0f);
+                ep = K.inverse()*ep;
+                ep = d/ep(2)*ep;
+                PointT p;
+                p.getVector3fMap() = ep;
+                cv::Vec3b colors = rgb.at<cv::Vec3b>(i, j);
+                p.r = colors[2]; p.g = colors[1]; p.b = colors[0];
+                cloud->push_back(p);
+            }
+        }
+        CloudT::Ptr temp_cloud(new CloudT);
+        pcl::transformPointCloud(*cloud, *temp_cloud, T);
+        return temp_cloud;
+    }
+
+    void callback(const quasimodo_msgs::retrieval_query_result& result)
+    {
+        const int m = 10;//result.result.retrieved_clouds.size(); // 10!
+
+        for (int j = 0; j < m; ++j) {
+            int n = result.result.retrieved_images[j].images.size();
+            CloudT::Ptr cloud(new CloudT);
+
+            for (int i = 0; i < n; ++i) {
+                Eigen::Matrix3f K; // = Eigen::Map<Eigen::Matrix3d>(cvK.val).cast<float>();
+                K << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f;
+
+                Eigen::Affine3d e;
+                tf::poseMsgToEigen(result.result.retrieved_initial_poses[j].poses[i], e);
+                Eigen::Matrix4f T = e.matrix().cast<float>();
+
+                cv_bridge::CvImagePtr cv_ptr;
+                try {
+                    cv_ptr = cv_bridge::toCvCopy(result.result.retrieved_images[j].images[i], sensor_msgs::image_encodings::BGR8);
+                }
+                catch (cv_bridge::Exception& e) {
+                    ROS_ERROR("cv_bridge exception: %s", e.what());
+                    exit(-1);
+                }
+
+                cv_bridge::CvImagePtr cv_depth_ptr;
+                try {
+                    cv_depth_ptr = cv_bridge::toCvCopy(result.result.retrieved_depths[j].images[i], sensor_msgs::image_encodings::MONO16);
+                }
+                catch (cv_bridge::Exception& e) {
+                    ROS_ERROR("cv_bridge exception: %s", e.what());
+                    exit(-1);
+                }
+
+                cv_bridge::CvImagePtr cv_mask_ptr;
+                try {
+                    cv_mask_ptr = cv_bridge::toCvCopy(result.result.retrieved_masks[j].images[i], sensor_msgs::image_encodings::MONO8);
+                }
+                catch (cv_bridge::Exception& e) {
+                    ROS_ERROR("cv_bridge exception: %s", e.what());
+                    exit(-1);
+                }
+
+                CloudT::Ptr frame_cloud = construct_cloud(cv_ptr->image, cv_depth_ptr->image, cv_mask_ptr->image, K, T);
+                *cloud += *frame_cloud;
+            }
+
+            sensor_msgs::PointCloud2 msg_cloud;
+            pcl::toROSMsg(*cloud, msg_cloud);
+            msg_cloud.header.stamp = ros::Time::now();
+            msg_cloud.header.frame_id = "/map";
+            //tf::transformTFToMsg(room_transform, res.query.room_transform);
+            pubs[j].publish(msg_cloud);
+
+        }
+    }
+
+};
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "quasimodo_visualize_retrieval_cloud");
+
+    retrieval_cloud_visualizer vrc(ros::this_node::getName());
+
+    ros::spin();
+
+    return 0;
+}
+
diff --git a/quasimodo/retrieval_processing/launch/processing.launch b/quasimodo/retrieval_processing/launch/processing.launch
index 13a50de0..6954da56 100644
--- a/quasimodo/retrieval_processing/launch/processing.launch
+++ b/quasimodo/retrieval_processing/launch/processing.launch
@@ -1,26 +1,32 @@
 <launch>
-    <arg name="data_path" default="/home/strands/.semanticMap"/>
-    <arg name="add_previous_maps" default="false"/>
+    <arg name="data_path" default="/home/nbore/Data/KTH_longterm_dataset_labels"/>
+    <arg name="add_previous_maps" default="true"/>
+    <arg name="start_add_map_ind" default="-1"/>
+    <arg name="bypass_segmentation" default="false"/>
+    <arg name="bypass_features" default="false"/>
+    <arg name="bypass_vocabulary" default="false"/>
+    <arg name="bypass_surfelize" default="false"/>
 
     <node pkg="surfelize_it" type="run_map_processor.py" name="run_map_processor" output="screen" respawn="true"/>
     <node pkg="retrieval_processing" type="retrieval_segmentation" name="retrieval_segmentation" output="screen" respawn="false">
         <param name="threshold" value="0.3"/>
         <param name="data_path" value="$(arg data_path)"/>
-        <param name="bypass" value="false" type="bool"/>
+        <param name="bypass" value="$(arg bypass_segmentation)" type="bool"/>
     </node>
     <node pkg="retrieval_processing" type="retrieval_features" name="retrieval_features" output="screen" respawn="false">
-        <param name="bypass" value="false" type="bool"/>
+        <param name="bypass" value="$(arg bypass_features)" type="bool"/>
     </node>
     <node pkg="retrieval_processing" type="retrieval_vocabulary" name="retrieval_vocabulary" output="screen" respawn="false">
         <param name="vocabulary_path" value="$(arg data_path)/vocabulary"/>
         <param name="data_path" value="$(arg data_path)"/>
-        <param name="min_training_sweeps" value="20"/>
-        <param name="bypass" value="false" type="bool"/>
+        <param name="min_training_sweeps" value="100"/>
+        <param name="bypass" value="$(arg bypass_vocabulary)" type="bool"/>
     </node>
     <group if="$(arg add_previous_maps)">
         <node pkg="retrieval_processing" type="retrieval_simulate_observations" name="retrieval_simulate_observations" output="screen" respawn="false">
             <param name="data_path" value="$(arg data_path)"/>
-            <param name="bypass_surfelize" value="false" type="bool"/>
+            <param name="bypass_surfelize" value="$(arg bypass_surfelize)" type="bool"/>
+            <param name="start_add_map_ind" value="$(arg start_add_map_ind)"/>
         </node>
     </group>
 </launch>
diff --git a/quasimodo/retrieval_processing/src/retrieval_features.cpp b/quasimodo/retrieval_processing/src/retrieval_features.cpp
index 11d7a03d..7fa7f128 100644
--- a/quasimodo/retrieval_processing/src/retrieval_features.cpp
+++ b/quasimodo/retrieval_processing/src/retrieval_features.cpp
@@ -15,6 +15,9 @@
 #include <metaroom_xml_parser/load_utilities.h>
 #include <dynamic_object_retrieval/definitions.h>
 
+#include <quasimodo_msgs/transform_cloud.h>
+#include <pcl_ros/point_cloud.h>
+
 #include <ros/ros.h>
 #include <std_msgs/String.h>
 
@@ -35,6 +38,7 @@ using SurfelT = SurfelType;
 using SurfelCloudT = pcl::PointCloud<SurfelT>;
 
 ros::Publisher pub;
+ros::ServiceServer service;
 
 /*
 void test_compute_features(HistCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud,
@@ -136,6 +140,42 @@ void test_compute_features(HistCloudT::Ptr& features, CloudT::Ptr& keypoints, Cl
 }
 */
 
+bool features_service(quasimodo_msgs::transform_cloud::Request& req, quasimodo_msgs::transform_cloud::Response& resp)
+{   
+    double threshold = 0.4;
+
+    SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT);
+    pcl::fromROSMsg(req.cloud, *surfel_cloud);
+    cout << "Got service cloud with size: " << surfel_cloud->size() << endl;
+
+    HistCloudT::Ptr desc_cloud(new HistCloudT);
+    CloudT::Ptr kp_cloud(new CloudT);
+
+    NormalCloudT::Ptr normals(new NormalCloudT);
+    CloudT::Ptr cloud(new CloudT);
+    cloud->reserve(surfel_cloud->size());
+    normals->reserve(surfel_cloud->size());
+    for (const SurfelT& s : surfel_cloud->points) {
+        if (s.confidence < threshold) {
+            continue;
+        }
+        PointT p;
+        p.getVector3fMap() = s.getVector3fMap();
+        p.rgba = s.rgba;
+        NormalT n;
+        n.getNormalVector3fMap() = s.getNormalVector3fMap();
+        cloud->push_back(p);
+        normals->push_back(n);
+    }
+    cout << "Service points after confidence " << threshold << ": " << cloud->size() << endl;
+
+    dynamic_object_retrieval::compute_features(desc_cloud, kp_cloud, cloud, normals);
+    pcl::toROSMsg(*desc_cloud, resp.cloud1);
+    pcl::toROSMsg(*kp_cloud, resp.cloud2);
+
+    return true;
+}
+
 void features_callback(const std_msgs::String::ConstPtr& msg)
 {
     cout << "Received callback with path " << msg->data << endl;
@@ -157,6 +197,9 @@ void features_callback(const std_msgs::String::ConstPtr& msg)
         HistCloudT::Ptr desc_cloud(new HistCloudT);
         CloudT::Ptr kp_cloud(new CloudT);
         dynamic_object_retrieval::compute_features(desc_cloud, kp_cloud, segment, surfel_map);
+
+        cout << "Saving " << feature_path.string() << " with " << desc_cloud->size() << " number of features. " << endl;
+
         //test_compute_features(desc_cloud, kp_cloud, segment, surfel_map);
         if (desc_cloud->empty()) {
             // push back one inf point on descriptors and keypoints
@@ -197,6 +240,7 @@ int main(int argc, char** argv)
     pn.param<bool>("bypass", bypass, 0);
 
     pub = n.advertise<std_msgs::String>("/features_done", 1);
+    service = n.advertiseService("/retrieval_features_service", features_service);
 
     ros::Subscriber sub;
     if (bypass) {
diff --git a/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp b/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp
index 02accb90..ed705211 100644
--- a/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp
+++ b/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp
@@ -13,6 +13,11 @@
 
 #include <ros/ros.h>
 #include <std_msgs/String.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include <quasimodo_msgs/mask_pointclouds.h>
+
+#include <sensor_msgs/image_encodings.h>
 
 #define VISUALIZE 1
 
@@ -57,10 +62,46 @@ int colormap[][3] = {
 
 ros::Publisher pub;
 ros::Publisher vis_cloud_pub;
+ros::ServiceServer service;
 double threshold;
 dynamic_object_retrieval::data_summary data_summary;
 boost::filesystem::path data_path;
 
+bool segmentation_service(quasimodo_msgs::mask_pointclouds::Request& req, quasimodo_msgs::mask_pointclouds::Response& resp)
+{
+    for (size_t i = 0; i < req.clouds.size(); ++i) {
+        CloudT::Ptr cloud(new CloudT);
+        pcl::fromROSMsg(req.clouds[i], *cloud);
+        cv_bridge::CvImagePtr cv_ptr;
+        try {
+            cv_ptr = cv_bridge::toCvCopy(req.masks[i], sensor_msgs::image_encodings::BGR8);
+        }
+        catch (cv_bridge::Exception& e) {
+            ROS_ERROR("cv_bridge exception: %s", e.what());
+            exit(-1);
+        }
+        cv::Mat mask;
+        cv::cvtColor(cv_ptr->image, mask, CV_BGR2GRAY);
+
+        for (size_t y = 0; y < mask.rows; ++y) {
+            for (size_t x = 0; x < mask.cols; ++x) {
+                size_t index = y*mask.cols + x;
+                if (int(mask.at<uchar>(y, x)) != 255) {
+                    cloud->points[index].x = std::numeric_limits<float>::infinity();
+                    cloud->points[index].y = std::numeric_limits<float>::infinity();
+                    cloud->points[index].z = std::numeric_limits<float>::infinity();
+                }
+            }
+        }
+
+        sensor_msgs::PointCloud2 masked_msg;
+        pcl::toROSMsg(*cloud, masked_msg);
+        resp.clouds.push_back(masked_msg);
+    }
+
+    return true;
+}
+
 void segmentation_callback(const std_msgs::String::ConstPtr& msg)
 {
     data_summary.load(data_path);
@@ -192,6 +233,7 @@ int main(int argc, char** argv)
 
     pub = n.advertise<std_msgs::String>("/segmentation_done", 1);
     vis_cloud_pub = n.advertise<sensor_msgs::PointCloud2>("/retrieval_processing/segmentation_cloud", 1);
+    service = n.advertiseService("/retrieval_segmentation_service", segmentation_service);
 
     ros::Subscriber sub;
     if (bypass) {
diff --git a/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp b/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp
index 4dc24ad2..a8d8e636 100644
--- a/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp
+++ b/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp
@@ -94,6 +94,8 @@ int main(int argc, char** argv)
     pn.param<string>("data_path", data_path, "");
 
     pn.param<bool>("bypass_surfelize", bypass_surfelize, true);
+    int start_add_map_ind;
+    pn.param<int>("start_add_map_ind", start_add_map_ind, -1);
 
     sweep_xmls = semantic_map_load_utilties::getSweepXmls<PointT>(data_path, true);
     if (sweep_xmls.empty()) {
@@ -101,14 +103,20 @@ int main(int argc, char** argv)
         return 0;
     }
 
-    sweep_ind = get_correct_sweep_ind(data_path); // 0;
+    if (start_add_map_ind < 0) {
+        sweep_ind = get_correct_sweep_ind(data_path);
+    }
+    else {
+        sweep_ind = start_add_map_ind;
+    }
     cout << "Starting at sweep number: " << sweep_ind << endl;
 
-
     if (bypass_surfelize) {
+        cout << "Advertising " << "/surfelization_done" << endl;
         string_pub = n.advertise<std_msgs::String>("/surfelization_done", 1);
     }
     else {
+        cout << "Advertising " << "/local_metric_map/room_observations" << endl;
         room_pub = n.advertise<semantic_map::RoomObservation>("/local_metric_map/room_observations", 1);
     }
     ros::Subscriber sub = n.subscribe("/vocabulary_done", 1, callback);
@@ -116,11 +124,13 @@ int main(int argc, char** argv)
     ros::Duration(1.0).sleep();
 
     if (bypass_surfelize) {
+        cout << "Publishing initial simulated observation after surfelization: " << sweep_xmls[sweep_ind] << endl;
         std_msgs::String sweep_msg;
         sweep_msg.data = sweep_xmls[sweep_ind++];
         string_pub.publish(sweep_msg);
     }
     else {
+        cout << "Publishing initial simulated observation: " << sweep_xmls[sweep_ind] << endl;
         semantic_map::RoomObservation sweep_msg;
         sweep_msg.xml_file_name = sweep_xmls[sweep_ind++];
         room_pub.publish(sweep_msg);
diff --git a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp
index 7c9c7238..06bc456c 100644
--- a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp
+++ b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp
@@ -12,9 +12,17 @@
 #include <metaroom_xml_parser/load_utilities.h>
 #include <dynamic_object_retrieval/definitions.h>
 
+#include <pcl_ros/point_cloud.h>
+#include <quasimodo_msgs/index_cloud.h>
+
 #include <ros/ros.h>
 #include <std_msgs/String.h>
 
+#include <chrono>  // chrono::system_clock
+#include <ctime>   // localtime
+#include <sstream> // stringstream
+#include <iomanip> // put_time
+
 /**
  * The most reasonable thing to keep track of which metaroom folders that we have traversed
  * would be to create a new json file in the vocabulary folder with a list of all the sweeps
@@ -51,6 +59,7 @@ using AdjacencyT = vector<set<pair<int, int> > >;
 // does that mean we need to provide the querying
 // interface as well? maybe
 ros::Publisher pub;
+ros::ServiceServer service;
 int min_training_sweeps;
 size_t nbr_added_sweeps;
 boost::filesystem::path vocabulary_path;
@@ -124,6 +133,8 @@ void train_vocabulary(const boost::filesystem::path& data_path)
     AdjacencyT adjacencies;
     vector<IndexT> indices;
 
+    dynamic_object_retrieval::segment_uris uris;
+
     size_t counter = 0; // index among all segments
     size_t sweep_i; // index of sweep
     size_t last_sweep = 0; // last index of sweep
@@ -169,6 +180,9 @@ void train_vocabulary(const boost::filesystem::path& data_path)
             sweep_counter = 0;
         }
 
+        // here we insert in the uris
+        uris.uris.push_back(string("file://") + segment_path.string());
+
         if (features_i->size() < min_segment_features) {
             ++counter;
             ++sweep_counter;
@@ -210,7 +224,13 @@ void train_vocabulary(const boost::filesystem::path& data_path)
     summary.nbr_annotated_segments = 0;
     summary.nbr_annotated_sweeps = 0;
 
+    auto now = std::chrono::system_clock::now();
+    auto in_time_t = std::chrono::system_clock::to_time_t(now);
+    char buffer [80];
+    std::strftime(buffer, 80, "%Y-%m-%d %H:%M:%S", std::localtime(&in_time_t));
+    summary.last_updated = string(buffer);
     summary.save(vocabulary_path);
+    uris.save(vocabulary_path);
     dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path);
 }
 
@@ -241,14 +261,14 @@ pair<size_t, size_t> get_offsets_in_data(const boost::filesystem::path& sweep_pa
         // skips 0, that's ok
         if (sweep_i != last_sweep) {
             // this should actually work, right?
-            cout << sweep_path.string() << endl;
-            cout << segment_path.parent_path().parent_path().string() << endl;
+            //cout << sweep_path.string() << endl;
+            //cout << segment_path.parent_path().parent_path().string() << endl;
             if (sweep_path == segment_path.parent_path().parent_path()) {
                 cout << "Matching!" << endl;
                 return make_pair(sweep_i, counter);
             }
             else {
-                cout << "Not matching!" << endl;
+                //cout << "Not matching!" << endl;
             }
         }
 
@@ -260,6 +280,73 @@ pair<size_t, size_t> get_offsets_in_data(const boost::filesystem::path& sweep_pa
     return make_pair(0, 0);
 }
 
+bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msgs::index_cloud::Response& resp)
+{
+    // let's just create a "ghost" indexing for now, to see if it actually works by comparing to the actual
+    // segments_summary.json, maybe even have a separate file for the segment uri:s
+
+    // maybe just skip this before we have actually trained the vocabulary, would make things a lot easier, yes, good idea
+
+    if (vt == NULL) {
+        cout << "Vocabulary not trained yet, can not add any object search observations yet, perform more sweeps first..." << endl;
+        return false;
+    }
+
+    HistCloudT::Ptr features(new HistCloudT);
+    pcl::fromROSMsg(req.cloud, *features);
+    vector<IndexT> indices;
+    // the question is what offsets we should assign these, I think in reality it doesn't matter, possibly the sweep offset might not get set automatically
+    // let's just try 0, 0 for now
+    //IndexT index(sweep_offset + sweep_i, offset, 0); // we only have one segment within these observations -> sweep_index = 0
+    dynamic_object_retrieval::vocabulary_summary summary;
+    summary.load(vocabulary_path);
+    IndexT index(100000, summary.nbr_noise_segments, 0); // this is a bit weird, did not think we relied on these, we only have one segment within these observations -> sweep_index = 0
+    for (size_t i = 0; i < features->size(); ++i) {
+        indices.push_back(index);
+    }
+    AdjacencyT adjacencies;
+
+    if (features->size() > 0) {
+        vt->append_cloud(features, indices, adjacencies, false);
+    }
+
+    {
+        dynamic_object_retrieval::segment_uris uris;
+        if (boost::filesystem::exists(vocabulary_path / "segment_uris.json")) {
+            cout << "Segment uris file already exists, loading..." << endl;
+            uris.load(vocabulary_path);
+        }
+        else {
+            // load all the segments summaries and push them as uris
+            dynamic_object_retrieval::data_summary segments_summary;
+            segments_summary.load(vocabulary_path.parent_path());
+            for (const string& segment_path : segments_summary.index_convex_segment_paths) {
+                uris.uris.push_back(string("file://") + segment_path);
+            }
+        }
+        uris.uris.push_back(string("mongodb://") + "world_state" + "/" + "quasimodo" + "/" + req.object_id);
+        uris.save(vocabulary_path);
+    }
+
+    // This part is new!
+
+    resp.id = summary.nbr_noise_segments;
+    summary.nbr_noise_segments += 1;
+    summary.nbr_noise_sweeps++;
+    auto now = std::chrono::system_clock::now();
+    auto in_time_t = std::chrono::system_clock::to_time_t(now);
+    char buffer [80];
+    std::strftime(buffer, 80, "%Y-%m-%d %H:%M:%S", std::localtime(&in_time_t));
+    //std::stringstream ss;
+    //ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d %H:%M:%S");
+    //summary.last_updated = ss.str();
+    summary.last_updated = string(buffer);
+    summary.save(vocabulary_path);
+    dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path);
+
+    return true;
+}
+
 void vocabulary_callback(const std_msgs::String::ConstPtr& msg)
 {
     cout << "Received callback with path " << msg->data << endl;
@@ -280,6 +367,9 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg)
                 break;
             }
             ++nbr_segmented_sweeps;
+            if (boost::filesystem::path(xml) == sweep_xml) {
+                break;
+            }
         }
         if (nbr_segmented_sweeps >= min_training_sweeps + 1) {
             train_vocabulary(data_path);
@@ -290,6 +380,8 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg)
         return;
     }
 
+    cout << "After training: " << endl;
+
     size_t offset; // same thing here, index among segments, can't take this from vocab, instead parse everything?
     size_t sweep_offset;
     // how do we get this to not require anything
@@ -305,11 +397,16 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg)
     dynamic_object_retrieval::sweep_convex_segment_cloud_map clouds(sweep_xml.parent_path());
     dynamic_object_retrieval::sweep_convex_feature_cloud_map segment_features(sweep_xml.parent_path());
     dynamic_object_retrieval::sweep_convex_keypoint_cloud_map segment_keypoints(sweep_xml.parent_path());
-    for (auto tup : dynamic_object_retrieval::zip(segment_features, segment_keypoints, clouds)) {
+    dynamic_object_retrieval::sweep_convex_segment_map segment_paths(sweep_xml.parent_path());
+    // maybe also iterate over the segment paths?
+    vector<string> segment_strings;
+    for (auto tup : dynamic_object_retrieval::zip(segment_features, segment_keypoints, clouds, segment_paths)) {
         HistCloudT::Ptr features_i;
         CloudT::Ptr keypoints_i;
         CloudT::Ptr segment;
-        tie(features_i, keypoints_i, segment) = tup;
+        boost::filesystem::path segment_path;
+        tie(features_i, keypoints_i, segment, segment_path) = tup;
+        segment_strings.push_back(segment_path.string());
 
         features->insert(features->end(), features_i->begin(), features_i->end());
 
@@ -326,11 +423,26 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg)
         vt->append_cloud(features, indices, adjacencies, false);
     }
 
+    {
+        dynamic_object_retrieval::segment_uris uris;
+        uris.load(vocabulary_path);
+        uris.uris.reserve(uris.uris.size() + segment_strings.size());
+        for (const string& segment_path : segment_strings) {
+            uris.uris.push_back(string("file://") + segment_path);
+        }
+        uris.save(vocabulary_path);
+    }
+
     // This part is new!
     dynamic_object_retrieval::vocabulary_summary summary;
     summary.load(vocabulary_path);
     summary.nbr_noise_segments += counter;
     summary.nbr_noise_sweeps++;
+    auto now = std::chrono::system_clock::now();
+    auto in_time_t = std::chrono::system_clock::to_time_t(now);
+    char buffer [80];
+    std::strftime(buffer, 80, "%Y-%m-%d %H:%M:%S", std::localtime(&in_time_t));
+    summary.last_updated = string(buffer);
     summary.save(vocabulary_path);
     dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path);
     // End of new part!
@@ -374,12 +486,14 @@ int main(int argc, char** argv)
         vt = new VocT(vocabulary_path.string());
         vt->set_min_match_depth(3);
         dynamic_object_retrieval::load_vocabulary(*vt, vocabulary_path);
+        vt->set_cache_path(vocabulary_path.string());
     }
     else {
         vt = NULL;
     }
 
     pub = n.advertise<std_msgs::String>("/vocabulary_done", 1);
+    service = n.advertiseService("/retrieval_vocabulary_service", vocabulary_service);
 
     ros::Subscriber sub;
     if (bypass) {
diff --git a/semantic_map/CMakeLists.txt b/semantic_map/CMakeLists.txt
index 40002022..4f102928 100644
--- a/semantic_map/CMakeLists.txt
+++ b/semantic_map/CMakeLists.txt
@@ -4,7 +4,7 @@ project(semantic_map)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS mongodb_store roscpp rospy std_msgs sensor_msgs pcl_ros message_generation qt_build image_geometry cv_bridge tf_conversions strands_sweep_registration metaroom_xml_parser observation_registration_services)
+find_package(catkin REQUIRED COMPONENTS  quasimodo_msgs semantic_map_msgs mongodb_store roscpp rospy std_msgs sensor_msgs pcl_ros message_generation qt_build image_geometry cv_bridge tf_conversions strands_sweep_registration metaroom_xml_parser observation_registration_services)
 find_package(Boost REQUIRED COMPONENTS system thread program_options filesystem)
 
 set(CMAKE_CXX_FLAGS "-O4 -fPIC -std=c++0x -fpermissive ${CMAKE_CXX_FLAGS}")
@@ -18,21 +18,21 @@ add_definitions(${PCL_DEFINITIONS})
 
 rosbuild_prepare_qt4(QtCore QtXml)
   
-add_message_files(
-  FILES
-  RoomObservation.msg
-)
-
-add_service_files(
-   FILES
-   ClearMetaroomService.srv
-)
-
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-  sensor_msgs
-)
+#add_message_files(
+#  FILES
+#  RoomObservation.msg
+#)
+
+#add_service_files(
+#   FILES
+#   ClearMetaroomService.srv
+#)
+
+#generate_messages(
+#  DEPENDENCIES
+#  std_msgs
+#  sensor_msgs
+#)
 
 catkin_package(
    INCLUDE_DIRS include
@@ -94,11 +94,13 @@ set(SRCS
 
 add_library(semantic_map ${HDRS} ${HDRS_IMPL} ${SRCS})
 add_executable(semantic_map_node include/semantic_map/semantic_map_node.h src/semantic_map_node.cpp src/semantic_map_main.cpp)
+#add_executable(semantic_map_recompute include/semantic_map/semantic_map_node.h src/semantic_map_node.cpp src/semantic_map_recompute.cpp)
 add_executable(load_from_mongo src/load_from_mongo.cpp)
 add_executable(add_to_mongo src/add_to_mongo.cpp)
 
-add_dependencies(semantic_map semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp)
-add_dependencies(semantic_map_node semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp)
+add_dependencies(semantic_map semantic_map_msgs_generate_messages_cpp quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp)
+add_dependencies(semantic_map_node semantic_map_msgs_generate_messages_cpp quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp)
+#add_dependencies(semantic_map_recompute quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp)
 
  target_link_libraries(semantic_map
    ${catkin_LIBRARIES}
@@ -115,6 +117,14 @@ add_dependencies(semantic_map_node semantic_map_generate_messages_cpp primitive_
    semantic_map
   )
 
+ #target_link_libraries(semantic_map_recompute
+ #  ${catkin_LIBRARIES}
+ #  ${PCL_LIBRARIES}
+ #  ${QT_LIBRARIES}
+ #  ${Boost_LIBRARIES}
+ #  semantic_map
+ # )
+
  target_link_libraries(load_from_mongo
    ${catkin_LIBRARIES}
    ${PCL_LIBRARIES}
@@ -133,7 +143,7 @@ add_dependencies(semantic_map_node semantic_map_generate_messages_cpp primitive_
 
 ############################# INSTALL TARGETS
 
-install(TARGETS semantic_map semantic_map_node load_from_mongo
+install(TARGETS semantic_map semantic_map_node load_from_mongo #semantic_map_recompute
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/semantic_map/include/semantic_map/room.h b/semantic_map/include/semantic_map/room.h
index fd4ed897..7f855362 100644
--- a/semantic_map/include/semantic_map/room.h
+++ b/semantic_map/include/semantic_map/room.h
@@ -95,6 +95,7 @@ class SemanticRoom : public RoomBase<PointType>{
     bool getSaveIntermediateClouds();
     void setSaveIntermediateClouds(bool saveIntermediate);
     auto getIntermediateClouds() -> decltype (m_vIntermediateRoomClouds);
+	void setIntermediateClouds(std::vector< CloudPtr > clouds_);
 
     void clearIntermediateCloudRegisteredTransforms();
     void addIntermediateRoomCloudRegisteredTransform(tf::StampedTransform cloud_reg_tf);
diff --git a/semantic_map/include/semantic_map/room.hpp b/semantic_map/include/semantic_map/room.hpp
index 6edf8cb3..237f074c 100644
--- a/semantic_map/include/semantic_map/room.hpp
+++ b/semantic_map/include/semantic_map/room.hpp
@@ -166,6 +166,38 @@ auto SemanticRoom<PointType>::getIntermediateClouds() -> decltype(m_vIntermediat
 {
     return m_vIntermediateRoomClouds;
 }
+/*
+typedef pcl::PointCloud<PointType> Cloud;
+typedef typename Cloud::Ptr CloudPtr;
+
+struct IntermediatePositionImages
+{
+	std::vector<cv::Mat>                vIntermediateDepthImages;
+	std::vector<cv::Mat>                vIntermediateRGBImages;
+	tf::StampedTransform                intermediateDepthTransform;
+	tf::StampedTransform                intermediateRGBTransform;
+	image_geometry::PinholeCameraModel  intermediateRGBCamParams;
+	image_geometry::PinholeCameraModel  intermediateDepthCamParams;
+	int                                 numRGBImages, numDepthImages;
+	bool                                images_loaded;
+};
+
+private:
+
+CloudPtr                                         m_DynamicClustersCloud;
+bool                                             m_DynamicClustersLoaded;
+std::string                                      m_DynamicClustersFilename;
+// intermediate room clouds
+std::vector<CloudPtr>                            m_vIntermediateRoomClouds;
+*/
+
+template <class PointType>
+void SemanticRoom<PointType>::setIntermediateClouds(std::vector< CloudPtr > clouds_)
+{
+	printf("setIntermediateClouds\n");
+	m_vIntermediateRoomClouds = clouds_;
+}
+//std::vector<CloudPtr>                            m_vIntermediateRoomClouds;
 
 template <class PointType>
 void SemanticRoom<PointType>::addIntermediateRoomCloudRegisteredTransform(tf::StampedTransform cloud_reg_tf)
diff --git a/semantic_map/include/semantic_map/room_xml_parser.hpp b/semantic_map/include/semantic_map/room_xml_parser.hpp
index fb699a90..bc198d3d 100644
--- a/semantic_map/include/semantic_map/room_xml_parser.hpp
+++ b/semantic_map/include/semantic_map/room_xml_parser.hpp
@@ -55,6 +55,7 @@ bool SemanticRoomXMLParser<PointType>::setRootFolderFromRoomXml(std::string room
 template <class PointType>
 std::string SemanticRoomXMLParser<PointType>::saveRoomAsXML(SemanticRoom<PointType>& aRoom, std::string xmlFile, bool verbose )
 {    
+	printf("saveRoomAsXML\n");
     // create folder structure
     QString roomLogName(aRoom.getRoomLogName().c_str());
     int index = roomLogName.indexOf('_');
@@ -135,15 +136,15 @@ std::string SemanticRoomXMLParser<PointType>::saveRoomAsXML(SemanticRoom<PointTy
     QString cloudFilename("complete_cloud.pcd");
     QString completeCloudFilename = roomFolder + cloudFilename; // add the folder prefix
     xmlWriter->writeAttribute("filename",cloudFilename);
-    xmlWriter->writeEndElement();
+	xmlWriter->writeEndElement();
     if (aRoom.getCompleteRoomCloudLoaded()) // only save the cloud file if it's been loaded
-    {
+	{
         QFile file(completeCloudFilename);
 //        if (!file.exists())
-        {
+		{
             if (aRoom.getCompleteRoomCloud()->points.size()>0)
-            {
-                pcl::io::savePCDFileBinary(completeCloudFilename.toStdString(), *aRoom.getCompleteRoomCloud());
+			{
+				pcl::io::savePCDFileBinary(completeCloudFilename.toStdString(), *aRoom.getCompleteRoomCloud());
                 if (verbose)
                 {
                     ROS_INFO_STREAM("Saving complete cloud file name "<<completeCloudFilename.toStdString());
@@ -165,7 +166,7 @@ std::string SemanticRoomXMLParser<PointType>::saveRoomAsXML(SemanticRoom<PointTy
         {
             if (aRoom.getInteriorRoomCloud()->points.size()>0)
             {
-                pcl::io::savePCDFileBinary(interiorCloudFilename.toStdString(), *aRoom.getInteriorRoomCloud());
+				pcl::io::savePCDFileBinary(interiorCloudFilename.toStdString(), *aRoom.getInteriorRoomCloud());
                 if (verbose)
                 {
                     ROS_INFO_STREAM("Saving interior cloud file name "<<interiorCloudFilename.toStdString());
@@ -188,7 +189,7 @@ std::string SemanticRoomXMLParser<PointType>::saveRoomAsXML(SemanticRoom<PointTy
         {
             if (aRoom.getDeNoisedRoomCloud()->points.size())
             {
-                pcl::io::savePCDFileBinary(denoisedCloudFilename.toStdString(), *aRoom.getDeNoisedRoomCloud());
+				pcl::io::savePCDFileBinary(denoisedCloudFilename.toStdString(), *aRoom.getDeNoisedRoomCloud());
                 if (verbose)
                 {
                     ROS_INFO_STREAM("Saving denoised cloud file name "<<denoisedCloudFilename.toStdString());
@@ -202,11 +203,15 @@ std::string SemanticRoomXMLParser<PointType>::saveRoomAsXML(SemanticRoom<PointTy
     QString dynamicClustersFilename = roomFolder + dynamicClustersFilenameLocal; // add the folder prefix
     QFile dynamicClustersFile(dynamicClustersFilename);
 
+	//
+
+
     if (aRoom.getDynamicClustersCloudLoaded() && aRoom.getDynamicClustersCloud()->points.size()) // only save the cloud file if it's been loaded
     {
+		printf("there are dynamic clusters found\n");
         //            if (!dynamicClustersFile.exists())
         //            {
-        pcl::io::savePCDFileBinary(dynamicClustersFilename.toStdString(), *aRoom.getDynamicClustersCloud());
+		pcl::io::savePCDFileBinary(dynamicClustersFilename.toStdString(), *aRoom.getDynamicClustersCloud());
 
         if (verbose)
         {
@@ -218,6 +223,7 @@ std::string SemanticRoomXMLParser<PointType>::saveRoomAsXML(SemanticRoom<PointTy
         xmlWriter->writeAttribute("filename",dynamicClustersFilenameLocal);
         xmlWriter->writeEndElement();
     } else {
+		printf("no dynamic clusters found\n");
         if (dynamicClustersFile.exists())
         {
             xmlWriter->writeStartElement("RoomDynamicClusters");
@@ -271,7 +277,7 @@ std::string SemanticRoomXMLParser<PointType>::saveRoomAsXML(SemanticRoom<PointTy
             ss << "intermediate_cloud"<<std::setfill('0')<<std::setw(4)<<i<<".pcd";
             intermediateCloudLocalPath = ss.str().c_str();
             intermediateCloudPath = roomFolder + intermediateCloudLocalPath;
-        }
+		}
         xmlWriter->writeAttribute("filename",intermediateCloudLocalPath);
 
         if(roomIntermediateCloudsLoaded[i] && aRoom.getSaveIntermediateClouds())
@@ -279,7 +285,7 @@ std::string SemanticRoomXMLParser<PointType>::saveRoomAsXML(SemanticRoom<PointTy
             QFile file(intermediateCloudPath);
             if (!file.exists())
             {
-                pcl::io::savePCDFileBinary(intermediateCloudPath.toStdString(), *roomIntermediateClouds[i]);
+				pcl::io::savePCDFileBinary(intermediateCloudPath.toStdString(), *roomIntermediateClouds[i]);
                 if (verbose)
                 {
                     ROS_INFO_STREAM("Saving intermediate cloud file name "<<intermediateCloudPath.toStdString());
diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h
index 7076b4f6..0601282f 100644
--- a/semantic_map/include/semantic_map/semantic_map_node.h
+++ b/semantic_map/include/semantic_map/semantic_map_node.h
@@ -14,16 +14,19 @@
 
 #include <mongodb_store/message_store.h>
 
-#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/PointCloud2.h> 
 #include <sensor_msgs/image_encodings.h>
 
 // Services
-#include <semantic_map/ClearMetaroomService.h>
+#include <semantic_map_msgs/ClearMetaroomService.h>
 #include <observation_registration_services/ObservationRegistrationService.h>
 //#include <semantic_map/DynamicClusterService.h>
 //#include <semantic_map/ObservationService.h>
 
-#include <semantic_map/RoomObservation.h>
+
+//#include "object_manager/dynamic_object.h"
+
+#include <semantic_map_msgs/RoomObservation.h>
 
 // PCL includes
 #include <pcl_ros/point_cloud.h>
@@ -45,6 +48,16 @@
 #include "reg_transforms.h"
 #include "room_utilities.h"
 
+#include "quasimodo_msgs/model.h"
+#include "quasimodo_msgs/rgbd_frame.h"
+#include "quasimodo_msgs/model_from_frame.h"
+#include "quasimodo_msgs/index_frame.h"
+#include "quasimodo_msgs/fuse_models.h"
+#include "quasimodo_msgs/get_model.h"
+#include "quasimodo_msgs/segment_model.h"
+#include "quasimodo_msgs/metaroom_pair.h"
+#include <cv_bridge/cv_bridge.h>
+
 
 
 template <class PointType>
@@ -59,15 +72,15 @@ class SemanticMapNode {
     typedef typename std::map<std::string, SemanticRoom<PointType> >::iterator WaypointRoomMapIterator;
     typedef typename std::map<std::string, CloudPtr>::iterator WaypointPointCloudMapIterator;
 
-    typedef typename semantic_map::ClearMetaroomService::Request ClearMetaroomServiceRequest;
-    typedef typename semantic_map::ClearMetaroomService::Response ClearMetaroomServiceResponse;
+    typedef typename semantic_map_msgs::ClearMetaroomService::Request ClearMetaroomServiceRequest;
+    typedef typename semantic_map_msgs::ClearMetaroomService::Response ClearMetaroomServiceResponse;
 
     SemanticMapNode(ros::NodeHandle nh);
     ~SemanticMapNode();
 
     void processRoomObservation(std::string xml_file_name);
 
-    void roomObservationCallback(const semantic_map::RoomObservationConstPtr& obs_msg);
+    void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg);
     bool clearMetaroomServiceCallback(ClearMetaroomServiceRequest &req, ClearMetaroomServiceResponse &res);
 
     ros::Subscriber                                                             m_SubscriberRoomObservation;
@@ -76,6 +89,7 @@ class SemanticMapNode {
     ros::Publisher                                                              m_PublisherDynamicClusters;
     ros::Publisher                                                              m_PublisherStatus;
     ros::ServiceServer                                                          m_ClearMetaroomServiceServer;
+    ros::ServiceClient                                                          m_segmentation_client;
 
 private:
 
@@ -83,7 +97,7 @@ class SemanticMapNode {
     SemanticMapSummaryParser                                         m_SummaryParser;
     bool                                                                        m_bSaveIntermediateData;
     std::vector<boost::shared_ptr<MetaRoom<PointType> > >                       m_vLoadedMetarooms;
-    mongodb_store::MessageStoreProxy                                           m_messageStore;
+    //mongodb_store::MessageStoreProxy                                           m_messageStore;
     bool                                                                        m_bLogToDB;
     std::map<std::string, boost::shared_ptr<MetaRoom<PointType> > >             m_WaypointToMetaroomMap;
     std::map<std::string, SemanticRoom<PointType> >                             m_WaypointToRoomMap;
@@ -92,15 +106,18 @@ class SemanticMapNode {
     bool                                                                        m_bNewestClusters;
     bool                                                                        m_bUseNDTRegistration;
 	int									m_MinObjectSize;
+    int segmentationtype;
 
 };
 
 template <class PointType>
-SemanticMapNode<PointType>::SemanticMapNode(ros::NodeHandle nh) : m_messageStore(nh)
+SemanticMapNode<PointType>::SemanticMapNode(ros::NodeHandle nh)// : m_messageStore(nh)
 {
+    segmentationtype = 1;
     ROS_INFO_STREAM("Semantic map node initialized");
     m_NodeHandle = nh;
 
+    m_segmentation_client = nh.serviceClient<quasimodo_msgs::metaroom_pair>("segment_metaroom");
     m_SubscriberRoomObservation = m_NodeHandle.subscribe("/local_metric_map/room_observations",1, &SemanticMapNode::roomObservationCallback,this);
 
     m_PublisherMetaroom = m_NodeHandle.advertise<sensor_msgs::PointCloud2>("/local_metric_map/metaroom", 1, true);
@@ -119,6 +136,7 @@ SemanticMapNode<PointType>::SemanticMapNode(ros::NodeHandle nh) : m_messageStore
     m_NodeHandle.param<bool>("log_to_db",m_bLogToDB,false);
     if (m_bLogToDB)
     {
+        //m_messageStore = mongodb_store::MessageStoreProxy(nh);
         ROS_INFO_STREAM("Logging dynamic clusters to the database.");
     } else {
         ROS_INFO_STREAM("NOT logging dynamic clusters to the database.");
@@ -150,7 +168,7 @@ SemanticMapNode<PointType>::SemanticMapNode(ros::NodeHandle nh) : m_messageStore
 //    }
 
 
-    m_NodeHandle.param<int>("min_object_size",m_MinObjectSize,500);
+	m_NodeHandle.param<int>("min_object_size",m_MinObjectSize,500);
     ROS_INFO_STREAM("Min object size set to"<<m_MinObjectSize);
 
     std::string statusTopic;
@@ -171,6 +189,16 @@ void SemanticMapNode<PointType>::processRoomObservation(std::string xml_file_nam
 {
     std::cout<<"File name "<<xml_file_name<<std::endl;
 
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("%i\n",__LINE__);
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("//////////////////////////////////////////////////////////\n");
+
+
+
     if ( ! boost::filesystem::exists( xml_file_name ) )
     {
         ROS_ERROR_STREAM("Xml file does not exist. Aborting.");
@@ -282,7 +310,123 @@ void SemanticMapNode<PointType>::processRoomObservation(std::string xml_file_nam
         meta_parser.saveMetaRoomAsXML(*metaroom);
     }
 
-    CloudPtr difference(new Cloud()); // this stores the difference between a) current obs and previous obs or b) current obs and metaroom (depending on the node parameters)
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("%i\n",__LINE__);
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("//////////////////////////////////////////////////////////\n");
+	printf("//////////////////////////////////////////////////////////\n");
+	usleep(10*1000*1000);
+
+	CloudPtr dynamicClusters(new Cloud());
+	CloudPtr difference(new Cloud());
+    if(segmentationtype == 1){
+        std::string previousObservationXml;
+        passwd* pw = getpwuid(getuid());
+        std::string dataPath(pw->pw_dir);
+        dataPath+="/.semanticMap";
+
+        std::vector<std::string> matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint<PointType>(dataPath, aRoom.getRoomStringId());
+        if (matchingObservations.size() == 0) // no observations -> first observation
+        {
+            ROS_INFO_STREAM("No observations for this waypoint saved on the disk "+aRoom.getRoomStringId()+" cannot compare with previous observation.");
+            std_msgs::String msg;
+            msg.data = "finished_processing_observation";
+            m_PublisherStatus.publish(msg);
+            return;
+        }
+        if (matchingObservations.size() == 1) // first observation at this waypoint
+        {
+            ROS_INFO_STREAM("No dynamic clusters.");
+            std_msgs::String msg;
+            msg.data = "finished_processing_observation";
+            m_PublisherStatus.publish(msg);
+            return;
+        }
+
+        int stopind = 0;
+
+        for(unsigned int i = 0; i < matchingObservations.size(); i++){
+            if(matchingObservations[i].compare(xml_file_name) == 0){
+                break;
+            }else{
+                stopind = i;//latest = matchingObservations[i];
+            }
+        }
+
+        if(stopind == 0) // first observation at this waypoint
+        {
+            ROS_INFO_STREAM("No dynamic clusters.");
+            std_msgs::String msg;
+            msg.data = "finished_processing_observation";
+            m_PublisherStatus.publish(msg);
+            return;
+        }
+        previousObservationXml = matchingObservations[stopind];
+        printf("prev: %s\n",previousObservationXml.c_str());
+
+		quasimodo_msgs::metaroom_pair sm;
+		sm.request.background = previousObservationXml;
+		sm.request.foreground = xml_file_name;
+
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("%i\n",__LINE__);
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("//////////////////////////////////////////////////////////\n");
+
+		usleep(10*1000*1000);
+		if (m_segmentation_client.call(sm)){
+			CloudPtr dynamiccloud(new Cloud());
+			std::vector<cv::Mat> dynamicmasks;
+			for(unsigned int i = 0; i < sm.response.dynamicmasks.size(); i++){
+				cv_bridge::CvImagePtr			img_ptr;
+				try{							img_ptr = cv_bridge::toCvCopy(sm.response.dynamicmasks[i], sensor_msgs::image_encodings::MONO8);}
+				catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}
+				cv::Mat mask = img_ptr->image;
+				dynamicmasks.push_back(mask);
+
+				CloudPtr currentcloud = aRoom.getIntermediateClouds()[i];
+				CloudPtr currentcloudTMP(new Cloud());
+				aRoom.getIntermediateCloudTransformsRegistered()[i];
+				//pcl::transformPointCloud (*currentcloud, *currentcloudTMP, aRoom.m_vIntermediateRoomCloudTransformsRegistered[i]);
+
+				pcl_ros::transformPointCloud(*currentcloud, *currentcloudTMP,aRoom.getIntermediateCloudTransformsRegistered()[i]);
+				unsigned char * maskdata = mask.data;
+				for(unsigned int j = 0; j < currentcloudTMP->points.size(); j++){
+					if(maskdata[j] > 0){
+						//difference->points.push_back(currentcloudTMP->points[j]);
+						dynamicClusters->points.push_back(currentcloudTMP->points[j]);
+					}
+				}
+			}
+
+
+//			std::vector<CloudPtr> vClusters = MetaRoom<PointType>::clusterPointCloud(dynamiccloud,0.03,100,1000000);
+//			ROS_INFO_STREAM("Clustered differences. "<<vClusters.size()<<" different clusters.");
+		}else{
+			ROS_ERROR("Failed to call service segment_model");
+			std_msgs::String msg;
+			msg.data = "error_processing_observation";
+			m_PublisherStatus.publish(msg);
+			return;
+		}
+
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("%i\n",__LINE__);
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("//////////////////////////////////////////////////////////\n");
+		printf("//////////////////////////////////////////////////////////\n");
+		usleep(10*1000*1000);
+
+    }else{
+
+	// this stores the difference between a) current obs and previous obs or b) current obs and metaroom (depending on the node parameters)
     if (m_bNewestClusters){
          // computing the latest clusters by comparison with the previous observation -> register with the previous observation
         // look for the previous observation in ~/.semanticMap/
@@ -429,10 +573,10 @@ void SemanticMapNode<PointType>::processRoomObservation(std::string xml_file_nam
             ROS_INFO_STREAM("Comparison cloud "<<metaroomCloud->points.size()<<"  room cloud "<<room_interior_cloud->points.size());
 
     }
-
+//}
     ROS_INFO_STREAM("Raw difference "<<difference->points.size());
 
-
+/*
     if (difference->points.size() == 0)
     {
         // -> no dynamic clusters can be computed
@@ -442,39 +586,48 @@ void SemanticMapNode<PointType>::processRoomObservation(std::string xml_file_nam
         m_PublisherStatus.publish(msg);
         return;
     }
-
+*/
 
     std::vector<CloudPtr> vClusters = MetaRoom<PointType>::clusterPointCloud(difference,0.03,m_MinObjectSize,100000);
     ROS_INFO_STREAM("Clustered differences. "<<vClusters.size()<<" different clusters.");
-    MetaRoom<PointType>::filterClustersBasedOnDistance(aRoom.getIntermediateCloudTransforms()[0].getOrigin(), vClusters,4.0);
-    ROS_INFO_STREAM(vClusters.size()<<" different clusters after max distance filtering.");
+	MetaRoom<PointType>::filterClustersBasedOnDistance(aRoom.getIntermediateCloudTransforms()[0].getOrigin(), vClusters,4.0);
+	ROS_INFO_STREAM(vClusters.size()<<" different clusters after max distance filtering.");
 
     ROS_INFO_STREAM("Clustered differences. "<<vClusters.size()<<" different clusters.");
 
     // Check cluster planarity and discard the absolutely planar ones (usually parts of walls, floor, ceiling).
-    CloudPtr dynamicClusters(new Cloud());
-    for (size_t i=0; i<vClusters.size(); i++)
-    {
-        pcl::SACSegmentation<PointType> seg;
-        pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
-        pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
-
-        seg.setOptimizeCoefficients (true);
-        seg.setModelType (pcl::SACMODEL_PLANE);
-        seg.setMethodType (pcl::SAC_RANSAC);
-        seg.setMaxIterations (100);
-        seg.setDistanceThreshold (0.02);
-
-        seg.setInputCloud (vClusters[i]);
-        seg.segment (*inliers, *coefficients);
-        if (inliers->indices.size () > 0.9 * vClusters[i]->points.size())
-        {
-            ROS_INFO_STREAM("Discarding planar dynamic cluster");
-        } else {
-            *dynamicClusters += *vClusters[i];
-        }
-    }
+	//CloudPtr dynamicClusters(new Cloud());
+	/*
+	for (size_t i=0; i<vClusters.size(); i++)
+	{
+		pcl::SACSegmentation<PointType> seg;
+		pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+
+		seg.setOptimizeCoefficients (true);
+		seg.setModelType (pcl::SACMODEL_PLANE);
+		seg.setMethodType (pcl::SAC_RANSAC);
+		seg.setMaxIterations (100);
+		seg.setDistanceThreshold (0.02);
+
+		seg.setInputCloud (vClusters[i]);
+		seg.segment (*inliers, *coefficients);
+		if (inliers->indices.size () > 0.9 * vClusters[i]->points.size())
+		{
+			ROS_INFO_STREAM("Discarding planar dynamic cluster");
+		} else {
+			*dynamicClusters += *vClusters[i];
+		}
+	}
+*/
+
+	for (size_t i=0; i<vClusters.size(); i++)
+	{
+		*dynamicClusters += *vClusters[i];
+	}
+}
 
+	printf("dynamicClusters->points.size() = %i\n",dynamicClusters->points.size());
     // publish dynamic clusters
     // transform back into the sweep frame of ref (before metaroom registration, to align with the previously published sweep point cloud)
     Eigen::Matrix4f inverseRoomTransform = aRoom.getRoomTransform().inverse();
@@ -496,32 +649,34 @@ void SemanticMapNode<PointType>::processRoomObservation(std::string xml_file_nam
         ROS_INFO_STREAM("Updated map with new dynamic clusters for waypoint "<<aRoom.getRoomStringId());
     }
 
-    if (m_bLogToDB)
-    {
-        QString databaseName = QString(aRoom.getRoomLogName().c_str()) + QString("/room_")+ QString::number(aRoom.getRoomRunNumber()) +QString("/dynamic_clusters");
-        std::string id(m_messageStore.insertNamed(databaseName.toStdString(), msg_clusters));
-        ROS_INFO_STREAM("Dynamic clusters \""<<databaseName.toStdString()<<"\" inserted with id "<<id);
-
-        std::vector< boost::shared_ptr<sensor_msgs::PointCloud2> > results;
-        if(m_messageStore.queryNamed<sensor_msgs::PointCloud2>(databaseName.toStdString(), results)) {
+//    if (m_bLogToDB)
+//    {
+//        QString databaseName = QString(aRoom.getRoomLogName().c_str()) + QString("/room_")+ QString::number(aRoom.getRoomRunNumber()) +QString("/dynamic_clusters");
+//        std::string id(m_messageStore.insertNamed(databaseName.toStdString(), msg_clusters));
+//        ROS_INFO_STREAM("Dynamic clusters \""<<databaseName.toStdString()<<"\" inserted with id "<<id);
+
+//        std::vector< boost::shared_ptr<sensor_msgs::PointCloud2> > results;
+//        if(m_messageStore.queryNamed<sensor_msgs::PointCloud2>(databaseName.toStdString(), results)) {
+
+//            BOOST_FOREACH( boost::shared_ptr<sensor_msgs::PointCloud2> p,  results)
+//            {
+//                CloudPtr databaseCloud(new Cloud());
+//                pcl::fromROSMsg(*p,*databaseCloud);
+//                ROS_INFO_STREAM("Got pointcloud by name. No points: " << databaseCloud->points.size());
+//            }
+//        }
+//    }
 
-            BOOST_FOREACH( boost::shared_ptr<sensor_msgs::PointCloud2> p,  results)
-            {
-                CloudPtr databaseCloud(new Cloud());
-                pcl::fromROSMsg(*p,*databaseCloud);
-                ROS_INFO_STREAM("Got pointcloud by name. No points: " << databaseCloud->points.size());
-            }
-        }
-    }
 
     std_msgs::String msg;
     msg.data = "finished_processing_observation";
     m_PublisherStatus.publish(msg);
+
     return;
 }
 
 template <class PointType>
-void SemanticMapNode<PointType>::roomObservationCallback(const semantic_map::RoomObservationConstPtr& obs_msg)
+void SemanticMapNode<PointType>::roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg)
 {
     std::cout<<"Room obs message received"<<std::endl;
     this->processRoomObservation(obs_msg->xml_file_name);
diff --git a/semantic_map/launch/semantic_map.launch b/semantic_map/launch/semantic_map.launch
index f420ae33..7b27d1bc 100644
--- a/semantic_map/launch/semantic_map.launch
+++ b/semantic_map/launch/semantic_map.launch
@@ -9,7 +9,7 @@
 
   <arg name="user"   	default="" />
   <arg name="newest_dynamic_clusters" default="false" />
- 
+
   <machine name="$(arg machine)" address="$(arg machine)" env-loader="$(optenv ROS_ENV_LOADER )" user="$(arg user)"/>
 
   <node machine="$(arg machine)" pkg="semantic_map" type="semantic_map_node" name="semantic_map_node" output="screen" respawn="true">
diff --git a/semantic_map/package.xml b/semantic_map/package.xml
index 080dcfe0..df218cbd 100644
--- a/semantic_map/package.xml
+++ b/semantic_map/package.xml
@@ -28,8 +28,10 @@
   <build_depend>strands_sweep_registration</build_depend>
   <build_depend>metaroom_xml_parser</build_depend>
   <build_depend>observation_registration_services</build_depend>
+  <build_depend>semantic_map_msgs</build_depend>
 
 
+  <run_depend>semantic_map_msgs</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
diff --git a/semantic_map/src/reg_transforms.cpp b/semantic_map/src/reg_transforms.cpp
index bcd79e29..8bdcca3e 100644
--- a/semantic_map/src/reg_transforms.cpp
+++ b/semantic_map/src/reg_transforms.cpp
@@ -372,18 +372,23 @@ std::vector<tf::StampedTransform> semantic_map_registration_transforms::loadCorr
                                                                           std::string sweep_params_file,
                                                                           bool verbose)
 {
+if(false){printf("%s::%i\n",__FILE__,__LINE__);}
     // load calibrated data
     std::vector<tf::StampedTransform> allTransforms = semantic_map_registration_transforms::loadRegistrationTransforms(transforms_file, verbose);
+if(false){printf("%s::%i\n",__FILE__,__LINE__);}
     std::vector<tf::StampedTransform> empty;
     std::vector<tf::StampedTransform> toRet;
     SweepParameters calibrate_sweep_parameters;
+if(false){printf("%s::%i\n",__FILE__,__LINE__);}
     semantic_map_registration_transforms::loadSweepParameters(calibrate_sweep_parameters, verbose, sweep_params_file);
-
+if(false){printf("%s::%i\n",__FILE__,__LINE__);}
     // find matching transforms for current sweep parameters from calibrate sweep parameters
     int number_of_positions = my_sweep_params.getNumberOfIntermediatePositions();
     for (int k=0; k<number_of_positions; k++){
+if(false){printf("%s::%i\n",__FILE__,__LINE__);}
         int corresp_pos;
         my_sweep_params.findCorrespondingPosition(calibrate_sweep_parameters,k, corresp_pos);
+if(false){printf("%s::%i\n",__FILE__,__LINE__);}
         if (corresp_pos == -1){
             cerr<<"Error while finding corresponding registered transform. Sweep parameters: "<<my_sweep_params<<endl;
             return empty; // could not find a corresponding position
@@ -391,14 +396,17 @@ std::vector<tf::StampedTransform> semantic_map_registration_transforms::loadCorr
         int my_pan, my_tilt, their_pan, their_tilt;
         my_sweep_params.getAnglesForPosition(my_pan, my_tilt, k);
         calibrate_sweep_parameters.getAnglesForPosition(their_pan, their_tilt, corresp_pos);
+if(false){printf("%s::%i\n",__FILE__,__LINE__);}
         if ((my_pan != their_pan) ||(my_tilt != their_tilt)){
             cerr<<"Error while finding corresponding registered transform. Sweep parameters: "<<my_sweep_params<<endl;
             cerr<<"Calibrated sweep parameters "<<calibrate_sweep_parameters<<endl;
             cerr<<"Current position "<<k<<". Sweep angles "<<my_pan<<" "<<my_tilt<<". Calibrated sweep angles "<<their_pan<<" "<<their_tilt<<endl;
             return empty;
         }
+if(false){printf("%s::%i\n",__FILE__,__LINE__);}
         toRet.push_back(allTransforms[corresp_pos]); // add the corresponding transform
     }
+    if(false){printf("%s::%i\n",__FILE__,__LINE__);}
 
     return toRet;
 }
diff --git a/semantic_map/src/semantic_map_main.cpp b/semantic_map/src/semantic_map_main.cpp
index 2a3dd1fd..2f3082db 100644
--- a/semantic_map/src/semantic_map_main.cpp
+++ b/semantic_map/src/semantic_map_main.cpp
@@ -2,13 +2,12 @@
 
 int main(int argc, char **argv)
 {
-    // Set up ROS.
     ros::init(argc, argv, "Semantic_map_node");
     ros::NodeHandle n;
 
     ros::NodeHandle aRosNode("~");
-
-    SemanticMapNode<pcl::PointXYZRGB> aSemanticMapNode(aRosNode);
+	SemanticMapNode<pcl::PointXYZRGB> aSemanticMapNode(n);
+    aSemanticMapNode.processRoomObservation(argv[1]);
 
     ros::Rate loop_rate(10);
     while (ros::ok())
diff --git a/semantic_map/src/semantic_map_recompute.cpp b/semantic_map/src/semantic_map_recompute.cpp
new file mode 100644
index 00000000..37623ab9
--- /dev/null
+++ b/semantic_map/src/semantic_map_recompute.cpp
@@ -0,0 +1,392 @@
+#include "semantic_map/semantic_map_node.h"
+
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
+
+
+#include "Util/Util.h"
+
+typedef pcl::PointXYZRGB PointType;
+typedef pcl::PointCloud<PointType> Cloud;
+typedef typename Cloud::Ptr CloudPtr;
+typedef pcl::search::KdTree<PointType> Tree;
+typedef semantic_map_load_utilties::DynamicObjectData<PointType> ObjectData;
+
+using namespace std;
+using namespace semantic_map_load_utilties;
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+
+std::string getPrevXML(std::string xml_file_name){
+	std::cout<<"File name "<<xml_file_name<<std::endl;
+
+	if ( ! boost::filesystem::exists( xml_file_name ) )
+	{
+		return "";
+	}
+
+	SemanticRoomXMLParser<PointType> parser;
+	SemanticRoom<PointType> aRoom = SemanticRoomXMLParser<PointType>::loadRoomFromXML(xml_file_name,true);
+	//	    aRoom.resetRoomTransform();
+	//	    CloudPtr room_complete_cloud = aRoom.getCompleteRoomCloud();
+
+	//	    // update summary xml
+	//	    m_SummaryParser.createSummaryXML<PointType>();
+
+	//	    // update list of rooms & metarooms
+	//	    m_SummaryParser.refresh();
+
+	ROS_INFO_STREAM("Summary XML created. Looking for metaroom for room with id: "<<aRoom.getRoomStringId());
+	return "";
+//	    boost::shared_ptr<MetaRoom<PointType> > metaroom;
+//	    bool found = false;
+
+//	    std::string matchingMetaroomXML = "";
+//	    // first check if the matching metaroom has already been loaded
+//	    for (size_t i=0; i<m_vLoadedMetarooms.size(); i++)
+//	    {
+//	        // compare by waypoint, if set
+//	        if (aRoom.getRoomStringId() != "")
+//	        {
+//	            if (aRoom.getRoomStringId() == m_vLoadedMetarooms[i]->m_sMetaroomStringId)
+//	            {
+//	                metaroom = m_vLoadedMetarooms[i];
+//	                found = true;
+//	                break;
+//	            }
+//	        } else {
+//	            // if not set, compare by centroid
+//	            double centroidDistance = pcl::distances::l2(m_vLoadedMetarooms[i]->getCentroid(),aRoom.getCentroid());
+//	            if (! (centroidDistance < ROOM_CENTROID_DISTANCE) )
+//	            {
+//	                continue;
+//	            } else {
+//	                ROS_INFO_STREAM("Matching metaroom already loaded.");
+//	                metaroom = m_vLoadedMetarooms[i];
+//	                found = true;
+//	                break;
+//	            }
+//	        }
+//	    }
+
+//	    // if not loaded already, look through already saved metarooms
+//	    if (!found)
+//	    {
+//	        std::vector<Entities> allMetarooms = m_SummaryParser.getMetaRooms();
+//	        ROS_INFO_STREAM("Loaded "<<allMetarooms.size()<<" metarooms.");
+//	        for (size_t i=0; i<allMetarooms.size();i++)
+//	        {
+//	            // compare by waypoint, if set
+//	            if (aRoom.getRoomStringId() != "")
+//	            {
+//	                if (aRoom.getRoomStringId() == allMetarooms[i].stringId)
+//	                {
+//	                    matchingMetaroomXML = allMetarooms[i].roomXmlFile;
+//	                    break;
+//	                }
+//	            } else {
+//	                // if not set, compare by centroid
+//	                double centroidDistance = pcl::distances::l2(allMetarooms[i].centroid,aRoom.getCentroid());
+//	                if (! (centroidDistance < ROOM_CENTROID_DISTANCE) )
+//	                {
+//	                    continue;
+//	                } else {
+//	                    matchingMetaroomXML = allMetarooms[i].roomXmlFile;
+//	                    break;
+//	                }
+//	            }
+//	        }
+
+//	        if (matchingMetaroomXML == "")
+//	        {
+//	            ROS_INFO_STREAM("No matching metaroom found. Create new metaroom.");
+//	            metaroom =  boost::shared_ptr<MetaRoom<PointType> >(new MetaRoom<PointType>());
+//	            metaroom->m_sMetaroomStringId = aRoom.getRoomStringId(); // set waypoint, to figure out where to save
+
+//	        } else {
+//	            ROS_INFO_STREAM("Matching metaroom found. XML file: "<<matchingMetaroomXML);
+//	            metaroom =  MetaRoomXMLParser<PointType>::loadMetaRoomFromXML(matchingMetaroomXML,true);
+//	            found = true;
+//	        }
+//	    } else {
+//	        MetaRoomXMLParser<PointType> meta_parser;
+//	        matchingMetaroomXML= meta_parser.findMetaRoomLocation(metaroom.get()).toStdString();
+//	        matchingMetaroomXML+="/metaroom.xml";
+//	    }
+
+//	    if (!found)
+//	    {
+//	        ROS_INFO_STREAM("Initializing metaroom.");
+//	        metaroom->setUpdateMetaroom(m_bUpdateMetaroom);
+//	        metaroom->setSaveIntermediateSteps(m_bSaveIntermediateData);
+//	        // also update the metaroom here
+//	        auto updateIteration = metaroom->updateMetaRoom(aRoom,"",true);
+//	        // save metaroom
+//	        ROS_INFO_STREAM("Saving metaroom.");
+//	        MetaRoomXMLParser<PointType> meta_parser;
+//	        meta_parser.saveMetaRoomAsXML(*metaroom);
+//	    }
+
+//		CloudPtr difference(new Cloud());
+//	    if(segmentationtype == 1){
+//	        std::string previousObservationXml;
+//	        passwd* pw = getpwuid(getuid());
+//	        std::string dataPath(pw->pw_dir);
+//	        dataPath+="/.semanticMap";
+
+//	        std::vector<std::string> matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint<PointType>(dataPath, aRoom.getRoomStringId());
+//	        if (matchingObservations.size() == 0) // no observations -> first observation
+//	        {
+//	            ROS_INFO_STREAM("No observations for this waypoint saved on the disk "+aRoom.getRoomStringId()+" cannot compare with previous observation.");
+//	            std_msgs::String msg;
+//	            msg.data = "finished_processing_observation";
+//	            m_PublisherStatus.publish(msg);
+//	            return;
+//	        }
+//	        if (matchingObservations.size() == 1) // first observation at this waypoint
+//	        {
+//	            ROS_INFO_STREAM("No dynamic clusters.");
+//	            std_msgs::String msg;
+//	            msg.data = "finished_processing_observation";
+//	            m_PublisherStatus.publish(msg);
+//	            return;
+//	        }
+
+//	        int stopind = 0;
+
+//	        for(unsigned int i = 0; i < matchingObservations.size(); i++){
+//	            if(matchingObservations[i].compare(xml_file_name) == 0){
+//	                break;
+//	            }else{
+//	                stopind = i;//latest = matchingObservations[i];
+//	            }
+//	        }
+
+//	        if(stopind == 0) // first observation at this waypoint
+//	        {
+//	            ROS_INFO_STREAM("No dynamic clusters.");
+//	            std_msgs::String msg;
+//	            msg.data = "finished_processing_observation";
+//	            m_PublisherStatus.publish(msg);
+//	            return;
+//	        }
+//	        previousObservationXml = matchingObservations[stopind];
+//	        printf("prev: %s\n",previousObservationXml.c_str());
+
+//			quasimodo_msgs::metaroom_pair sm;
+//			sm.request.background = previousObservationXml;
+//			sm.request.foreground = xml_file_name;
+
+//			if (m_segmentation_client.call(sm)){
+//				CloudPtr dynamiccloud(new Cloud());
+//				std::vector<cv::Mat> dynamicmasks;
+//				for(unsigned int i = 0; i < sm.response.dynamicmasks.size(); i++){
+//					cv_bridge::CvImagePtr			img_ptr;
+//					try{							img_ptr = cv_bridge::toCvCopy(sm.response.dynamicmasks[i], sensor_msgs::image_encodings::MONO8);}
+//					catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}
+//					cv::Mat mask = img_ptr->image;
+//					dynamicmasks.push_back(mask);
+
+//					CloudPtr currentcloud = aRoom.getIntermediateClouds()[i];
+//					CloudPtr currentcloudTMP(new Cloud());
+//					aRoom.getIntermediateCloudTransformsRegistered()[i];
+//					//pcl::transformPointCloud (*currentcloud, *currentcloudTMP, aRoom.m_vIntermediateRoomCloudTransformsRegistered[i]);
+
+//					pcl_ros::transformPointCloud(*currentcloud, *currentcloudTMP,aRoom.getIntermediateCloudTransformsRegistered()[i]);
+//					unsigned char * maskdata = mask.data;
+//					for(unsigned int j = 0; j < currentcloudTMP->points.size(); j++){
+//						if(maskdata[j] > 0){
+//							difference->points.push_back(currentcloudTMP->points[j]);
+//						}
+//					}
+//				}
+
+//	//			std::vector<CloudPtr> vClusters = MetaRoom<PointType>::clusterPointCloud(dynamiccloud,0.03,100,1000000);
+//	//			ROS_INFO_STREAM("Clustered differences. "<<vClusters.size()<<" different clusters.");
+//			}else{
+//				ROS_ERROR("Failed to call service segment_model");
+//				std_msgs::String msg;
+//				msg.data = "error_processing_observation";
+//				m_PublisherStatus.publish(msg);
+//				return;
+//			}
+}
+
+
+
+std::string outtopic = "/some/topic";
+ros::Publisher out_pub;
+void processMetaroom(std::string path){
+    printf("processing: %s\n",path.c_str());
+
+    int slash_pos = path.find_last_of("/");
+    std::string sweep_folder = path.substr(0, slash_pos) + "/";
+
+    std::vector<cv::Mat> viewrgbs;
+    std::vector<cv::Mat> viewdepths;
+    std::vector<tf::StampedTransform > viewtfs;
+
+    QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml"));
+    for (auto objectFile : objectFiles){
+        auto object = loadDynamicObjectFromSingleSweep<PointType>(sweep_folder+objectFile.toStdString(),false);
+        for (unsigned int i=0; i<object.vAdditionalViews.size(); i++){
+
+            CloudPtr cloud = object.vAdditionalViews[i];
+
+            cv::Mat rgb;
+            rgb.create(cloud->height,cloud->width,CV_8UC3);
+            unsigned char * rgbdata = (unsigned char *)rgb.data;
+
+            cv::Mat depth;
+            depth.create(cloud->height,cloud->width,CV_16UC1);
+            unsigned short * depthdata = (unsigned short *)depth.data;
+
+            unsigned int nr_data = cloud->height * cloud->width;
+            for(unsigned int j = 0; j < nr_data; j++){
+                PointType p = cloud->points[j];
+                rgbdata[3*j+0]	= p.b;
+                rgbdata[3*j+1]	= p.g;
+                rgbdata[3*j+2]	= p.r;
+                depthdata[j]	= short(5000.0 * p.z);
+            }
+
+            viewrgbs.push_back(rgb);
+            viewdepths.push_back(depth);
+            viewtfs.push_back(object.vAdditionalViewsTransforms[i]);
+            //viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front());
+
+            cv::namedWindow("rgbimage",     cv::WINDOW_AUTOSIZE);
+            cv::imshow(		"rgbimage",     rgb);
+            cv::namedWindow("depthimage",	cv::WINDOW_AUTOSIZE);
+            cv::imshow(		"depthimage",	depth);
+            cv::waitKey(30);
+
+        }
+    }
+
+	reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path);
+
+    std::vector<Eigen::Matrix4d> unrefined;
+    std::vector<double> times;
+    for(unsigned int i = 0; i < viewrgbs.size(); i++){
+        geometry_msgs::TransformStamped msg;
+        tf::transformStampedTFToMsg(viewtfs[i], msg);
+        long sec = msg.header.stamp.sec;
+        long nsec = msg.header.stamp.nsec;
+        double time = double(sec)+1e-9*double(nsec);
+
+        Eigen::Quaterniond q;
+        q.w() = msg.transform.rotation.w;
+        q.x() = msg.transform.rotation.x;
+        q.y() = msg.transform.rotation.y;
+        q.z() = msg.transform.rotation.z;
+
+        Eigen::Affine3d a (q);
+        a(0,3) = msg.transform.translation.x;
+        a(1,3) = msg.transform.translation.y;
+        a(2,3) = msg.transform.translation.z;
+
+
+        unrefined.push_back(a.matrix());
+        times.push_back(time);
+
+    }
+
+//    xmlWriter->writeStartElement("Transform");
+//    xmlWriter->writeStartElement("Translation");
+//    xmlWriter->writeStartElement("x");
+//    xmlWriter->writeCharacters(QString::number(msg.transform.translation.x));
+//    xmlWriter->writeEndElement();
+//    xmlWriter->writeStartElement("y");
+//    xmlWriter->writeCharacters(QString::number(msg.transform.translation.y));
+//    xmlWriter->writeEndElement();
+//    xmlWriter->writeStartElement("z");
+//    xmlWriter->writeCharacters(QString::number(msg.transform.translation.z));
+//    xmlWriter->writeEndElement();
+//    xmlWriter->writeEndElement(); // Translation
+//    xmlWriter->writeStartElement("Rotation");
+//    xmlWriter->writeStartElement("w");
+//    xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w));
+//    xmlWriter->writeEndElement();
+//    xmlWriter->writeStartElement("x");
+//    xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x));
+//    xmlWriter->writeEndElement();
+//    xmlWriter->writeStartElement("y");
+//    xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y));
+//    xmlWriter->writeEndElement();
+//    xmlWriter->writeStartElement("z");
+//    xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z));
+
+    std_msgs::String msg;
+    msg.data = path;
+    out_pub.publish(msg);
+    ros::spinOnce();
+}
+
+void chatterCallback(const std_msgs::String::ConstPtr& msg){processMetaroom(msg->data);}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "metaroom_additional_view_processing");
+    ros::NodeHandle n;
+
+
+    int inputstate = 0;
+    for(int i = 1; i < argc;i++){
+        printf("input: %s\n",argv[i]);
+        if(std::string(argv[i]).compare("-file") == 0){inputstate = 2;}
+        else if(std::string(argv[i]).compare("-intopic") == 0){inputstate = 0;}
+        else if(std::string(argv[i]).compare("-outtopic") == 0){inputstate = 1;}
+        else if(inputstate == 0){
+            out_pub = n.advertise<std_msgs::String>(outtopic, 1000);
+            ros::Subscriber sub = n.subscribe(std::string(argv[i]), 1000, chatterCallback);
+            ros::spin();
+        }else if(inputstate == 1){
+            outtopic = std::string(argv[i]);
+        }else{
+
+            out_pub = n.advertise<std_msgs::String>(outtopic, 1000);
+            processMetaroom(std::string(argv[i]));
+        }
+    }
+
+//    std::string listentopic = "/some/stupid/topic";
+//    if(argc == 2){listentopic = std::string(argv[1]);}
+//    else{printf("robot listener: incorrect number of args. Should take a single arguement with a topicname!\n");}
+
+
+
+//	for(int ar = 1; ar < argc; ar++){
+//		std::string overall_folder = std::string(argv[ar]);
+//		printf("overall folder: %s\n",overall_folder.c_str());
+
+//		std::vector<std::string> sweep_xmls = semantic_map_load_utilties::getSweepXmls<pcl::PointXYZRGB>(overall_folder);
+//		for (int i = 0; i < sweep_xmls.size(); i++){//auto sweep_xml : sweep_xmls) {
+//			printf("current xml: %s\n",sweep_xmls[i].c_str());
+//			fixXml(sweep_xmls[i]);
+//			if(i > 1){
+//				segmentWithAdditionalViewsXml(sweep_xmls[i-1],sweep_xmls[i]);
+//			}
+//		}
+//	}
+
+//	printf("test\n");
+//	exit(0);
+//	// Set up ROS.
+//	ros::init(argc, argv, "Semantic_map_node");
+//	ros::NodeHandle n;
+
+//	ros::NodeHandle aRosNode("~");
+//	printf("test\n");
+//	//SemanticMapNode<pcl::PointXYZRGB> aSemanticMapNode(aRosNode);
+//	SemanticMapNode<pcl::PointXYZRGB> aSemanticMapNode(n);
+//	aSemanticMapNode.processRoomObservation(argv[1]);
+//	exit(0);
+//	printf("test\n");
+//	ros::Rate loop_rate(10);
+//	while (ros::ok())
+//	{
+//		ros::spinOnce();
+//		loop_rate.sleep();
+//	}
+}
diff --git a/semantic_map_launcher/launch/semantic_map.launch b/semantic_map_launcher/launch/semantic_map.launch
index f2e26a42..93f04261 100644
--- a/semantic_map_launcher/launch/semantic_map.launch
+++ b/semantic_map_launcher/launch/semantic_map.launch
@@ -38,25 +38,18 @@
     </include>
 
     <!-- The system doing the analysis of static vs dynamic parts of the local maps -->
-    
-    <include file="$(find semantic_map)/launch/semantic_map.launch"> 
-        <arg name="min_object_size" value="$(arg min_object_size)"/>
-        <arg name="update_metaroom" value="$(arg update_metaroom)"/>
-    	<arg name="machine" value="$(arg mapping_ip)"/>
-	<arg name="user" value="$(arg mapping_user)" />  
-	<arg name="newest_dynamic_clusters" value="$(arg newest_dynamic_clusters)" />  
-	<!--<arg name="use_NDT_registration" value="$(arg use_NDT_registration)" />  -->
-    </include>
 
     <node machine="$(arg machine)" pkg="calibrate_sweeps" type="calibrate_sweep_as" name="calibrate_sweep_as" output="screen" respawn="true"/>
     
     <node machine="$(arg machine)" pkg="semantic_map_publisher" type="semantic_map_publisher" name="semantic_map_publisher" output="screen" respawn="true"/>
     
+    <!--
     <node machine="$(arg machine)" pkg="object_manager" type="object_manager_node" name="object_manager_node" output="screen" respawn="true">
         <param name="log_objects_to_db" value="$(arg log_objects_to_db)"/>
         <param name="min_object_size" value="$(arg min_object_size)"/>
         <param name="save_mask" value="$(arg save_mask)"/>
     </node>
+    -->
 
         <node name="do_sweep" pkg="cloud_merge" type="do_sweep.py" output="screen" respawn="true">
         </node>
diff --git a/semantic_map_msgs/CMakeLists.txt b/semantic_map_msgs/CMakeLists.txt
new file mode 100644
index 00000000..ab3a39fd
--- /dev/null
+++ b/semantic_map_msgs/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(semantic_map_msgs)
+
+find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs geometry_msgs message_generation)
+
+add_message_files(
+  FILES
+  RoomObservation.msg
+)
+
+add_service_files(
+   FILES
+   ClearMetaroomService.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  sensor_msgs
+)
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES semantic_map_msgs
+  CATKIN_DEPENDS message_runtime
+#  DEPENDS system_lib
+)
diff --git a/semantic_map_msgs/README.md b/semantic_map_msgs/README.md
new file mode 100644
index 00000000..e69de29b
diff --git a/semantic_map/msg/RoomObservation.msg b/semantic_map_msgs/msg/RoomObservation.msg
similarity index 100%
rename from semantic_map/msg/RoomObservation.msg
rename to semantic_map_msgs/msg/RoomObservation.msg
diff --git a/semantic_map_msgs/package.xml b/semantic_map_msgs/package.xml
new file mode 100644
index 00000000..b1f38924
--- /dev/null
+++ b/semantic_map_msgs/package.xml
@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<package>
+  <name>semantic_map_msgs</name>
+  <version>0.0.0</version>
+  <description>The semantic map msgs package</description>
+
+  <maintainer email="ekz@kth.se">Johan Ekekrantz</maintainer>
+
+
+  <license>MIT</license>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>libqt4-dev</build_depend>
+  <build_depend>qt_build</build_depend>
+  <build_depend>qt_ros</build_depend>
+  <build_depend>mongodb_store</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>image_geometry</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>cv_bridge</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>libqt4-dev</run_depend>
+  <run_depend>qt_build</run_depend>
+  <run_depend>qt_ros</run_depend>
+  <run_depend>mongodb_store</run_depend>
+  <run_depend>libpcl-all</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>tf_conversions</run_depend>
+  <run_depend>image_geometry</run_depend>
+  
+  <export>
+  </export>
+</package>
diff --git a/semantic_map/srv/ClearMetaroomService.srv b/semantic_map_msgs/srv/ClearMetaroomService.srv
similarity index 100%
rename from semantic_map/srv/ClearMetaroomService.srv
rename to semantic_map_msgs/srv/ClearMetaroomService.srv
diff --git a/semantic_map/srv/DynamicClusterService.srv b/semantic_map_msgs/srv/DynamicClusterService.srv
similarity index 100%
rename from semantic_map/srv/DynamicClusterService.srv
rename to semantic_map_msgs/srv/DynamicClusterService.srv
diff --git a/semantic_map/srv/MetaroomService.srv b/semantic_map_msgs/srv/MetaroomService.srv
similarity index 100%
rename from semantic_map/srv/MetaroomService.srv
rename to semantic_map_msgs/srv/MetaroomService.srv
diff --git a/semantic_map/srv/ObservationService.srv b/semantic_map_msgs/srv/ObservationService.srv
similarity index 100%
rename from semantic_map/srv/ObservationService.srv
rename to semantic_map_msgs/srv/ObservationService.srv
diff --git a/strands_sweep_registration/include/strands_sweep_registration/Camera.h b/strands_sweep_registration/include/strands_sweep_registration/Camera.h
index e8bbcde6..702561a4 100644
--- a/strands_sweep_registration/include/strands_sweep_registration/Camera.h
+++ b/strands_sweep_registration/include/strands_sweep_registration/Camera.h
@@ -20,10 +20,18 @@ class Camera {
 		int version;
 		
 		PixelFunction ** pixelsFunctions;
+
+		unsigned int coefs_degree;
+		unsigned int coefs_width;
+		unsigned int coefs_height;
+		double * multiplierCoeffs;
 		
 		Camera(	float fx_, float fy_, float cx_, float cy_,	int width_,	int height_);
 		virtual ~Camera();
-		
+
+		virtual double * getCoeffs(unsigned int w, unsigned int h);
+		virtual double getGridMultiplier(unsigned int w, unsigned int h, double z);
+		virtual double getMultiplier(double w, double h, double z);
 		virtual void save(std::string path);
 		virtual void load(std::string path);
 		virtual void show();
diff --git a/strands_sweep_registration/include/strands_sweep_registration/Frame.h b/strands_sweep_registration/include/strands_sweep_registration/Frame.h
index 68e337ff..1bff8978 100644
--- a/strands_sweep_registration/include/strands_sweep_registration/Frame.h
+++ b/strands_sweep_registration/include/strands_sweep_registration/Frame.h
@@ -29,6 +29,7 @@ class Frame
 
 	std::vector<Eigen::Vector3f> keypoint_location;
 
+	Frame(Camera * camera_);
 	Frame(Camera * camera_, float * rgb_data_, float * depth_data_);
     Frame(Camera * camera_, std::vector<cv::KeyPoint> k, std::vector<double> depth, cv::Mat d); // initialize from ORB features driectly
 	~Frame();
diff --git a/strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h b/strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h
index 9f53d6f3..77302b1b 100644
--- a/strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h
+++ b/strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h
@@ -15,10 +15,12 @@ class ProblemFrameConnection {
 
 	std::vector< Eigen::Vector3f > full_src_points;
 	std::vector< Eigen::Vector3f > full_dst_points;
+	std::vector< float > full_noisemul;
 
 
 	std::vector< Eigen::Vector3f > src_points;
 	std::vector< Eigen::Vector3f > dst_points;
+	std::vector< float > noisemul;
 
 	std::vector< float > possible_matches_fdistance;
 	std::vector< float > possible_matches_edistance;
@@ -29,7 +31,7 @@ class ProblemFrameConnection {
 	std::vector<int> dst_matches;
 
 	ProblemFrameConnection(ceres::Problem & problem, Frame * src_, Frame * dst_, double * shared_params, double * src_variable_, double * dst_variable_, float weight = 1, bool show = false);
-	void addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions);
+	void addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions, double hubersize = 0.1);
 	void addMatchesToProblem(ceres::Problem & problem, float weight = 1);
 	void findPossibleMatches(float di = 1, float pi = 0, Eigen::Matrix4f pose = Eigen::Matrix4f::Identity());
 	void recalculatePoints();
diff --git a/strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h b/strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h
index da6e8ce6..63690178 100644
--- a/strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h
+++ b/strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h
@@ -10,6 +10,8 @@
 #include "ProblemFrameConnection.h"
 
 #include <pcl/common/transformation_from_correspondences.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl_ros/transforms.h>
 
 class RobotContainer
 {
@@ -35,6 +37,12 @@ class RobotContainer
 	std::vector<Sweep *> sweeps;
 	std::vector<bool> alignedSweep;
 
+
+	std::vector<std::string> paths;
+
+	bool viewer_initialized;
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+
 	RobotContainer(unsigned int gx_,unsigned int todox_,unsigned int gy_,unsigned int todoy_);
 	~RobotContainer();
 
@@ -51,6 +59,8 @@ class RobotContainer
 
     void saveAllSweeps(std::string path);
 
+	void show();
+
 private:
     void saveSweep(Sweep*, std::string path);
 
diff --git a/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h b/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h
index 9aae4815..238ce2cd 100644
--- a/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h
+++ b/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h
@@ -22,6 +22,7 @@ using ceres::Solve;
 
 class pair3DError : public SizedCostFunction<3, 6, 6, 4> {
 	public:
+    static bool optimizeCameraParams;
 	pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight);
 	~pair3DError();
 	bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const ;
@@ -34,7 +35,7 @@ class pair3DError : public SizedCostFunction<3, 6, 6, 4> {
 	double dh;
 	double dz;
 	double weight;
-    bool optimizeCameraParams;
+    //bool optimizeCameraParams;
     double information;
 };
 #endif
diff --git a/strands_sweep_registration/src/Camera.cpp b/strands_sweep_registration/src/Camera.cpp
index b253d042..d80e140e 100644
--- a/strands_sweep_registration/src/Camera.cpp
+++ b/strands_sweep_registration/src/Camera.cpp
@@ -1,4 +1,4 @@
-#include "strands_sweep_registration/Camera.h"
+#include "../include/strands_sweep_registration/Camera.h"
 #include <fstream>
 #include "opencv2/highgui/highgui.hpp"
 #include "opencv/cv.hpp"
@@ -17,16 +17,57 @@ Camera::Camera(float fx_, float fy_, float cx_, float cy_,	int width_,	int heigh
 	for(int i = 0; i < width*height; i++){
 		pixelsFunctions[i] = new PixelFunction(); 
 	}
-	
+
 	version = 2;
+
+//	coefs_degree = 1;
+//	coefs_width = width/40;
+//	coefs_height = height/40;
+
+
+//	multiplierCoeffs = new double[coefs_degree*coefs_width*coefs_height];
+//	for(int w = 0; w < width; w++){
+//		for(int h = 0; h < height; h++){
+//			double * coeffs = getCoeffs(w,h);
+//			coeffs[0] = 1.0;
+//			for(unsigned int i = 1; i < coefs_degree; i++){
+//				coeffs[i] = 0.0;
+//			}
+//		}
+//	}
+
+	//getMultiplier(55, 55, 1);
 };
 
 Camera::~Camera(){
+	//delete[] multiplierCoeffs;
+
 	for(int i = 0; i < width*height; i++){
 		delete pixelsFunctions[i]; 
 	}
 	delete[] pixelsFunctions;
-};
+}
+
+double * Camera::getCoeffs(unsigned int w, unsigned int h){
+	return multiplierCoeffs + coefs_degree*(coefs_width*h + w);
+}
+
+double Camera::getGridMultiplier(unsigned int w, unsigned int h, double z){
+	double * coeffs = getCoeffs(w,h);
+	return 0;
+}
+
+double Camera::getMultiplier(double w, double h, double z){
+	double wgrid = w/(coefs_width+1);
+	double hgrid = h/(coefs_height+1);
+
+	double pw = wgrid-double(int(wgrid));
+	double ph = hgrid-double(int(hgrid));
+
+	printf("%f %f -> %f %f -> %f %f\n",w,h,wgrid,hgrid,pw,ph);
+exit(0);
+	return 1;
+}
 
 void Camera::save(std::string path){
 	std::vector<char *> pixeldata_char;
diff --git a/strands_sweep_registration/src/Frame.cpp b/strands_sweep_registration/src/Frame.cpp
index c8364170..125e73d2 100644
--- a/strands_sweep_registration/src/Frame.cpp
+++ b/strands_sweep_registration/src/Frame.cpp
@@ -1,4 +1,4 @@
-#include "strands_sweep_registration/Frame.h"
+#include "../include/strands_sweep_registration/Frame.h"
 #include <iostream>
 #include <fstream>
 #include <string>
@@ -15,6 +15,13 @@
 
 int frame_id = 0;
 
+Frame::Frame(Camera * camera_){
+	id = frame_id;
+	frame_id++;
+	camera = camera_;
+	featuretype = 0;
+}
+
 Frame::Frame(Camera * camera_, float * rgb_data_, float * depth_data_){
 	id = frame_id;
 	frame_id++;
@@ -133,6 +140,7 @@ void Frame::recalculateFullPoints(){
 	float centerY		= camera->cy;
 	float invFocalX		= 1.0f/camera->fx;
     float invFocalY		= 1.0f/camera->fy;
+	//keypoint_location.clear(); //We should probably clear this....
 	for(unsigned int i = 0; i < keypoints.size(); i++){
 		float w = keypoints.at(i).pt.x;
 		float h = keypoints.at(i).pt.y;
diff --git a/strands_sweep_registration/src/PixelFunction.cpp b/strands_sweep_registration/src/PixelFunction.cpp
index 83f4c09a..b04a17c6 100644
--- a/strands_sweep_registration/src/PixelFunction.cpp
+++ b/strands_sweep_registration/src/PixelFunction.cpp
@@ -1,4 +1,4 @@
-#include "strands_sweep_registration/PixelFunction.h"
+#include "../include/strands_sweep_registration/PixelFunction.h"
 #include <iostream>
 #include <fstream>
 #include <string>
diff --git a/strands_sweep_registration/src/ProblemFrameConnection.cpp b/strands_sweep_registration/src/ProblemFrameConnection.cpp
index 84c5b006..759be5f2 100644
--- a/strands_sweep_registration/src/ProblemFrameConnection.cpp
+++ b/strands_sweep_registration/src/ProblemFrameConnection.cpp
@@ -1,4 +1,4 @@
-#include "strands_sweep_registration/ProblemFrameConnection.h"
+#include "../include/strands_sweep_registration/ProblemFrameConnection.h"
 
 ProblemFrameConnection::ProblemFrameConnection(ceres::Problem & problem, Frame * src_, Frame * dst_, double * shared_params, double * src_variable_, double * dst_variable_, float weight, bool show){
 	src = src_;
@@ -13,9 +13,11 @@ ProblemFrameConnection::ProblemFrameConnection(ceres::Problem & problem, Frame *
 	recalculatePoints();
 }
 
-void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions){
+void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions, double hubersize){
+	//printf("addMatchesToProblem: %i\n",costfunctions.size());
 	for(unsigned int i = 0; i < costfunctions.size() && i < 1000; i++ ){
-		problem.AddResidualBlock(costfunctions.at(i), 0 , src_variable, dst_variable, params);
+		//problem.AddResidualBlock(costfunctions.at(i), 0 , src_variable, dst_variable, params);
+		problem.AddResidualBlock(costfunctions.at(i), new ceres::HuberLoss(hubersize) , src_variable, dst_variable, params);
 	}
 }
 
@@ -30,8 +32,10 @@ void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, float
 		double sz	= src->keypoint_depth.at(src_kp_id);
 		double dz	= dst->keypoint_depth.at(dst_kp_id);			
 
-		CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight);
-		problem.AddResidualBlock(err, NULL, src_variable, dst_variable, params);
+        //CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight/pow(sz*sz+dz*dz,2));
+        CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight);
+        problem.AddResidualBlock(err, 0, src_variable, dst_variable, params);
+		//problem.AddResidualBlock(err, new ceres::HuberLoss(0.1), src_variable, dst_variable, params);
 	}
 }
 
@@ -163,6 +167,7 @@ void ProblemFrameConnection::findPossibleMatches(float di, float pi, Eigen::Matr
 void ProblemFrameConnection::recalculatePoints(){
 	src_points.clear();
 	dst_points.clear();
+	noisemul.clear();
 	
 	for(unsigned int i = 0; i < src_possible_matches_id.size(); i++){
 		int src_kp_id = src_possible_matches_id.at(i);
@@ -182,6 +187,10 @@ void ProblemFrameConnection::recalculatePoints(){
 
 		src_points.push_back(Eigen::Vector3f(sx,sy,sz));
 		dst_points.push_back(Eigen::Vector3f(dx,dy,dz));
+		double snoise = sz*sz;
+		double dnoise = dz*dz;
+		double noise = sqrt( snoise*snoise + dnoise*dnoise );
+		noisemul.push_back( noise );
 	}
 }
 
diff --git a/strands_sweep_registration/src/RobotContainer.cpp b/strands_sweep_registration/src/RobotContainer.cpp
index 41a27a37..9d5290f5 100644
--- a/strands_sweep_registration/src/RobotContainer.cpp
+++ b/strands_sweep_registration/src/RobotContainer.cpp
@@ -1,10 +1,10 @@
 #include <tf_conversions/tf_eigen.h>
 #include <metaroom_xml_parser/load_utilities.h>
 
-#include "strands_sweep_registration/pair3DError.h"
-#include "strands_sweep_registration/RobotContainer.h"
+#include "../include/strands_sweep_registration/pair3DError.h"
+#include "../include/strands_sweep_registration/RobotContainer.h"
 
-#include "strands_sweep_registration/camera_parameters.h"
+#include "../include/strands_sweep_registration/camera_parameters.h"
 
 typedef pcl::PointXYZRGB PointType;
 typedef typename SimpleSummaryParser::EntityStruct Entities;
@@ -12,530 +12,789 @@ typedef typename SimpleSummaryParser::EntityStruct Entities;
 
 void RobotContainer::saveAllSweeps(std::string savePath)
 {
-    for (auto sweep: sweeps)
-    {
-        saveSweep(sweep, savePath);
-    }
+	for (auto sweep: sweeps)
+	{
+		saveSweep(sweep, savePath);
+	}
 }
 
 void RobotContainer::saveSweep(Sweep * sweep, std::string savePath){//std::vector<tf::StampedTransform> transforms)
-   /* std::string sweep_xml = sweep->xmlpath;
-    std::vector<Eigen::Matrix4f> poses = sweep->getPoseVector();
-
-    std::cout<<"Sweep xml "<<sweep_xml<<std::endl;
-    std::cout<<"No poses "<<poses.size()<<std::endl;
-
-    auto room = SemanticRoomXMLParser<PointType>::loadRoomFromXML(sweep_xml, true);
-    room.clearIntermediateCloudRegisteredTransforms();
-    room.clearIntermediateCloudCameraParametersCorrected();
-    auto original_transforms = room.getIntermediateCloudTransforms();
-    auto intClouds = room.getIntermediateClouds();
-    auto original_params = room.getIntermediateCloudCameraParameters();
-
-    static tf::StampedTransform firstTransform = original_transforms[0];
-
-    Camera * camera = sweep->frames[0][0]->camera;
-    for (unsigned int i=0; i<poses.size(); i++){
-        Eigen::Matrix4f pose = poses[i];
-        // add reg transform and corrected camera parameters to sweep
-        auto transform = original_transforms[i];
-        tf::Transform tfTr;
-        const Eigen::Affine3d eigenTr(pose.cast<double>());
-        tf::transformEigenToTF(eigenTr, tfTr);
-        tf::Transform combinedTransform = firstTransform * tfTr;
-
-        sensor_msgs::CameraInfo camInfo;
-        camInfo.P = {camera->fx, 0.0, camera->cx, 0.0, 0.0, camera->fy, camera->cy, 0.0,0.0, 0.0, 1.0,0.0};
-        camInfo.D = {0,0,0,0,0};
-        image_geometry::PinholeCameraModel aCameraModel;
-        aCameraModel.fromCameraInfo(camInfo);
-        room.addIntermediateCloudCameraParametersCorrected(aCameraModel);
-
-        transform.setOrigin(tfTr.getOrigin());
-        transform.setBasis(tfTr.getBasis());
-        room.addIntermediateRoomCloudRegisteredTransform(transform);
-    }
-
-    SemanticRoomXMLParser<PointType> parser(savePath);
-    parser.saveRoomAsXML(room);
-    */
+	/* std::string sweep_xml = sweep->xmlpath;
+	std::vector<Eigen::Matrix4f> poses = sweep->getPoseVector();
+
+	std::cout<<"Sweep xml "<<sweep_xml<<std::endl;
+	std::cout<<"No poses "<<poses.size()<<std::endl;
+
+	auto room = SemanticRoomXMLParser<PointType>::loadRoomFromXML(sweep_xml, true);
+	room.clearIntermediateCloudRegisteredTransforms();
+	room.clearIntermediateCloudCameraParametersCorrected();
+	auto original_transforms = room.getIntermediateCloudTransforms();
+	auto intClouds = room.getIntermediateClouds();
+	auto original_params = room.getIntermediateCloudCameraParameters();
+
+	static tf::StampedTransform firstTransform = original_transforms[0];
+
+	Camera * camera = sweep->frames[0][0]->camera;
+	for (unsigned int i=0; i<poses.size(); i++){
+		Eigen::Matrix4f pose = poses[i];
+		// add reg transform and corrected camera parameters to sweep
+		auto transform = original_transforms[i];
+		tf::Transform tfTr;
+		const Eigen::Affine3d eigenTr(pose.cast<double>());
+		tf::transformEigenToTF(eigenTr, tfTr);
+		tf::Transform combinedTransform = firstTransform * tfTr;
+
+		sensor_msgs::CameraInfo camInfo;
+		camInfo.P = {camera->fx, 0.0, camera->cx, 0.0, 0.0, camera->fy, camera->cy, 0.0,0.0, 0.0, 1.0,0.0};
+		camInfo.D = {0,0,0,0,0};
+		image_geometry::PinholeCameraModel aCameraModel;
+		aCameraModel.fromCameraInfo(camInfo);
+		room.addIntermediateCloudCameraParametersCorrected(aCameraModel);
+
+		transform.setOrigin(tfTr.getOrigin());
+		transform.setBasis(tfTr.getBasis());
+		room.addIntermediateRoomCloudRegisteredTransform(transform);
+	}
+
+	SemanticRoomXMLParser<PointType> parser(savePath);
+	parser.saveRoomAsXML(room);
+	*/
 }
 
 RobotContainer::RobotContainer(unsigned int gx_,unsigned int todox_,unsigned int gy_,unsigned int todoy_){
-    gx = gx_;
-    todox = todox_;
-    gy = gy_;
-    todoy = todoy_;
-
-    inds = new unsigned int*[todox];
-    for(unsigned int x = 0; x < todox; x++){inds[x] = new unsigned int[todoy];}
-
-    for(unsigned int y = 0; y < gy; y++){//Camera moving forward
-        for (unsigned int x = 0; x < gx ; x++){inds[x][y] = 0;}
-        if(y % 2 == 0){
-            for (unsigned int x = 0; x < gx ; x++){inds[x][y] = y*gx+x;}
-        }else{
-            for (unsigned int x = 0; x < gx ; x++){inds[x][y] = y*gx+gx-x-1;}
-        }
-    }
-
-    poses = new double**[todox];
-    for(unsigned int x = 0; x < todox; x++){
-        poses[x] = new double*[todoy];
-        for(unsigned int y = 0; y < todoy; y++){
-            poses[x][y] = new double[6];
-            for(unsigned int k = 0; k < 6; k++){poses[x][y][k] = 0;}
-        }
-    }
-
-    width = 0;
-    height = 0;
-
-    shared_params	= new double[5];
-    camera			= 0;
-    rgb				= 0;
-    depth			= 0;
+	gx = gx_;
+	todox = todox_;
+	gy = gy_;
+	todoy = todoy_;
+
+	inds = new unsigned int*[todox];
+	for(unsigned int x = 0; x < todox; x++){inds[x] = new unsigned int[todoy];}
+
+	for(unsigned int y = 0; y < gy; y++){//Camera moving forward
+		for (unsigned int x = 0; x < gx ; x++){inds[x][y] = 0;}
+		if(y % 2 == 0){
+			for (unsigned int x = 0; x < gx ; x++){inds[x][y] = y*gx+x;}
+		}else{
+			for (unsigned int x = 0; x < gx ; x++){inds[x][y] = y*gx+gx-x-1;}
+		}
+	}
+
+	poses = new double**[todox];
+	for(unsigned int x = 0; x < todox; x++){
+		poses[x] = new double*[todoy];
+		for(unsigned int y = 0; y < todoy; y++){
+			poses[x][y] = new double[6];
+			for(unsigned int k = 0; k < 6; k++){poses[x][y][k] = 0;}
+		}
+	}
+
+	width = 0;
+	height = 0;
+
+	shared_params	= new double[5];
+	camera			= 0;
+	rgb				= 0;
+	depth			= 0;
+
+	viewer_initialized = false;
 }
 
 RobotContainer::~RobotContainer(){
-    for(unsigned int x = 0; x < todox; x++){delete[] inds[x];}
-    delete[] inds;
-
-    for(unsigned int s = 0; s < sweeps.size(); s++){delete sweeps.at(s);}
-    if(camera != 0){		delete camera;}
-    if(shared_params != 0){	delete shared_params;}
-    if(rgb != 0){			delete rgb;}
-    if(depth != 0){			delete depth;}
-
-    for(unsigned int x = 0; x < todox; x++){
-        for(unsigned int y = 0; y < todoy; y++){
-            delete[] poses[x][y];
-        }
-        delete[] poses[x];
-    }
-    delete[] poses;
+	for(unsigned int x = 0; x < todox; x++){delete[] inds[x];}
+	delete[] inds;
+
+	for(unsigned int s = 0; s < sweeps.size(); s++){delete sweeps.at(s);}
+	if(camera != 0){		delete camera;}
+	if(shared_params != 0){	delete shared_params;}
+	if(rgb != 0){			delete rgb;}
+	if(depth != 0){			delete depth;}
+
+	for(unsigned int x = 0; x < todox; x++){
+		for(unsigned int y = 0; y < todoy; y++){
+			delete[] poses[x][y];
+		}
+		delete[] poses[x];
+	}
+	delete[] poses;
 };
 
 void RobotContainer::initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h)
 {
-    std::cout<<"Initializing camera with parameters "<<fx<<"  "<<fy<<"  "<<cx<<"  "<<cy<<"  "<<w<<"  "<<h<<std::endl;
-    width = w;
-    height = h;
+	std::cout<<"Initializing camera with parameters "<<fx<<"  "<<fy<<"  "<<cx<<"  "<<cy<<"  "<<w<<"  "<<h<<std::endl;
+	width = w;
+	height = h;
 
-    if (camera != 0)
-    {
-        delete camera;
-    }
+	if (camera != 0)
+	{
+		delete camera;
+	}
 
-    camera = new Camera(fx, fy, cx, cy, width,	height);
-    camera->version = 1;
+	camera = new Camera(fx, fy, cx, cy, width,	height);
+	camera->version = 1;
 
-    rgb		=	new float[3*width*height];
-    depth	=	new float[	width*height];
+	rgb		=	new float[3*width*height];
+	depth	=	new float[	width*height];
 
-    shared_params[0] = 1.0/fx;		//invfx
-    shared_params[1] = 1.0/fy;		//invfy
-    shared_params[2] = cx;
-    shared_params[3] = cy;
-    shared_params[4] = 0.1;
+	shared_params[0] = 1.0/fx;		//invfx
+	shared_params[1] = 1.0/fy;		//invfy
+	shared_params[2] = cx;
+	shared_params[3] = cy;
+	shared_params[4] = 0.1;
 
-    // initialize singleton class
-    CameraParameters::get(fx, fy, cx, cy ,w, h);
+	// initialize singleton class
+	CameraParameters::get(fx, fy, cx, cy ,w, h);
 }
 
 bool RobotContainer::addToTrainingORBFeatures(std::string path)
 {
-    std::cout<<"Adding ORB features to training from sweep "<<path<<std::endl;
-
-    //    typedef semantic_map_registration_features::RegistrationFeatures RegFeatures;
-
-    //    // load precomputed orb features
-    std::vector<semantic_map_registration_features::RegistrationFeatures> features = semantic_map_registration_features::loadRegistrationFeaturesFromSingleSweep(path, false);
-    if (features.size() == 0)
-    {
-        return false;
-    }
-
-    std::vector<Frame *> sweep_frames;
-    sweep_frames.resize(todox*todoy);
-    for(unsigned int y = 0; y < todoy; y++){
-        for (unsigned int x = 0; x < todox ; x++){
-            int index = inds[x][y];
-            sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors);
-            sweep_frames.at(y*todox+x)->framepos = inds[x][y];
-        }
-    }
-
-    sweeps.push_back(new Sweep(todox, todoy, sweep_frames));
-    alignedSweep.push_back(false);
-    sweeps.back()->xmlpath = path;
-
-    return true;
+	std::cout<<"Adding ORB features to training from sweep "<<path<<std::endl;
+	paths.push_back(path);
+	//    typedef semantic_map_registration_features::RegistrationFeatures RegFeatures;
+
+	//    // load precomputed orb features
+	std::vector<semantic_map_registration_features::RegistrationFeatures> features = semantic_map_registration_features::loadRegistrationFeaturesFromSingleSweep(path, false);
+
+	if (features.size() == 0)
+	{
+		return false;
+	}
+
+	std::vector<Frame *> sweep_frames;
+	sweep_frames.resize(todox*todoy);
+
+	if (features.size() == 51){
+		for(unsigned int y = 0; y < todoy; y++){
+			for (unsigned int x = 0; x < todox ; x++){
+				int index = inds[x][y];
+
+				//printf("index: %i %i -> %i\n",y,x,index);
+				sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors);
+				sweep_frames.at(y*todox+x)->framepos = inds[x][y];
+			}
+		}
+	} else if (features.size() == 17){
+		for(unsigned int y = 0; y < todoy; y++){
+			for (unsigned int x = 0; x < todox ; x++){
+				int index = inds[x][y];
+				if(y == 0){
+					sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors);
+				}else{
+					sweep_frames.at(y*todox+x) = new Frame(camera);
+				}
+				sweep_frames.at(y*todox+x)->framepos = inds[x][y];
+			}
+		}
+	} else if (features.size() == 9){
+		for(unsigned int y = 0; y < todoy; y++){
+			for (unsigned int x = 0; x < todox ; x++){
+				int index = inds[x][y];
+				if(y == 0 && x % 2 == 0){
+					sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors);
+				}else{
+					sweep_frames.at(y*todox+x) = new Frame(camera);
+				}
+				sweep_frames.at(y*todox+x)->framepos = inds[x][y];
+			}
+		}
+	} else if (features.size() == 6){
+		for(unsigned int y = 0; y < todoy; y++){
+			for (unsigned int x = 0; x < todox ; x++){
+				int index = inds[x][y];
+				if(y == 0 && x % 3 == 0){
+					sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors);
+				}else{
+					sweep_frames.at(y*todox+x) = new Frame(camera);
+				}
+				sweep_frames.at(y*todox+x)->framepos = inds[x][y];
+			}
+		}
+	}
+
+	sweeps.push_back(new Sweep(todox, todoy, sweep_frames));
+	alignedSweep.push_back(false);
+	sweeps.back()->xmlpath = path;
+
+	return true;
 
 }
 
 void RobotContainer::addToTraining(std::string path){
-    printf("Not supported. Add to training using precomputed ORB features. \n");
-
-/*
-
-    SimpleXMLParser<PointType> simple_parser;
-    SimpleXMLParser<PointType>::RoomData roomData = simple_parser.loadRoomFromXML(path);
-    if(roomData.vIntermediateRoomClouds.size() < todox*todoy){return;}
-
-    //	float fx = roomData.vIntermediateRoomCloudCamParams[0].fx();
-    //	float fy = roomData.vIntermediateRoomCloudCamParams[0].fy();
-    //	float cx = roomData.vIntermediateRoomCloudCamParams[0].cx();
-    //	float cy = roomData.vIntermediateRoomCloudCamParams[0].cy();
-    float fx = 540.0;//roomData.vIntermediateRoomCloudCamParams[i].fx();
-    float fy = 540.0;//roomData.vIntermediateRoomCloudCamParams[i].fy();
-    float cx = 319.5;//roomData.vIntermediateRoomCloudCamParams[i].cx();
-    float cy = 239.5;//roomData.vIntermediateRoomCloudCamParams[i].cy();
-
-    cv::Size res	= roomData.vIntermediateRoomCloudCamParams[0].fullResolution();
-    height			= res.height;
-    width			= res.width;
-
-    if(camera == 0){
-        camera = new Camera(fx, fy, cx, cy, width,	height);
-        camera->version = 1;
-        rgb		=	new float[3*width*height];
-        depth	=	new float[	width*height];
-
-        shared_params[0] = 1.0/fx;		//invfx
-        shared_params[1] = 1.0/fy;		//invfy
-        shared_params[2] = cx;
-        shared_params[3] = cy;
-        shared_params[4] = 0.1;
-    }
-
-    std::vector<Frame *> sweep_frames;
-    sweep_frames.resize(todox*todoy);
-    for(unsigned int y = 0; y < todoy; y++){
-        for (unsigned int x = 0; x < todox ; x++){
-            pcl::PointCloud<PointType>::Ptr clouddata = roomData.vIntermediateRoomClouds[inds[x][y]];
-            for(unsigned int w = 0; w < width; w++){
-                for(unsigned int h = 0; h < height; h++){
-                    unsigned int ind = h*width+w;
-                    unsigned int ind3 = 3*ind;
-                    rgb[ind3+0] = clouddata->points.at(ind).r;
-                    rgb[ind3+1] = clouddata->points.at(ind).g;
-                    rgb[ind3+2] = clouddata->points.at(ind).b;
-                    depth[ind] = clouddata->points.at(ind).z;
-                }
-            }
-            sweep_frames.at(y*todox+x) = new Frame(camera,rgb,depth);
-            sweep_frames.at(y*todox+x)->framepos = inds[x][y];
-        }
-    }
-
-    sweeps.push_back(new Sweep(todox, todoy, sweep_frames));
-    alignedSweep.push_back(false);
-    sweeps.back()->idtag = roomData.roomWaypointId;
-    sweeps.back()->xmlpath = path;
-    */
+	printf("Not supported. Add to training using precomputed ORB features. \n");
+
+	/*
+
+	SimpleXMLParser<PointType> simple_parser;
+	SimpleXMLParser<PointType>::RoomData roomData = simple_parser.loadRoomFromXML(path);
+	if(roomData.vIntermediateRoomClouds.size() < todox*todoy){return;}
+
+	//	float fx = roomData.vIntermediateRoomCloudCamParams[0].fx();
+	//	float fy = roomData.vIntermediateRoomCloudCamParams[0].fy();
+	//	float cx = roomData.vIntermediateRoomCloudCamParams[0].cx();
+	//	float cy = roomData.vIntermediateRoomCloudCamParams[0].cy();
+	float fx = 540.0;//roomData.vIntermediateRoomCloudCamParams[i].fx();
+	float fy = 540.0;//roomData.vIntermediateRoomCloudCamParams[i].fy();
+	float cx = 319.5;//roomData.vIntermediateRoomCloudCamParams[i].cx();
+	float cy = 239.5;//roomData.vIntermediateRoomCloudCamParams[i].cy();
+
+	cv::Size res	= roomData.vIntermediateRoomCloudCamParams[0].fullResolution();
+	height			= res.height;
+	width			= res.width;
+
+	if(camera == 0){
+		camera = new Camera(fx, fy, cx, cy, width,	height);
+		camera->version = 1;
+		rgb		=	new float[3*width*height];
+		depth	=	new float[	width*height];
+
+		shared_params[0] = 1.0/fx;		//invfx
+		shared_params[1] = 1.0/fy;		//invfy
+		shared_params[2] = cx;
+		shared_params[3] = cy;
+		shared_params[4] = 0.1;
+	}
+
+	std::vector<Frame *> sweep_frames;
+	sweep_frames.resize(todox*todoy);
+	for(unsigned int y = 0; y < todoy; y++){
+		for (unsigned int x = 0; x < todox ; x++){
+			pcl::PointCloud<PointType>::Ptr clouddata = roomData.vIntermediateRoomClouds[inds[x][y]];
+			for(unsigned int w = 0; w < width; w++){
+				for(unsigned int h = 0; h < height; h++){
+					unsigned int ind = h*width+w;
+					unsigned int ind3 = 3*ind;
+					rgb[ind3+0] = clouddata->points.at(ind).r;
+					rgb[ind3+1] = clouddata->points.at(ind).g;
+					rgb[ind3+2] = clouddata->points.at(ind).b;
+					depth[ind] = clouddata->points.at(ind).z;
+				}
+			}
+			sweep_frames.at(y*todox+x) = new Frame(camera,rgb,depth);
+			sweep_frames.at(y*todox+x)->framepos = inds[x][y];
+		}
+	}
+
+	sweeps.push_back(new Sweep(todox, todoy, sweep_frames));
+	alignedSweep.push_back(false);
+	sweeps.back()->idtag = roomData.roomWaypointId;
+	sweeps.back()->xmlpath = path;
+	*/
 }
 
-std::vector< CostFunction * > getMatchesRansac(std::vector< ProblemFrameConnection * > & pc_vec, float weight = 1, float threshold = 0.005, int ransac_iter = 200000, int nr_points = 3){
-    std::vector<int> owner;
-    std::vector<int> match_id;
-    std::vector< Eigen::Vector3f > src_points;
-    std::vector< Eigen::Vector3f > dst_points;
-
-    for(unsigned int i = 0; i < pc_vec.size(); i++){
-        for(unsigned int j = 0; j < pc_vec.at(i)->src_points.size(); j++){
-            owner.push_back(i);
-            match_id.push_back(j);
-            src_points.push_back(pc_vec.at(i)->src_points.at(j));
-            dst_points.push_back(pc_vec.at(i)->dst_points.at(j));
-        }
-    }
-
-    int nr_kp = src_points.size();
-    std::vector< CostFunction * > errors;
-    if(nr_kp == 0){return errors;}
-    int nr_consistent = 0;
-
-
-    Eigen::Matrix4f retpose;
-    std::vector<int> bestmatching;
-    for(int it = 0; it < ransac_iter; it++){
-        //Sample points
-        std::vector< int > indexes;
-        std::vector< Eigen::Vector3f > src_samples;
-        std::vector< Eigen::Vector3f > dst_samples;
-
-        for(int j = 0; j < nr_points; j++){
-            int ind;
-            bool failed = true;
-            while(failed){
-                ind = rand()%nr_kp;
-                failed = false;
-                for(int k = 0; k < j; k++){
-                    if(ind == indexes.at(k)){failed = true;}
-                }
-            }
-            indexes.push_back(ind);
-            src_samples.push_back(src_points.at(ind));
-            dst_samples.push_back(dst_points.at(ind));
-        }
-
-        //Check consistency
-        bool consistent = true;
-
-        for(int j = 0; j < nr_points; j++){
-            for(int k = j+1; k < nr_points; k++){
-                float src_distance = (src_samples.at(j)-src_samples.at(k)).norm();
-                float dst_distance = (dst_samples.at(j)-dst_samples.at(k)).norm();
-                if(fabs(src_distance-dst_distance) > 0.02){consistent = false; break;break;}
-            }
-        }
-
-        //Check inliers
-        nr_consistent += consistent;
-        if(consistent){
-            pcl::TransformationFromCorrespondences tfc;
-            for(int i = 0; i < nr_points; i++){tfc.add(dst_samples.at(i),src_samples.at(i),1);}
-            Eigen::Affine3f ret = tfc.getTransformation();
-
-            Eigen::Matrix4f pose = ret.matrix().inverse();
-
-            std::vector<int> matching;
-            for(int j = 0; j < nr_kp; j++){
-                Eigen::Vector3f src_data = src_points.at(j);
-                Eigen::Vector3f dst_data = dst_points.at(j);
-
-                Eigen::Vector4f sd = pose*Eigen::Vector4f(src_data(0),src_data(1),src_data(2),1);
-                Eigen::Vector4f dd = Eigen::Vector4f(dst_data(0),dst_data(1),dst_data(2),1);
-                float t = threshold;//0.1*0.005*(dst_data(2)*dst_data(2)+src_data(2)*src_data(2));
-                if((sd-dd).norm() < t){matching.push_back(j);}
-            }
-
-            //save if best
-            if(matching.size() > bestmatching.size()){
-                bestmatching = matching;
-                retpose = pose;
-                //printf("%i -> %i\n",it,matching.size());
-            }
-            if(nr_consistent == 30000){printf("break at %i\n",it);break;}
-        }
-    }
-
-    printf("nr matches best: %i / %i consistent: %i / %i\n",int(bestmatching.size()),nr_kp,nr_consistent,ransac_iter);
-
-    //getMatchesICP(pc_vec,retpose);
-
-    for(int iter = 0; iter < 200; iter++){
-        bestmatching.clear();
-        pcl::TransformationFromCorrespondences tfc;
-        int matching = 0;
-        for(int j = 0; j < nr_kp; j++){
-            Eigen::Vector3f src_data = src_points.at(j);
-            Eigen::Vector3f dst_data = dst_points.at(j);
-
-            Eigen::Vector4f sd = retpose*Eigen::Vector4f(src_data(0),src_data(1),src_data(2),1);
-            Eigen::Vector4f dd = Eigen::Vector4f(dst_data(0),dst_data(1),dst_data(2),1);
-
-            if((sd-dd).norm() < threshold){
-                bestmatching.push_back(j);
-                tfc.add(dst_data.head<3>(),src_data.head<3>(),1);
-                matching++;
-            }
-        }
-        if(iter % 50 == 0){printf("iteration: %i matches: %i\n",iter,matching);}
-
-        Eigen::Affine3f ret = tfc.getTransformation();
-        retpose = ret.matrix().inverse();
-    }
-    printf("nr matches best: %i / %i \n",int(bestmatching.size()),nr_kp);
-    //exit(0);
-    if(bestmatching.size() < 15){return errors;}
-    for(unsigned int i = 0; i < pc_vec.size(); i++){
-        pc_vec.at(i)->src_matches.clear();
-        pc_vec.at(i)->dst_matches.clear();
-    }
-
-    for(unsigned int i = 0; i < bestmatching.size() && i < 1000; i++){
-        ProblemFrameConnection * pc = pc_vec.at(owner.at(bestmatching.at(i)));
-        int id = match_id.at(bestmatching.at(i));
-        //printf("id: %i %i %i\n ",id,pc->src_matches.size(),pc->dst_matches.size());
-        int src_kp_id = pc->src_possible_matches_id.at(id);
-        int dst_kp_id = pc->dst_possible_matches_id.at(id);
-        pc->src_matches.push_back(src_kp_id);
-        pc->dst_matches.push_back(dst_kp_id);
-        cv::KeyPoint src_kp = pc->src->keypoints.at(src_kp_id);
-        cv::KeyPoint dst_kp = pc->dst->keypoints.at(dst_kp_id);
-        double sz	= pc->src->keypoint_depth.at(src_kp_id);
-        double dz	= pc->dst->keypoint_depth.at(dst_kp_id);
-        CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight);
-        errors.push_back(err);
-    }
-
-    return errors;
+std::vector< CostFunction * > getMatchesRansac(std::vector< ProblemFrameConnection * > & pc_vec, float weight = 1, float threshold = 0.015, int ransac_iter = 200000, int nr_points = 3){
+	printf("weight: %f\n",weight);
+	std::vector<int> owner;
+	std::vector<int> match_id;
+	std::vector< Eigen::Vector3f > src_points;
+	std::vector< Eigen::Vector3f > dst_points;
+	std::vector< double > noiseFactor;
+
+	for(unsigned int i = 0; i < pc_vec.size(); i++){
+		for(unsigned int j = 0; j < pc_vec.at(i)->src_points.size(); j++){
+			owner.push_back(i);
+			match_id.push_back(j);
+			src_points.push_back(pc_vec.at(i)->src_points.at(j));
+			dst_points.push_back(pc_vec.at(i)->dst_points.at(j));
+			noiseFactor.push_back(pc_vec.at(i)->noisemul.at(j));
+		}
+	}
+
+	int nr_kp = src_points.size();
+	std::vector< CostFunction * > errors;
+	if(nr_kp == 0){return errors;}
+	int nr_consistent = 0;
+
+
+	Eigen::Matrix4f retpose;
+	std::vector<int> bestmatching;
+	for(int it = 0; it < ransac_iter; it++){
+		//Sample points
+		std::vector< int > indexes;
+		std::vector< Eigen::Vector3f > src_samples;
+		std::vector< Eigen::Vector3f > dst_samples;
+		std::vector< double> noise_samples;
+
+		for(int j = 0; j < nr_points; j++){
+			int ind;
+			bool failed = true;
+			while(failed){
+				ind = rand()%nr_kp;
+				failed = false;
+				for(int k = 0; k < j; k++){
+					if(ind == indexes.at(k)){failed = true;}
+				}
+			}
+			indexes.push_back(ind);
+			src_samples.push_back(src_points.at(ind));
+			dst_samples.push_back(dst_points.at(ind));
+			noise_samples.push_back(noiseFactor.at(ind));
+		}
+
+		//Check consistency
+		bool consistent = true;
+
+		for(int j = 0; j < nr_points; j++){
+			for(int k = j+1; k < nr_points; k++){
+				float src_distance = (src_samples.at(j)-src_samples.at(k)).norm();
+				float dst_distance = (dst_samples.at(j)-dst_samples.at(k)).norm();
+				double noise = noise_samples.at(k);
+				double t = threshold*noise;
+
+				if(fabs(src_distance-dst_distance) > 4.0*t){consistent = false; break;break;}
+			}
+		}
+
+		//Check inliers
+		nr_consistent += consistent;
+		if(consistent){
+			pcl::TransformationFromCorrespondences tfc;
+			for(int i = 0; i < nr_points; i++){tfc.add(dst_samples.at(i),src_samples.at(i),1);}
+			Eigen::Affine3f ret = tfc.getTransformation();
+
+			Eigen::Matrix4f pose = ret.matrix().inverse();
+
+			std::vector<int> matching;
+			for(int j = 0; j < nr_kp; j++){
+				Eigen::Vector3f src_data = src_points.at(j);
+				Eigen::Vector3f dst_data = dst_points.at(j);
+
+				Eigen::Vector4f sd = pose*Eigen::Vector4f(src_data(0),src_data(1),src_data(2),1);
+				Eigen::Vector4f dd = Eigen::Vector4f(dst_data(0),dst_data(1),dst_data(2),1);
+				double noise = noiseFactor[j];
+				float t = threshold*noise;//0.1*0.005*(dst_data(2)*dst_data(2)+src_data(2)*src_data(2));
+				if((sd-dd).norm() < t){matching.push_back(j);}
+			}
+
+			//save if best
+			if(matching.size() > bestmatching.size()){
+				bestmatching = matching;
+				retpose = pose;
+				//printf("%i -> %i\n",it,matching.size());
+			}
+			if(nr_consistent == 2500){printf("break at %i\n",it);break;}
+		}
+	}
+
+	printf("nr matches best: %i / %i consistent: %i / %i\n",int(bestmatching.size()),nr_kp,nr_consistent,ransac_iter);
+
+	//getMatchesICP(pc_vec,retpose);
+
+	for(int iter = 0; iter < 200; iter++){
+		bestmatching.clear();
+		pcl::TransformationFromCorrespondences tfc;
+		int matching = 0;
+		for(int j = 0; j < nr_kp; j++){
+			Eigen::Vector3f src_data = src_points.at(j);
+			Eigen::Vector3f dst_data = dst_points.at(j);
+
+			Eigen::Vector4f sd = retpose*Eigen::Vector4f(src_data(0),src_data(1),src_data(2),1);
+			Eigen::Vector4f dd = Eigen::Vector4f(dst_data(0),dst_data(1),dst_data(2),1);
+			double noise = noiseFactor[j];
+
+			if((sd-dd).norm() < noise*threshold){
+				bestmatching.push_back(j);
+				tfc.add(dst_data.head<3>(),src_data.head<3>(),1.0/(noise*noise));
+				matching++;
+			}
+		}
+		if(iter % 50 == 0){printf("iteration: %i matches: %i\n",iter,matching);}
+
+		Eigen::Affine3f ret = tfc.getTransformation();
+		retpose = ret.matrix().inverse();
+	}
+	printf("nr matches best: %i / %i \n",int(bestmatching.size()),nr_kp);
+	//exit(0);
+	if(bestmatching.size() < 15){return errors;}
+	for(unsigned int i = 0; i < pc_vec.size(); i++){
+		pc_vec.at(i)->src_matches.clear();
+		pc_vec.at(i)->dst_matches.clear();
+	}
+
+
+	//for(unsigned int i = 0; i < bestmatching.size() && i < 1000; i++){
+	for(unsigned int i = 0; i < bestmatching.size(); i++){
+		ProblemFrameConnection * pc = pc_vec.at(owner.at(bestmatching.at(i)));
+		int id = match_id.at(bestmatching.at(i));
+		//printf("id: %i %i %i\n ",id,pc->src_matches.size(),pc->dst_matches.size());
+		int src_kp_id = pc->src_possible_matches_id.at(id);
+		int dst_kp_id = pc->dst_possible_matches_id.at(id);
+		pc->src_matches.push_back(src_kp_id);
+		pc->dst_matches.push_back(dst_kp_id);
+		cv::KeyPoint src_kp = pc->src->keypoints.at(src_kp_id);
+		cv::KeyPoint dst_kp = pc->dst->keypoints.at(dst_kp_id);
+		double sz	= pc->src->keypoint_depth.at(src_kp_id);
+		double dz	= pc->dst->keypoint_depth.at(dst_kp_id);
+
+		double snoise = sz*sz;
+		double dnoise = dz*dz;
+		double noise = sqrt( snoise*snoise + dnoise*dnoise );
+
+		if(bestmatching.size() > 5000 && (sz > 2.5 || dz > 2.5)){continue;}
+
+		CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight/noise);
+		errors.push_back(err);
+	}
+
+	return errors;
 }
 
 template <typename T> Eigen::Matrix<T,4,4> getMat(const T* const camera, int mode = 0){
-    Eigen::Matrix<T,4,4> ret = Eigen::Matrix<T,4,4>::Identity();
-    if(mode == 0){//yaw pitch roll tx ty tz
-        Eigen::AngleAxis<T> yawAngle(camera[0], Eigen::Matrix<T,3,1>::UnitY());
-        Eigen::AngleAxis<T> pitchAngle(camera[1], Eigen::Matrix<T,3,1>::UnitX());
-        Eigen::AngleAxis<T> rollAngle(camera[2], Eigen::Matrix<T,3,1>::UnitZ());
-        Eigen::Quaternion<T> q = rollAngle * yawAngle * pitchAngle;
-        Eigen::Matrix<T,3,3> rotationMatrix = q.matrix();
-        ret.block(0,0,3,3) = rotationMatrix;
-        ret(0,3) = camera[3];
-        ret(1,3) = camera[4];
-        ret(2,3) = camera[5];
-    }
-    return ret;
+	Eigen::Matrix<T,4,4> ret = Eigen::Matrix<T,4,4>::Identity();
+	if(mode == 0){//yaw pitch roll tx ty tz
+		Eigen::AngleAxis<T> yawAngle(camera[0], Eigen::Matrix<T,3,1>::UnitY());
+		Eigen::AngleAxis<T> pitchAngle(camera[1], Eigen::Matrix<T,3,1>::UnitX());
+		Eigen::AngleAxis<T> rollAngle(camera[2], Eigen::Matrix<T,3,1>::UnitZ());
+		Eigen::Quaternion<T> q = rollAngle * yawAngle * pitchAngle;
+		Eigen::Matrix<T,3,3> rotationMatrix = q.matrix();
+		ret.block(0,0,3,3) = rotationMatrix;
+		ret(0,3) = camera[3];
+		ret(1,3) = camera[4];
+		ret(2,3) = camera[5];
+	}
+	return ret;
+}
+
+void RobotContainer::show(){
+	if(!viewer_initialized){
+		viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
+		viewer->setBackgroundColor (1.0, 1.0, 1.0);
+		viewer->addCoordinateSystem (1.0);
+		viewer->initCameraParameters ();
+		viewer_initialized = true;
+	}
+
+	for(unsigned int pa = 0; pa < paths.size(); pa += 60){
+		std::string path = paths[pa];
+		printf("path: %s\n",path.c_str());
+
+		SimpleXMLParser<PointType> simple_parser;
+		SimpleXMLParser<PointType>::RoomData roomData = simple_parser.loadRoomFromXML(path);
+
+		//pcl::PointCloud<PointType>::Ptr clouddata = roomData.vIntermediateRoomClouds[inds[x][y]];
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr	cloud	(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+		camera->fx = 1.0/shared_params[0];	camera->fy = 1.0/shared_params[1];	camera->cx = shared_params[2];		camera->cy = shared_params[3];
+		camera->print();    for(unsigned int s = 0; s < sweeps.size(); s++){
+			for(unsigned int x = 0; x < todox; x++){
+				for(unsigned int y = 0; y < todoy; y++){
+					sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast<float>();
+				}
+			}
+		}
+
+		std::vector<Eigen::Matrix4f> registeredPoses = sweeps.at(0)->getPoseVector(); // all the sweeps have the same poses. Return the first one
+
+		for(unsigned int i = 0; i < roomData.vIntermediateRoomClouds.size(); i++){
+			if(true || (i == 0) || (i == roomData.vIntermediateRoomClouds.size()-1)){
+				const float cx				= camera->cx;
+				const float cy				= camera->cy;
+				const float ifx				= 1.0/camera->fx;
+				const float ify				= 1.0/camera->fy;
+
+				Eigen::Matrix4f p = registeredPoses[i];
+
+				float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3);
+				float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3);
+				float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3);
+
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr clouddata = roomData.vIntermediateRoomClouds[i];
+
+				int r = rand()%256;
+				int g = rand()%256;
+				int b = rand()%256;
+
+//				if(i == 0){r = 0; g = 255; b = 0;}
+//				if(i == 16){r = 255; g = 0; b = 0;}
+
+				for(unsigned int w = 0; w < width; w++){
+					for(unsigned int h = 0; h < height;h++){
+						int ind = h*width+w;
+						pcl::PointXYZRGB & po = clouddata->points.at(ind);
+						double z = po.z;
+						double x = (w - cx) * z * ifx;
+						double y = (h - cy) * z * ify;
+						po.x	= m00*x + m01*y + m02*z + m03;
+						po.y	= m10*x + m11*y + m12*z + m13;
+						po.z	= m20*x + m21*y + m22*z + m23;
+						po.r	= r;
+						po.g	= g;
+						po.b	= b;
+					}
+				}
+				*cloud += *clouddata;
+			}
+		}
+		viewer->addPointCloud<pcl::PointXYZRGB> (cloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>(cloud), "cloud");
+		viewer->spin();
+		viewer->removeAllPointClouds();
+	}
+//
+//	viewer->spin();
+//
+
+//	cv::Size res	= roomData.vIntermediateRoomCloudCamParams[0].fullResolution();
+//	height			= res.height;
+//	width			= res.width;
+
+//	if(camera == 0){
+//		camera = new Camera(fx, fy, cx, cy, width,	height);
+//		camera->version = 1;
+//		rgb		=	new float[3*width*height];
+//		depth	=	new float[	width*height];
+
+//		shared_params[0] = 1.0/fx;		//invfx
+//		shared_params[1] = 1.0/fy;		//invfy
+//		shared_params[2] = cx;
+//		shared_params[3] = cy;
+//		shared_params[4] = 0.1;
+//	}
+
+
+
+
+//			sweep_frames.at(y*todox+x) = new Frame(camera,rgb,depth);
+//			sweep_frames.at(y*todox+x)->framepos = inds[x][y];
+//		}
+//	}
+
+//	sweeps.push_back(new Sweep(todox, todoy, sweep_frames));
+//	alignedSweep.push_back(false);
+//	sweeps.back()->idtag = roomData.roomWaypointId;
+//	sweeps.back()->xmlpath = path;
+
+
+	//SemanticRoom<PointType> aRoom = SemanticRoomXMLParser<PointType>::loadRoomFromXML(path,true);
+	//exit(0);
+
 }
 
 std::vector<Eigen::Matrix4f> RobotContainer::runInitialTraining(){
-    ceres::Problem problem;
-    Solver::Options options;
-    options.max_num_iterations = 1500;
-    options.minimizer_progress_to_stdout = true;
-    options.num_linear_solver_threads = 11;
-    options.num_threads = 11;
-    Solver::Summary summary;
-
-    //    double *** poses = new double**[todox];
-    //    for(unsigned int x = 0; x < todox; x++){
-    //        poses[x] = new double*[todoy];
-    //        for(unsigned int y = 0; y < todoy; y++){
-    //            poses[x][y] = new double[6];
-    //            for(unsigned int k = 0; k < 6; k++){poses[x][y][k] = 0;}
-    //        }
-    //    }
-
-
-    //1st forward X loop
-    std::vector< std::vector< ProblemFrameConnection * > > x1_vec;
-    x1_vec.resize(todoy);
-    for(unsigned int s = 0; s < sweeps.size(); s++){
-        printf("1st forward X loop: %i\n",s);
-        Sweep * sweep = sweeps.at(s);
-        for(unsigned int x = 0; x < todox-1; x++){
-            for(unsigned int y = 0; y < todoy; y++){
-                ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x+1][y], shared_params, poses[x][y], poses[x+1][y]);
-                x1_vec.at(y).push_back(pc);
-            }
-        }
-    }
-
-    for(unsigned int y = 0; y < todoy; y++){
-        std::vector< CostFunction * > matches = getMatchesRansac(x1_vec.at(y));
-        for(unsigned int i = 0; i < x1_vec.at(y).size(); i++){
-            x1_vec.at(y).at(i)->addMatchesToProblem(problem, matches);
-        }
-    }
-
-    Solve(options, &problem, &summary);
-    //std::cout << summary.FullReport() << "\n";
-
-    //1st forward Y loop
-    std::vector< std::vector< ProblemFrameConnection * > > y1_vec;
-    y1_vec.resize(todox);
-    for(unsigned int s = 0; s < sweeps.size(); s++){
-        printf("1st forward Y loop: %i\n",s);
-        Sweep * sweep = sweeps.at(s);
-        for(unsigned int x = 0; x < todox; x++){
-            for(unsigned int y = 0; y < todoy-1; y++){
-                ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x][y+1], shared_params, poses[x][y], poses[x][y+1]);
-                y1_vec.at(x).push_back(pc);
-            }
-        }
-    }
-
-    for(unsigned int x = 0; x < todox; x++){
-        std::vector< CostFunction * > matches = getMatchesRansac(y1_vec.at(x));
-        for(unsigned int i = 0; i < y1_vec.at(x).size(); i++){
-            y1_vec.at(x).at(i)->addMatchesToProblem(problem, matches);
-        }
-    }
-
-    Solve(options, &problem, &summary);
-    //std::cout << summary.FullReport() << "\n";
-
-    //2nd forward X loop
-    std::vector< std::vector< ProblemFrameConnection * > > x2_vec;
-    x2_vec.resize(todoy);
-    for(unsigned int s = 0; s < sweeps.size(); s++){
-        printf("2t forward X loop: %i\n",s);
-        Sweep * sweep = sweeps.at(s);
-        for(unsigned int x = 0; x < todox-2; x++){
-            for(unsigned int y = 0; y < todoy; y++){
-                ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x+2][y], shared_params, poses[x][y], poses[x+2][y]);
-                x2_vec.at(y).push_back(pc);
-            }
-        }
-    }
-
-    for(unsigned int y = 0; y < todoy; y++){
-        std::vector< CostFunction * > matches = getMatchesRansac(x2_vec.at(y));
-        for(unsigned int i = 0; i < x2_vec.at(y).size(); i++){
-            x2_vec.at(y).at(i)->addMatchesToProblem(problem, matches);
-        }
-    }
-
-    Solve(options, &problem, &summary);
-    //std::cout << summary.FullReport() << "\n";
-
-    //	camera->fx = 1.0/shared_params[0];	camera->fy = 1.0/shared_params[1];	camera->cx = shared_params[2];		camera->cy = shared_params[3];
-    //	camera->print();
-    //	for(unsigned int s = 0; s < sweeps.size(); s++){
-    //		for(unsigned int x = 0; x < todox; x++){
-    //			for(unsigned int y = 0; y < todoy; y++){
-    //				sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast<float>();
-    //			}
-    //		}
-    //	}
-
-    //Loop closure
-    std::vector< std::vector< ProblemFrameConnection * > > loop_vec;
-    loop_vec.resize(todoy);
-    for(unsigned int s = 0; s < sweeps.size(); s++){
-        printf("Loop closure: %i\n",s);
-        Sweep * sweep = sweeps.at(s);
-        for(unsigned int y = 0; y < todoy; y++){
-            ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[0][y],sweep->frames[todox-1][y], shared_params, poses[0][y], poses[todox-1][y],1,false);
-            loop_vec.at(y).push_back(pc);
-        }
-    }
-
-    for(unsigned int y = 0; y < todoy; y++){
-        std::vector< CostFunction * > matches = getMatchesRansac(loop_vec.at(y),x1_vec.at(y).size());
-        for(unsigned int i = 0; i < loop_vec.at(y).size(); i++){
-            loop_vec.at(y).at(i)->addMatchesToProblem(problem, matches);
-        }
-    }
-    Solve(options, &problem, &summary);
-    std::cout << summary.FullReport() << "\n";
-
-    //Optimize camera parameter
-    //optimizeCameraParams = true;
-    //Solve(options, &problem, &summary);
-    //std::cout << summary.FullReport() << "\n";
-
-    camera->fx = 1.0/shared_params[0];	camera->fy = 1.0/shared_params[1];	camera->cx = shared_params[2];		camera->cy = shared_params[3];
-    camera->print();    for(unsigned int s = 0; s < sweeps.size(); s++){
-        for(unsigned int x = 0; x < todox; x++){
-            for(unsigned int y = 0; y < todoy; y++){
-                sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast<float>();
-            }
-        }
-    }
-
-    std::vector<Eigen::Matrix4f> registeredPoses = sweeps.at(0)->getPoseVector(); // all the sweeps have the same poses. Return the first one
-    return registeredPoses;
+
+	double start_fx = camera->fx;
+	double start_fy = camera->fy;
+	double start_cx = camera->cx;
+	double start_cy = camera->cy;
+
+
+
+////for(double hs = 0.5; hs >= 0.0001; hs *= 0.5){
+////for(double current_fy = start_fy-100.0; current_fy <= start_fy+101.0; current_fy += 10.0){
+//{
+////	shared_params[0] = 1.0/start_fx;		//invfx
+////	shared_params[1] = 1.0/current_fy;		//invfy
+////	shared_params[2] = start_cx;
+////	shared_params[3] = start_cy;
+////	shared_params[4] = 0.1;
+
+//	ceres::Problem problem;
+//	Solver::Options options;
+//	options.max_num_iterations = 1500;
+//	options.minimizer_progress_to_stdout = true;
+//	options.num_linear_solver_threads = 11;
+//	options.num_threads = 11;
+//	Solver::Summary summary;
+
+//	//Loop closure
+//	std::vector< std::vector< ProblemFrameConnection * > > loop_vec;
+//	loop_vec.resize(todoy);
+//	for(unsigned int s = 0; s < sweeps.size(); s++){
+//		printf("Loop closure: %i\n",s);
+//		Sweep * sweep = sweeps.at(s);
+//		for(unsigned int y = 0; y < todoy; y++){
+//			ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[0][y],sweep->frames[todox-1][y], shared_params, poses[0][y], poses[todox-1][y],1,false);
+//			loop_vec.at(y).push_back(pc);
+//		}
+//	}
+
+//	for(unsigned int y = 0; y < todoy; y++){
+//		std::vector< CostFunction * > matches = getMatchesRansac(loop_vec.at(y));
+//		for(unsigned int i = 0; i < loop_vec.at(y).size(); i++){
+//			loop_vec.at(y).at(i)->addMatchesToProblem(problem, matches,1.0);
+//		}
+//	}
+
+
+//	Solve(options, &problem, &summary);
+//	std::cout << summary.FullReport() << "\n";
+//	show();
+
+
+
+////	for(double current_fy = start_fy-300.0; current_fy <= start_fy+501.0; current_fy += 50.0){
+////		shared_params[0] = 1.0/start_fx;		//invfx
+////		shared_params[1] = 1.0/current_fy;		//invfy
+////		shared_params[2] = start_cx;
+////		shared_params[3] = start_cy;
+////		shared_params[4] = 0.1;
+////		show();
+////	};
+//}
+
+ceres::Problem problem;
+Solver::Options options;
+options.max_num_iterations = 1500;
+options.minimizer_progress_to_stdout = true;
+options.num_linear_solver_threads = 11;
+options.num_threads = 11;
+Solver::Summary summary;
+
+
+	//    double *** poses = new double**[todox];
+	//    for(unsigned int x = 0; x < todox; x++){
+	//        poses[x] = new double*[todoy];
+	//        for(unsigned int y = 0; y < todoy; y++){
+	//            poses[x][y] = new double[6];
+	//            for(unsigned int k = 0; k < 6; k++){poses[x][y][k] = 0;}
+	//        }
+	//    }
+
+	printf("1st forward X loop\n");
+	//1st forward X loop
+	std::vector< std::vector< ProblemFrameConnection * > > x1_vec;
+	x1_vec.resize(todoy);
+	for(unsigned int s = 0; s < sweeps.size(); s++){
+
+		Sweep * sweep = sweeps.at(s);
+		for(unsigned int x = 0; x < todox-1; x++){
+			for(unsigned int y = 0; y < todoy; y++){
+				ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x+1][y], shared_params, poses[x][y], poses[x+1][y]);
+				x1_vec.at(y).push_back(pc);
+			}
+		}
+	}
+
+
+	for(unsigned int y = 0; y < todoy; y++){
+		std::vector< CostFunction * > matches = getMatchesRansac(x1_vec.at(y));
+		for(unsigned int i = 0; i < x1_vec.at(y).size(); i++){
+			//if(i % 100 == 0){printf("%i\n",i);}
+			x1_vec.at(y).at(i)->addMatchesToProblem(problem, matches);
+			//if(y == 0 && i == 300){Solve(options, &problem, &summary);}
+		}
+	}
+
+	//Solve(options, &problem, &summary);
+//show();
+	printf("1st forward Y loop:\n");
+	//1st forward Y loop
+	std::vector< std::vector< ProblemFrameConnection * > > y1_vec;
+	y1_vec.resize(todox);
+	for(unsigned int s = 0; s < sweeps.size(); s++){
+
+		Sweep * sweep = sweeps.at(s);
+		for(unsigned int x = 0; x < todox; x++){
+			for(unsigned int y = 0; y < todoy-1; y++){
+				ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x][y+1], shared_params, poses[x][y], poses[x][y+1]);
+				y1_vec.at(x).push_back(pc);
+			}
+		}
+	}
+
+	for(unsigned int x = 0; x < todox; x++){
+		std::vector< CostFunction * > matches = getMatchesRansac(y1_vec.at(x));
+		for(unsigned int i = 0; i < y1_vec.at(x).size(); i++){
+			y1_vec.at(x).at(i)->addMatchesToProblem(problem, matches);
+		}
+	}
+
+
+	Solve(options, &problem, &summary);
+//show();
+
+	//std::cout << summary.FullReport() << "\n";
+	printf("2t forward X loop\n");
+
+	//2nd forward X loop
+	std::vector< std::vector< ProblemFrameConnection * > > x2_vec;
+	x2_vec.resize(todoy);
+	for(unsigned int s = 0; s < sweeps.size(); s++){
+		Sweep * sweep = sweeps.at(s);
+		for(unsigned int x = 0; x < todox-2; x++){
+			for(unsigned int y = 0; y < todoy; y++){
+				ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x+2][y], shared_params, poses[x][y], poses[x+2][y]);
+				x2_vec.at(y).push_back(pc);
+			}
+		}
+	}
+
+
+	for(unsigned int y = 0; y < todoy; y++){
+		std::vector< CostFunction * > matches = getMatchesRansac(x2_vec.at(y));
+		for(unsigned int i = 0; i < x2_vec.at(y).size(); i++){
+			x2_vec.at(y).at(i)->addMatchesToProblem(problem, matches);
+		}
+	}
+
+	//Solve(options, &problem, &summary);
+//show();
+	//std::cout << summary.FullReport() << "\n";
+
+	//	camera->fx = 1.0/shared_params[0];	camera->fy = 1.0/shared_params[1];	camera->cx = shared_params[2];		camera->cy = shared_params[3];
+	//	camera->print();
+	//	for(unsigned int s = 0; s < sweeps.size(); s++){
+	//		for(unsigned int x = 0; x < todox; x++){
+	//			for(unsigned int y = 0; y < todoy; y++){
+	//				sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast<float>();
+	//			}
+	//		}
+	//	}
+
+	//Loop closure
+	printf("Loop closure\n");
+	std::vector< std::vector< ProblemFrameConnection * > > loop_vec;
+	loop_vec.resize(todoy);
+	for(unsigned int s = 0; s < sweeps.size(); s++){
+
+		Sweep * sweep = sweeps.at(s);
+		for(unsigned int y = 0; y < todoy; y++){
+			ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[0][y],sweep->frames[todox-1][y], shared_params, poses[0][y], poses[todox-1][y],1,false);
+			loop_vec.at(y).push_back(pc);
+		}
+	}
+
+	for(unsigned int y = 0; y < todoy; y++){
+		std::vector< CostFunction * > matches = getMatchesRansac(loop_vec.at(y),x1_vec.at(y).size());
+		for(unsigned int i = 0; i < loop_vec.at(y).size(); i++){
+			loop_vec.at(y).at(i)->addMatchesToProblem(problem, matches);
+		}
+	}
+
+
+	Solve(options, &problem, &summary);
+	std::cout << summary.FullReport() << "\n";
+//show();
+
+//	pair3DError::optimizeCameraParams = true;
+//	Solve(options, &problem, &summary);
+//show();
+	//Optimize camera parameter
+	//optimizeCameraParams = true;
+	//Solve(options, &problem, &summary);
+	//std::cout << summary.FullReport() << "\n";
+
+	camera->fx = 1.0/shared_params[0];	camera->fy = 1.0/shared_params[1];	camera->cx = shared_params[2];		camera->cy = shared_params[3];
+	camera->print();    for(unsigned int s = 0; s < sweeps.size(); s++){
+		for(unsigned int x = 0; x < todox; x++){
+			for(unsigned int y = 0; y < todoy; y++){
+				sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast<float>();
+			}
+		}
+	}
+
+	std::vector<Eigen::Matrix4f> registeredPoses = sweeps.at(0)->getPoseVector(); // all the sweeps have the same poses. Return the first one
+
+
+//show();
+
+	return registeredPoses;
 
 }
 
 void RobotContainer::refineTraining(){}
 
 std::vector<Eigen::Matrix4f> RobotContainer::train(){
-    return runInitialTraining();
-    //	refineTraining();
+	return runInitialTraining();
+	//	refineTraining();
 }
 
 bool RobotContainer::isCalibrated(){return true;}
@@ -544,45 +803,45 @@ bool RobotContainer::isCalibrated(){return true;}
 
 std::vector<Eigen::Matrix4f> RobotContainer::alignAndStoreSweeps(){
 
-    // aligns sweeps to the first one
-
-    using namespace Eigen;
-
-    std::vector<Eigen::Matrix4f> toRet;
-
-    if (sweeps.size() == 0)
-    {
-        std::cout<<"No sweeps to register."<<std::endl;
-        return toRet;
-    }
-
-//    for (size_t i=0; i<todox; i++){
-//        for (size_t j=0; j<todoy;j++){
-//            for (size_t k=0; k<6;k++)
-//            {
-//                std::cout<<poses[i][j][k]<<std::endl;
-//            }
-//        }
-//    }
-
-
-    std::cout<<"Initializing sweep camera positions."<<std::endl;
-    // setup sweep transforms
-    for(unsigned int s = 0; s < sweeps.size(); s++){
-        for(unsigned int x = 0; x < todox; x++){
-            for(unsigned int y = 0; y < todoy; y++){
-                Eigen::Matrix4f mat = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast<float>();
-                sweeps.at(s)->poses[x][y] = mat;
-            }
-        }
-    }
-
-    Sweep * sweep = sweeps.at(0);
-    for(unsigned int i = 1; i < sweeps.size(); i++){
-        Sweep * s = sweeps.at(i);
-        Eigen::Matrix4f m = sweep->align(s);
-        toRet.push_back(m);
-    }
-    return toRet;
+	// aligns sweeps to the first one
+
+	using namespace Eigen;
+
+	std::vector<Eigen::Matrix4f> toRet;
+
+	if (sweeps.size() == 0)
+	{
+		std::cout<<"No sweeps to register."<<std::endl;
+		return toRet;
+	}
+
+	//    for (size_t i=0; i<todox; i++){
+	//        for (size_t j=0; j<todoy;j++){
+	//            for (size_t k=0; k<6;k++)
+	//            {
+	//                std::cout<<poses[i][j][k]<<std::endl;
+	//            }
+	//        }
+	//    }
+
+
+	std::cout<<"Initializing sweep camera positions."<<std::endl;
+	// setup sweep transforms
+	for(unsigned int s = 0; s < sweeps.size(); s++){
+		for(unsigned int x = 0; x < todox; x++){
+			for(unsigned int y = 0; y < todoy; y++){
+				Eigen::Matrix4f mat = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast<float>();
+				sweeps.at(s)->poses[x][y] = mat;
+			}
+		}
+	}
+
+	Sweep * sweep = sweeps.at(0);
+	for(unsigned int i = 1; i < sweeps.size(); i++){
+		Sweep * s = sweeps.at(i);
+		Eigen::Matrix4f m = sweep->align(s);
+		toRet.push_back(m);
+	}
+	return toRet;
 }
 
diff --git a/strands_sweep_registration/src/Sweep.cpp b/strands_sweep_registration/src/Sweep.cpp
index a4133820..4ab75bf0 100644
--- a/strands_sweep_registration/src/Sweep.cpp
+++ b/strands_sweep_registration/src/Sweep.cpp
@@ -1,4 +1,4 @@
-#include "strands_sweep_registration/Sweep.h"
+#include "../include/strands_sweep_registration/Sweep.h"
 
 #include "cv.h" 
 #include "highgui.h"
diff --git a/strands_sweep_registration/src/pair3DError.cpp b/strands_sweep_registration/src/pair3DError.cpp
index 99532379..87510dba 100644
--- a/strands_sweep_registration/src/pair3DError.cpp
+++ b/strands_sweep_registration/src/pair3DError.cpp
@@ -1,12 +1,14 @@
-#include "strands_sweep_registration/pair3DError.h"
-#include "strands_sweep_registration/camera_parameters.h"
+#include "../include/strands_sweep_registration/pair3DError.h"
+#include "../include/strands_sweep_registration/camera_parameters.h"
 
 int sumid = 0;
 
+bool pair3DError::optimizeCameraParams = false;
+
 pair3DError::pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight) : sw(sw), sh(sh), sz(sz), dw(dw), dh(dh), dz(dz), weight(weight) \
 {
     id = sumid++;
-    optimizeCameraParams = false;
+    //optimizeCameraParams = true;
     information = 1.0/1.5;
 }
 
@@ -42,8 +44,8 @@ bool pair3DError::Evaluate(double const* const* parameters, double* residuals, d
     double cy = CameraParameters::get().cy();
 
     if(optimizeCameraParams){
-        invfx = params[0];
-        invfy = params[1];
+        invfx = fabs(params[0]);
+        invfy = fabs(params[1]);
         cx = params[2];
         cy = params[3];
     }
diff --git a/strands_sweep_registration/src/util.cpp b/strands_sweep_registration/src/util.cpp
index 08a42cfa..c8b13f14 100644
--- a/strands_sweep_registration/src/util.cpp
+++ b/strands_sweep_registration/src/util.cpp
@@ -1,4 +1,4 @@
-#include "strands_sweep_registration/util.h"
+#include "../include/strands_sweep_registration/util.h"
 
 int popcount_lauradoux(uint64_t *buf, uint32_t size) {
 	const uint64_t* data = (uint64_t*) buf;