Add scene graph publisher, fix data compliance check, add sim freq param

fjonath1 · cpaxton · commit 2e8a5ae2fada · 2017-09-06T14:13:42.000-04:00
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,9 +1,12 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(sequential_scene_parsing)
 
+SET(CMAKE_BUILD_TYPE "Debug")
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
+
 find_package(VTK REQUIRED)
 find_package(PCL REQUIRED)
 find_package(Boost REQUIRED)
@@ -30,14 +33,18 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   costar_objrec_msgs
   objrec_hypothesis_msgs
-  std_msgs message_generation
+  std_msgs geometry_msgs message_generation
 )
 
 add_service_files(
   FILES hypothesis_request.srv
 )
 
-generate_messages( DEPENDENCIES std_msgs )
+add_message_files(
+	FILES SceneNodes.msg StructureGraph.msg SceneGraph.msg
+)
+
+generate_messages( DEPENDENCIES std_msgs geometry_msgs)
 
 catkin_package(
 #  INCLUDE_DIRS include
diff --git a/include/physics_world_parameters.h b/include/physics_world_parameters.h
@@ -9,6 +9,7 @@
 #define GRAVITY_MAGNITUDE 9.807
 static int GRAVITY_SCALE_COMPENSATION = 3;
 static double SCALED_GRAVITY_MAGNITUDE = SCALING * GRAVITY_MAGNITUDE / GRAVITY_SCALE_COMPENSATION;
+static double SIMULATION_FREQUENCY_MULTIPLIER = 1.;
 
 enum SimulationMode {
 	//  RUN THE SIMULATION UNTIL THE NUMBER OF SIMULATION STEP IS REACHED OR ALL OBJECT IS IN DEACTIVATED STATE
diff --git a/include/ros_sequential_scene_parsing.h b/include/ros_sequential_scene_parsing.h
@@ -28,6 +28,11 @@
 #include "sequential_scene_parsing.h"
 #include "symmetric_orientation_realignment.h"
 
+#include "sequential_scene_parsing/SceneNodes.h"
+#include "sequential_scene_parsing/StructureGraph.h"
+#include "sequential_scene_parsing/SceneGraph.h"
+
+
 // Ros bundling for scene parsing
 class RosSceneHypothesisAssessor : public SceneHypothesisAssessor
 {
@@ -54,6 +59,7 @@ class RosSceneHypothesisAssessor : public SceneHypothesisAssessor
 	void processHypotheses();
 	void updateTfFromObjTransformMap(const std::map<std::string, ObjectParameter> &input_tf_map);
 
+	sequential_scene_parsing::SceneGraph generateSceneGraphMsgs() const;
 	// void initialize();
 	// bool debug_messages_;
 	bool class_ready_;
@@ -79,6 +85,7 @@ class RosSceneHypothesisAssessor : public SceneHypothesisAssessor
 	ros::Subscriber background_pcl_sub;
 	ros::Subscriber scene_pcl_sub;
 	ros::Publisher done_message_pub;
+	ros::Publisher scene_graph_pub;
 	
 	ros::NodeHandle nh_;
 	tf::TransformListener listener_;
diff --git a/include/scene_physics_engine.h b/include/scene_physics_engine.h
@@ -57,7 +57,7 @@ class PhysicsEngine : public PlatformDemoApplication
 
 	void setObjectPenaltyDatabase(std::map<std::string, ObjectPenaltyParameters> * penalty_database);
 
-	void setSimulationMode(const int &simulation_mode, const double simulation_step = 1./200,
+	void setSimulationMode(const int &simulation_mode, const double simulation_step = 1./120,
 		const unsigned int &number_of_world_tick = 100);
 
 	void setDebugMode(bool debug);
diff --git a/include/sequential_scene_parsing.h b/include/sequential_scene_parsing.h
@@ -66,6 +66,8 @@ class SceneHypothesisAssessor
 			btScalar(forces_magnitude_per_point),btScalar(max_point_distance_threshold));
 	}
 
+	SceneSupportGraph getSceneGraphData(std::map<std::string, vertex_t> &vertex_map) const;
+
 	ObjectDatabase obj_database_;
 
 private:
diff --git a/launch/block_scene.launch b/launch/block_scene.launch
@@ -17,17 +17,19 @@
   <arg name="objransac_model_directory"      default="$(find sequential_scene_parsing)/mesh"/>
   <arg name="objransac_model_names"          default="wood_cube" />
 
-  <arg name="data_forces_magnitude"          default="0.25"/>
+  <arg name="data_forces_magnitude"          default="0.15"/>
   <arg name="data_forces_max_distance"       default="0.015"/>
 
   <arg name="best_hypothesis_only"           default="true"/>
-  <arg name="small_obj_g_comp"               default="1"/>
+  <arg name="small_obj_g_comp"               default="5"/>
+  <arg name="sim_freq_multiplier"            default="1."/>
 
-  <node pkg="sequential_scene_parsing" type="sequential_scene_ros" name="sequential_scene_ros"
+  <node pkg="sequential_scene_parsing" type="sequential_scene_ros" name="sequential_scene_parsing"
   output="screen" 
   >  
     <!-- sequential_scene_ros common Node arg pass -->
     <param name="small_obj_g_comp"        type="int"     value="$(arg small_obj_g_comp)"/>
+    <param name="sim_freq_multiplier"     type="double"  value="$(arg sim_freq_multiplier)"/>
 
     <param name="background_pcl2_topic"   type="str"     value="$(arg background_pcl2_topic)" />
     <param name="scene_pcl2_topic"        type="str"     value="$(arg scene_pcl2_topic)" />
diff --git a/launch/ros_scene.launch b/launch/ros_scene.launch
@@ -22,12 +22,14 @@
 
   <arg name="best_hypothesis_only"           default="false"/>
   <arg name="small_obj_g_comp"               default="3"/>
+  <arg name="sim_freq_multiplier"            default="1."/>
 
-  <node pkg="sequential_scene_parsing" type="sequential_scene_ros" name="sequential_scene_ros"
+  <node pkg="sequential_scene_parsing" type="sequential_scene_ros" name="sequential_scene_parsing"
   output="screen" 
   >  
     <!-- sequential_scene_ros common Node arg pass -->
     <param name="small_obj_g_comp"        type="int"     value="$(arg small_obj_g_comp)"/>
+    <param name="sim_freq_multiplier"     type="double"  value="$(arg sim_freq_multiplier)"/>
 
     <param name="background_pcl2_topic"   type="str"     value="$(arg background_pcl2_topic)" />
     <param name="scene_pcl2_topic"        type="str"     value="$(arg scene_pcl2_topic)" />
diff --git a/src/ros_sequential_scene_parsing.cpp b/src/ros_sequential_scene_parsing.cpp
@@ -76,6 +76,8 @@ void RosSceneHypothesisAssessor::setNodeHandle(const ros::NodeHandle &nh)
 
 	nh.param("best_hypothesis_only",best_hypothesis_only_,false);
 	nh.param("small_obj_g_comp",GRAVITY_SCALE_COMPENSATION,3);
+	nh.param("sim_freq_multiplier",SIMULATION_FREQUENCY_MULTIPLIER,1.);
+
 	SCALED_GRAVITY_MAGNITUDE = SCALING * GRAVITY_MAGNITUDE / GRAVITY_SCALE_COMPENSATION;
 
 	if (load_table){
@@ -104,6 +106,7 @@ void RosSceneHypothesisAssessor::setNodeHandle(const ros::NodeHandle &nh)
 	this->scene_pcl_sub = this->nh_.subscribe(scene_pcl2_topic,1,&RosSceneHypothesisAssessor::addSceneCloud,this);
 
 	this->done_message_pub = this->nh_.advertise<std_msgs::Empty>("done_hypothesis_msg",1);
+	this->scene_graph_pub = this->nh_.advertise<sequential_scene_parsing::SceneGraph>("scene_structure_list",1);
 
 	// setup objrecransac tool
 	boost::split(object_names,objransac_model_list,boost::is_any_of(","));
@@ -334,6 +337,12 @@ void RosSceneHypothesisAssessor::processDetectedObjectMsgs()
 	std::cerr << "Getting corrected object transform...\n";
 	std::map<std::string, ObjectParameter> object_transforms = this->getCorrectedObjectTransform(best_hypothesis_only_);
 	this->updateTfFromObjTransformMap(object_transforms);
+
+	if (best_hypothesis_only_)
+	{
+		this->scene_graph_pub.publish(this->generateSceneGraphMsgs());
+	}
+
 	this->mtx_.unlock();
 	
 	this->has_tf_ = true;
@@ -415,5 +424,44 @@ void RosSceneHypothesisAssessor::processHypotheses()
 	std_msgs::Empty done_msg;
 	this->done_message_pub.publish(done_msg);
 
+	this->scene_graph_pub.publish(this->generateSceneGraphMsgs());
+
 	this->mtx_.unlock();
 }
+
+
+sequential_scene_parsing::SceneGraph RosSceneHypothesisAssessor::generateSceneGraphMsgs() const
+{
+	std::map<std::string, vertex_t> vertex_map;
+	SceneSupportGraph current_graph = this->getSceneGraphData(vertex_map);
+	std::vector<vertex_t> all_base_structs = getAllChildVertices(current_graph, vertex_map["background"]);
+	sequential_scene_parsing::SceneGraph structure_graph_msg;
+	structure_graph_msg.structure.reserve(all_base_structs.size());
+	structure_graph_msg.base_objects_id.reserve(all_base_structs.size());
+	for (std::vector<vertex_t>::const_iterator it = all_base_structs.begin(); it != all_base_structs.end(); ++it)
+	{
+		sequential_scene_parsing::StructureGraph new_structure;
+		OrderedVertexVisitor all_connected_structures = getOrderedVertexList(current_graph, *it, true);
+		std::map<std::size_t, std::vector<vertex_t> > vertex_visit_by_distances = all_connected_structures.getVertexVisitOrderByDistances();
+		for (std::map<std::size_t, std::vector<vertex_t> >::const_iterator dist_it = vertex_visit_by_distances.begin();
+			dist_it != vertex_visit_by_distances.end(); ++dist_it)
+		{
+			// Do nothing for disconnected vertices;
+			if (dist_it->first == 0) continue;
+
+			sequential_scene_parsing::SceneNodes nodes;
+			nodes.object_names.reserve(dist_it->second.size());
+			for (std::vector<vertex_t>::const_iterator node_it = dist_it->second.begin(); node_it != dist_it->second.end(); ++node_it)
+			{
+				nodes.object_names.push_back(current_graph[*node_it].object_id_);
+			}
+			new_structure.nodes_level.push_back(nodes);
+		}
+
+		structure_graph_msg.structure.push_back(new_structure);
+		structure_graph_msg.base_objects_id.push_back(current_graph[*it].object_id_);
+	}
+
+	return structure_graph_msg;
+}
+
diff --git a/src/scene_data_forces.cpp b/src/scene_data_forces.cpp
@@ -346,10 +346,14 @@ PointCloudXYZPtr FeedbackDataForcesGenerator::generateCorrespondenceCloud(PointC
 	{
 		std::vector< int > k_indices(1);
 		std::vector< float > distances(1);
-		this->scene_data_tree_.nearestKSearch(*input_cloud, i, 1, k_indices, distances);
-		if (!use_filter_distance || (use_filter_distance && distances[0] < max_squared_distance))
+
+		// any neighbor is found
+		if (this->scene_data_tree_.nearestKSearch(*input_cloud, i, 1, k_indices, distances) > 0)
 		{
-			nearest_point_correspondence_cloud->points.push_back(scene_data_->points[k_indices[0]]);
+			if (!use_filter_distance || (use_filter_distance && distances[0] < max_squared_distance))
+			{
+				nearest_point_correspondence_cloud->points.push_back(scene_data_->points[k_indices[0]]);
+			}
 		}
 	}
 	nearest_point_correspondence_cloud->width = nearest_point_correspondence_cloud->size();
diff --git a/src/sequential_scene_hypothesis.cpp b/src/sequential_scene_hypothesis.cpp
@@ -153,16 +153,16 @@ int SequentialSceneHypothesis::updateRemovedObjectStatus(const std::string &obje
 
 	// object is not detected by pose estimator
 	std::cerr << object_name;
-	// bool object_visible = checkObjectVisible(model_name,object_pose);
-	// if (object_visible)
-	// {
-	// 	std::cerr << " is visible, thus it will not be removed.\n";
-	// 	return NOT_REMOVED;
-	// }
-	// else
-	// {
-	// 	std::cerr << " is not visible,";
-	// }
+	bool object_visible = checkObjectVisible(model_name,object_pose);
+	if (object_visible)
+	{
+		std::cerr << " is visible, thus it will not be removed.\n";
+		return NOT_REMOVED;
+	}
+	else
+	{
+		std::cerr << " is not visible,";
+	}
 
 	bool object_obstructed = checkObjectObstruction(model_name,object_pose);
 	
@@ -220,9 +220,10 @@ bool SequentialSceneHypothesis::checkObjectObstruction(const std::string &model_
 bool SequentialSceneHypothesis::checkObjectReplaced(const std::string &object_name,
 	const std::string &model_name, const btTransform &object_pose)
 {
+	// disabled for now. Will be updated to OBB test instead
+	return false;
+
 	// use AABB to check if the original object volume has been occupied by something else
-	// TODO: IMPLEMENT THIS!!
-	// use bullet contactTest;
 	if (!obj_database_)
 	{
 		std::cerr << "Error! Cannot check for replaced objects since no object database has been set.\n";
@@ -247,6 +248,7 @@ bool SequentialSceneHypothesis::checkObjectReplaced(const std::string &object_na
 	Object target_model = obj_database_->getObjectProperty(model_name);
 	col_obj->setCollisionShape(target_model.getCollisionShape());
 	col_obj->setWorldTransform(object_pose);
+
 	// Replace AABB with OBB!!
 	OverlappingObjectSensor result(*col_obj, object_name);
 	this->physics_engine_->contactTest(col_obj,result);
diff --git a/src/sequential_scene_parsing.cpp b/src/sequential_scene_parsing.cpp
@@ -18,7 +18,8 @@ void SceneHypothesisAssessor::setPhysicsEngine(PhysicsEngine* physics_engine)
 	this->physics_engine_ready_ = (physics_engine_ != NULL);
 	if (this->physics_engine_ready_)
 	{
-		this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME + RUN_UNTIL_HAVE_SUPPORT_GRAPH);
+		this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME + RUN_UNTIL_HAVE_SUPPORT_GRAPH,
+			GRAVITY_SCALE_COMPENSATION/(120. * SIMULATION_FREQUENCY_MULTIPLIER), GRAVITY_SCALE_COMPENSATION*10);
 	}
 
 	sequential_scene_hypothesis_.setPhysicsEngine(physics_engine);
@@ -208,11 +209,11 @@ std::map<std::string, ObjectParameter> SceneHypothesisAssessor::getCorrectedObje
 	seq_mtx_.lock();
 	this->data_forces_generator_.resetCachedIcpResult();
 
-	this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME,GRAVITY_SCALE_COMPENSATION/120.,
+	this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME,GRAVITY_SCALE_COMPENSATION/(120. * SIMULATION_FREQUENCY_MULTIPLIER),
 		GRAVITY_SCALE_COMPENSATION*10);
 	
 	this->physics_engine_->stepSimulationWithoutEvaluation(0.10 * GRAVITY_SCALE_COMPENSATION, 
-		GRAVITY_SCALE_COMPENSATION/120.);
+		GRAVITY_SCALE_COMPENSATION/(120. * SIMULATION_FREQUENCY_MULTIPLIER));
 	this->getUpdatedSceneSupportGraph();
 
 	// this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME,GRAVITY_SCALE_COMPENSATION/120.,
@@ -270,7 +271,7 @@ std::map<std::string, ObjectParameter> SceneHypothesisAssessor::getCorrectedObje
 	map_string_transform original_pose_to_test;
 	if (include_prev_observation_)
 	{
-		this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME,GRAVITY_SCALE_COMPENSATION/120.,
+		this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME,GRAVITY_SCALE_COMPENSATION/(120. * SIMULATION_FREQUENCY_MULTIPLIER),
 			GRAVITY_SCALE_COMPENSATION*30);
 		hypotheses_to_test = 
 			sequential_scene_hypothesis_.generateObjectHypothesesWithPreviousKnowledge(object_test_pose_map_by_dist,
@@ -291,7 +292,7 @@ std::map<std::string, ObjectParameter> SceneHypothesisAssessor::getCorrectedObje
 		}
 
 		this->physics_engine_->stepSimulationWithoutEvaluation(0.25 * GRAVITY_SCALE_COMPENSATION, 
-			GRAVITY_SCALE_COMPENSATION/120.);
+			GRAVITY_SCALE_COMPENSATION/(120. * SIMULATION_FREQUENCY_MULTIPLIER));
 		this->getUpdatedSceneSupportGraph();
 
 		// resimulate based on the object structure distance to get the final hypothesis
@@ -306,10 +307,10 @@ std::map<std::string, ObjectParameter> SceneHypothesisAssessor::getCorrectedObje
 				this->physics_engine_->addExistingRigidBodyBackFromMap(it->first,original_pose_to_test[it->first]);
 			}
 			this->physics_engine_->stepSimulationWithoutEvaluation(0.5 * GRAVITY_SCALE_COMPENSATION, 
-				GRAVITY_SCALE_COMPENSATION/120.);
+				GRAVITY_SCALE_COMPENSATION/(120. * SIMULATION_FREQUENCY_MULTIPLIER));
 		}
