Improved the accuracy of quick scene estimate

Add an ability to costumize the gravity compensation for small objects using ros param.
Automatically load missing object models when these objects are detected

Author: fjonath1

include/physics_world_parameters.h

@@ -7,8 +7,8 @@
 
 #define SCALING 1000
 #define GRAVITY_MAGNITUDE 9.807
-#define GRAVITY_SCALE_COMPENSATION 2
-#define SCALED_GRAVITY_MAGNITUDE SCALING * GRAVITY_MAGNITUDE / GRAVITY_SCALE_COMPENSATION
+static int GRAVITY_SCALE_COMPENSATION = 3;
+static double SCALED_GRAVITY_MAGNITUDE = SCALING * GRAVITY_MAGNITUDE / GRAVITY_SCALE_COMPENSATION;
 
 enum SimulationMode {
 	//  RUN THE SIMULATION UNTIL THE NUMBER OF SIMULATION STEP IS REACHED OR ALL OBJECT IS IN DEACTIVATED STATE

include/scene_data_forces.h

@@ -2,6 +2,8 @@
 #define SCENE_PHYSICS_DATA_FORCES
 
 #include <iostream>
+
+#include <boost/filesystem.hpp>
 #include <btBulletDynamicsCommon.h>
 
 #include <pcl/point_types.h>
@@ -40,14 +42,17 @@ class FeedbackDataForcesGenerator
 
 	// Input sampled point cloud from the mesh
 	void setModelCloud(const PointCloudXYZPtr mesh_surface_sampled_cloud, const std::string &model_name);
+	bool setModelDirectory(const std::string &model_directory);
+	bool setModelCloud(const std::string &model_name);
+
 	void setForcesParameter(const btScalar &forces_magnitude_per_point, const btScalar &max_point_distance_threshold);
 	void resetCachedIcpResult();
 	void removeCachedIcpResult(const std::string &object_id);
 	void updateCachedIcpResultMap(const btRigidBody &object, 
 		const std::string &model_name);
 	void manualSetCachedIcpResultMapFromPose(const btRigidBody &object, 
 		const std::string &model_name);
-	double getIcpConfidenceResult(const std::string &model_name, const btTransform &object_pose) const;
+	double getIcpConfidenceResult(const std::string &model_name, const btTransform &object_pose);
 
 	PointCloudXYZPtr getTransformedObjectCloud(const std::string &model_name, const btTransform &object_real_pose) const;
 
@@ -80,6 +85,8 @@ class FeedbackDataForcesGenerator
 	PointCloudXYZPtr getTransformedObjectCloud(const btTransform &object_pose, 
 		const std::string &model_name, btTransform &object_real_pose) const;
 
+	std::string model_directory_;
+
 	bool have_scene_data_;
 	PointCloudXYZPtr scene_data_;
 	pcl::KdTreeFLANN<pcl::PointXYZ> scene_data_tree_;

launch/ros_scene.launch

@@ -13,19 +13,22 @@
   <arg name="debug"                          default="false"/>
   <arg name="load_table"                     default="true"/>
   <arg name="table_location"                 default="$(find sequential_scene_parsing)/mesh/table.pcd"/>
-  <arg name="render_scene"                   default="true" />
+  <arg name="render_scene"                   default="true"/>
   <arg name="objransac_model_directory"      default="$(find sequential_scene_parsing)/mesh"/>
-  <arg name="objransac_model_names"          default="block,cube" />
+  <arg name="objransac_model_names"          default="block,cube"/>
 
   <arg name="data_forces_magnitude"          default="0.25"/>
   <arg name="data_forces_max_distance"       default="0.015"/>
 
   <arg name="best_hypothesis_only"           default="false"/>
+  <arg name="small_obj_g_comp"               default="3"/>
 
   <node pkg="sequential_scene_parsing" type="sequential_scene_ros" name="sequential_scene_ros"
   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="background_pcl2_topic"   type="str"     value="$(arg background_pcl2_topic)" />
     <param name="scene_pcl2_topic"        type="str"     value="$(arg scene_pcl2_topic)" />
 
@@ -50,7 +53,6 @@
     <param name="objransac_model_directory"    type="str"     value="$(arg objransac_model_directory)"/>
     <param name="objransac_model_names"        type="str"     value="$(arg objransac_model_names)"/>
 
-
     <!-- objectDatabase -->
     <rosparam command="load" file="$(find sequential_scene_parsing)/mesh/object_property_database.yaml" />
 

script/publish_groundtruth_hypotheses.py

@@ -85,6 +85,8 @@ def __init__(self,folder_name):
 			y_symmetries = 1, y_rotation = 360*deg_to_rad,
 			z_symmetries = 1, z_rotation = 360*deg_to_rad)
 
