Fix background normal direction to always be on opposite direction of z camera axis

Author: fjonath1

src/ros_sequential_scene_parsing.cpp

@@ -80,6 +80,9 @@ void RosSceneHypothesisAssessor::setNodeHandle(const ros::NodeHandle &nh)
 	nh.param("small_obj_g_comp",GRAVITY_SCALE_COMPENSATION,3);
 	nh.param("sim_freq_multiplier",SIMULATION_FREQUENCY_MULTIPLIER,1.);
 
+	std::cerr << "Debug mode: " << debug_mode << std::endl;
+	this->setDebugMode(debug_mode);
+
 	// physics engine solver settings: check http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo
 	int num_iterations;
 	bool split_impulse, randomize_order;
@@ -91,6 +94,8 @@ void RosSceneHypothesisAssessor::setNodeHandle(const ros::NodeHandle &nh)
 	this->physics_engine_.setPhysicsSolverSetting(num_iterations, randomize_order, 
 		int(split_impulse), impulse_penetration_threshold);
 
+	this->physics_engine_.setGravityFromBackgroundNormal(background_normal_as_gravity_);
+
 	SCALED_GRAVITY_MAGNITUDE = SCALING * GRAVITY_MAGNITUDE / GRAVITY_SCALE_COMPENSATION;
 
 	if (load_table){
@@ -106,9 +111,6 @@ void RosSceneHypothesisAssessor::setNodeHandle(const ros::NodeHandle &nh)
 			std::cerr << "Failed to load the background points\n"; 
 	}
 
-	std::cerr << "Debug mode: " << debug_mode << std::endl;
-	this->setDebugMode(debug_mode);
-
 	this->obj_database_.setObjectFolderLocation(object_folder_location);
 	if (this->fillObjectPropertyDatabase()) this->obj_database_.loadDatabase(this->physical_properties_database_);
 	this->physics_engine_.setObjectPenaltyDatabase(this->obj_database_.getObjectPenaltyDatabase());

src/sequential_scene_parsing.cpp

@@ -54,6 +54,13 @@ void SceneHypothesisAssessor::addBackground(ImagePtr background_image, int mode)
 	btVector3 normal_bt = convertEigenToBulletVector(normal);
 	float coeff = coefficients->values[3];
 
+	// make the plane normal to always point in opposite direction to the camera frame Z axis
+	if (normal_bt.dot(btVector3(0,0,1)) > 0)
+	{
+		normal_bt = -normal_bt;
+		coeff = -coeff;
+	}
+
 	Eigen::Vector4f cloud_centroid;
 	pcl::compute3DCentroid(*background_image,cloud_centroid);
 	btVector3 bt_cloud_centroid(cloud_centroid[0],cloud_centroid[1],cloud_centroid[2]);