-		this->physics_engine_->stepSimulationWithoutEvaluation(1.0 * GRAVITY_SCALE_COMPENSATION, 
-			GRAVITY_SCALE_COMPENSATION/120.);
+		this->physics_engine_->stepSimulationWithoutEvaluation(1.5 * GRAVITY_SCALE_COMPENSATION, 
+			GRAVITY_SCALE_COMPENSATION/(120. * SIMULATION_FREQUENCY_MULTIPLIER));
 		this->getUpdatedSceneSupportGraph();
 
 		this->physics_engine_->changeBestTestPoseMap(this->physics_engine_->getCurrentObjectPoses());
@@ -397,6 +398,12 @@ void SceneHypothesisAssessor::setObjectSymmetryMap(const std::map<std::string, O
 	this->object_symmetry_map_ = object_symmetry_map;
 }
 
+SceneSupportGraph SceneHypothesisAssessor::getSceneGraphData(std::map<std::string, vertex_t> &vertex_map) const
+{
+	vertex_map = this->vertex_map_;
+	return this->scene_support_graph_;
+}
+
 void SceneHypothesisAssessor::setDebug(bool debug)
 {
 	this->debug_messages_ = debug;
@@ -744,7 +751,7 @@ void SceneHypothesisAssessor::evaluateAllObjectHypothesisProbability()
 	}
 	// set the position to the best result for all objects
 	this->physics_engine_->prepareSimulationForWithBestTestPose();