+		self.object_id_by_frame_ = dict()
+		objects_in_one_frame = []
 		for row in groundtruth_tf_list:
 			# print len(row)
 			if len(row) == 10:
@@ -99,6 +101,7 @@ def __init__(self,folder_name):
 					## NEED TO ADD THE TIME STAMP WHEN PUBLISHING
 
 					self.detected_object_in_frame.append((current_frame_list_of_objects,current_frame_tf_accumulator))
+					self.object_id_by_frame_[current_frame] = objects_in_one_frame
 					current_frame_tf_accumulator = []
 					current_frame_list_of_objects = DetectedObjectList()
 					current_frame = int(frame_num)
@@ -114,6 +117,9 @@ def __init__(self,folder_name):
 
 				det_object.id = "objects/%s/%s"%(object_type,obj_index)
 				det_object.object_class = object_type
+
+				objects_in_one_frame.append(det_object.id)
+
 				if object_type in obj_symmetry_dict:
 					det_object.symmetry = obj_symmetry_dict[object_type]
 				else:
@@ -136,11 +142,11 @@ def __init__(self,folder_name):
 			current_frame_list_of_objects.header.frame_id = ref_frame
 
 			self.detected_object_in_frame.append((current_frame_list_of_objects,current_frame_tf_accumulator))
+			self.object_id_by_frame_[current_frame] = objects_in_one_frame
 			current_frame_tf_accumulator = []
 			current_frame_list_of_objects = DetectedObjectList()
 			current_frame = int(frame_num)
 
-
 		self.saved_cloud = list()
 		cloud_dir = os.path.join(test_dir_path,'seg_cloud')
 
@@ -245,15 +251,17 @@ def publish_detected_object_msgs(self,req):
 			# print "Obj list published"
 			rospy.sleep(0.3)
 
-			self.hypo_obj_pub.publish(self.hypothesis_generator(current_frame_tf_accumulator))
+			# self.hypo_obj_pub.publish(self.hypothesis_generator(current_frame_tf_accumulator))
 			# print "all data published"
 			self.seq += 1
 		return []
 
 	def evaluate_scene_result(self,data):
 		obj_list_to_publish, current_frame_tf_accumulator = self.detected_object_in_frame[self.frame_to_publish]
-		for detected_object in obj_list_to_publish.objects:
-			object_id = detected_object.id
+		available_objects = self.object_id_by_frame_[self.frame_to_publish]
+		for detected_object in available_objects: # obj_list_to_publish.objects:
+			# object_id = detected_object.id
+			object_id = detected_object
 			result_id = 'scene/'+object_id
 			transform = [-1]*7
 			self.listener.waitForTransform(object_id,result_id,rospy.Time(), rospy.Duration(1.0))

src/ros_sequential_scene_parsing.cpp

@@ -64,6 +64,8 @@ void RosSceneHypothesisAssessor::setNodeHandle(const ros::NodeHandle &nh)
 	nh.param("data_forces_max_distance",data_forces_max_distance,0.01);
 
 	nh.param("best_hypothesis_only",best_hypothesis_only_,false);
+	nh.param("small_obj_g_comp",GRAVITY_SCALE_COMPENSATION,3);
+	SCALED_GRAVITY_MAGNITUDE = SCALING * GRAVITY_MAGNITUDE / GRAVITY_SCALE_COMPENSATION;
 
 	if (load_table){
 		nh.param("table_location",background_location,std::string(""));

src/scene_data_forces.cpp

@@ -21,6 +21,51 @@ FeedbackDataForcesGenerator::FeedbackDataForcesGenerator() :
 	icp_.setMaximumIterations(max_icp_iteration_);
 }
 