-	this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME,GRAVITY_SCALE_COMPENSATION/120.,
+	this->physics_engine_->setSimulationMode(RESET_VELOCITY_ON_EACH_FRAME,GRAVITY_SCALE_COMPENSATION/(120. * SIMULATION_FREQUENCY_MULTIPLIER),
 		GRAVITY_SCALE_COMPENSATION*30);
 	// this->physics_engine_->stepSimulationWithoutEvaluation(.75, 1/120.);
 	this->getUpdatedSceneSupportGraph();

Original file line number	Diff line number	Diff line change
`@@ -66,6 +66,8 @@ class SceneHypothesisAssessor`
`66`	`66`	`btScalar(forces_magnitude_per_point),btScalar(max_point_distance_threshold));`
`67`	`67`	`}`
`68`	`68`
	`69`	`+ SceneSupportGraph getSceneGraphData(std::map<std::string, vertex_t> &vertex_map) const;`
	`70`	`+`
`69`	`71`	`ObjectDatabase obj_database_;`
`70`	`72`
`71`	`73`	`private:`
Original file line number	Diff line number	Diff line change
`@@ -346,10 +346,14 @@ PointCloudXYZPtr FeedbackDataForcesGenerator::generateCorrespondenceCloud(PointC`
`346`	`346`	`{`
`347`	`347`	`std::vector< int > k_indices(1);`
`348`	`348`	`std::vector< float > distances(1);`
`349`		`- this->scene_data_tree_.nearestKSearch(*input_cloud, i, 1, k_indices, distances);`
`350`		`- if (!use_filter_distance \|\| (use_filter_distance && distances[0] < max_squared_distance))`
	`349`	`+`
	`350`	`+ // any neighbor is found`
	`351`	`+ if (this->scene_data_tree_.nearestKSearch(*input_cloud, i, 1, k_indices, distances) > 0)`
`351`	`352`	`{`
`352`		`- nearest_point_correspondence_cloud->points.push_back(scene_data_->points[k_indices[0]]);`
	`353`	`+ if (!use_filter_distance \|\| (use_filter_distance && distances[0] < max_squared_distance))`
	`354`	`+ {`
	`355`	`+ nearest_point_correspondence_cloud->points.push_back(scene_data_->points[k_indices[0]]);`
	`356`	`+ }`
`353`	`357`	`}`
`354`	`358`	`}`
`355`	`359`	`nearest_point_correspondence_cloud->width = nearest_point_correspondence_cloud->size();`