+bool FeedbackDataForcesGenerator::setModelDirectory(const std::string &model_directory)
+{
+	namespace fs = boost::filesystem;
+	fs::path directory_path(model_directory);
+	if (fs::is_directory(directory_path))
+	{
+		model_directory_ = model_directory;
+		return true;
+	}
+	else
+	{
+		std::cerr << "Error, object model directory: " << model_directory << " does not exist.\n";
+		return false;
+	}
+}
+
+bool FeedbackDataForcesGenerator::setModelCloud(const std::string &model_name)
+{
+	namespace fs = boost::filesystem;
+	fs::path directory_path(model_directory_);
+	fs::path model_path = directory_path / model_name;
+	model_path.replace_extension(".pcd");
+
+	if (fs::exists(model_path))
+	{
+		pcl::PCDReader reader;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr surface_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+		if (reader.read(model_path.string(),*surface_cloud) == 0)
+		{
+			this->setModelCloud(surface_cloud, model_name);
+			std::cerr << "Model point cloud loaded successfully\n";
+			return true;
+		}
+		else
+		{
+			std::cerr << "Failed to load " << model_name << " model point cloud\n";
+		}
+	}
+	else
+	{
+		std::cerr << "Error: '" << model_path << "' does not exist.\n";	 
+	}
+	return false;
+}
+
 void FeedbackDataForcesGenerator::setFeedbackForceMode(int mode)
 {
 	force_data_model_ = mode;
@@ -97,19 +142,22 @@ void FeedbackDataForcesGenerator::updateCachedIcpResultMap(const PointCloudXYZPt
 	icp_result_confidence_map_[object_id] = getIcpConfidenceResult(icp_result);
 }
 
-double FeedbackDataForcesGenerator::getIcpConfidenceResult(const std::string &model_name, const btTransform &object_pose) const
+double FeedbackDataForcesGenerator::getIcpConfidenceResult(const std::string &model_name, const btTransform &object_pose)
 {
 	btTransform dummy_transform;
-	if (keyExistInConstantMap(model_name,model_cloud_map_))
-	{
-		PointCloudXYZPtr dummy =  getTransformedObjectCloud(object_pose, model_name, dummy_transform);
-		return getIcpConfidenceResult(dummy);	
-	}
-	else
+	if (!keyExistInConstantMap(model_name,model_cloud_map_))
 	{
 		std::cerr << "ERROR, model name '" << model_name << "' does not exist in the database.\n";
-		return 0;
+		std::cerr << "Attempting to load Unrecognized model\n";
+		bool load_success = this->setModelCloud(model_name);
+		if (!load_success)
+		{
+			return 0;
+		}
 	}
+
+	PointCloudXYZPtr dummy =  getTransformedObjectCloud(object_pose, model_name, dummy_transform);
+	return getIcpConfidenceResult(dummy);
 }
 
 void FeedbackDataForcesGenerator::updateCachedIcpResultMap(const btRigidBody &object, 

src/sequential_scene_hypothesis.cpp

@@ -209,6 +209,7 @@ bool SequentialSceneHypothesis::checkObjectReplaced(const std::string &model_nam
 {
 	// use AABB to check if the original object volume has been occupied by something else
 	// TODO: IMPLEMENT THIS!!
+	// use bullet contactTest;
 	return false;
 }
 
@@ -246,32 +247,67 @@ std::map<std::string, AdditionalHypotheses> SequentialSceneHypothesis::generateO
 
 	std::map<std::string, std::string> &object_label_class_map = this->previous_scene_observation_.object_label_class_map_;
 
+	for (std::vector<std::string>::const_iterator it = change_in_scene.perturbed_objects_.begin();
+		it != change_in_scene.perturbed_objects_.end(); ++it)
+	{
+		const std::string &object_id = *it;
+		const vertex_t &prev_obj_vertex = prev_vertex_map[object_id];
+		const btTransform &prev_transform = previous_best_scene_hypothesis.scene_support_graph_[prev_obj_vertex].object_pose_;
+
+		const vertex_t &cur_obj_vertex = cur_vertex_map[object_id];
+		const std::size_t dist = current_best_data_scene_structure_.scene_support_graph_[cur_obj_vertex].distance_to_ground_;
+		const btTransform &cur_transform = current_best_data_scene_structure_.scene_support_graph_[cur_obj_vertex].object_pose_;
+		
+		const std::string &model_name = object_label_class_map[object_id];
+		
+		std::cerr << object_id << std::endl;
+
+		double current_data_confidence = this->data_probability_check_->getIcpConfidenceResult(model_name, cur_transform);
+		// use previous best pose if the confidence difference between current pose and previous pose is small
+		if (current_data_confidence < 0.05 || this->judgeHypothesis(model_name,prev_transform,0.7*current_data_confidence))
+		{
+			std::cerr << "Retained previous pose\n";
+			object_pose_by_dist[dist][object_id] = prev_transform;
+		}
+		else
+		{
+			std::cerr << "Use current pose\n";	
+			object_pose_by_dist[dist][object_id] = cur_transform;
+		}
+	}
+
 	for (std::vector<std::string>::const_iterator it = change_in_scene.support_retained_object_.begin();
 		it != change_in_scene.support_retained_object_.end(); ++it)
 	{
 		// add the object pose by distance information for support retained objects
 		const std::string &object_id = *it;
 		const vertex_t &obj_vertex = prev_vertex_map[object_id];
-		
+
 		// the object_pose_by_dist starts at 0 (already excludes background)
-		const std::size_t dist = previous_best_scene_hypothesis.scene_support_graph_[obj_vertex].distance_to_ground_ - 1;
+		const std::size_t prev_dist = previous_best_scene_hypothesis.scene_support_graph_[obj_vertex].distance_to_ground_;
+		const std::size_t dist = prev_dist > 1 && prev_dist <= object_pose_by_dist.size() 
+			? prev_dist - 1 : disconnected_object_dist;
+
 		const btTransform &prev_transform = previous_best_scene_hypothesis.scene_support_graph_[obj_vertex].object_pose_;
 
 		const std::string &model_name = object_label_class_map[object_id];
 
 		const vertex_t &cur_obj_vertex = cur_vertex_map[object_id];
 		const btTransform &cur_transform = current_best_data_scene_structure_.scene_support_graph_[cur_obj_vertex].object_pose_;
+		
+		std::cerr << object_id << std::endl;
 
 		double current_data_confidence = this->data_probability_check_->getIcpConfidenceResult(model_name, cur_transform);
-
 		// use previous best pose if the confidence difference between current pose and previous pose is small
 		if (current_data_confidence < 0.05 || this->judgeHypothesis(model_name,prev_transform,0.7*current_data_confidence))
 		{
-			object_pose_by_dist[dist][object_id] = prev_transform;	
+			std::cerr << "Retained previous pose\n";
+			object_pose_by_dist[dist][object_id] = prev_transform;
 		}
 		else
 		{
-			object_pose_by_dist[dist][object_id] = cur_transform;	
+			std::cerr << "Use current pose\n";	
+			object_pose_by_dist[dist][object_id] = cur_transform;
 		}
 
 		// add the child information of this object and move the optimization order of the child to its proper distance
@@ -310,10 +346,12 @@ std::map<std::string, AdditionalHypotheses> SequentialSceneHypothesis::generateO
 			else
 			{
 				std::cerr << "Warning, found inconsistency of object " << *obj_it << " in current scene graph\n";
+				continue;
 			}
 		}
 		object_childs_map[object_id] = obj_child_tf_supported;
 	}
+
 	if (additional_perturbed.size() > 0) result.insert(additional_perturbed.begin(),additional_perturbed.end());
 	if (additional_static.size() > 0) result.insert(additional_static.begin(),additional_static.end());
 	if (additional_support_retained.size() > 0) result.insert(additional_support_retained.begin(),additional_support_retained.end());
@@ -332,6 +370,7 @@ std::map<std::string, AdditionalHypotheses> SequentialSceneHypothesis::generateO
 			result[it->first] = AdditionalHypotheses(it->second.first, object_pose_hypotheses, ADD_OBJECT);
 		}
 	}
+
 	return result;
 }