Limit overlaps to 0.3 object bounding box volume

Author: fjonath1

include/scene_physics_engine.h

@@ -177,6 +177,7 @@ struct OverlappingObjectSensor : public btCollisionWorld::ContactResultCallback
 	std::string object_id_;
 	double total_intersecting_volume_;
 	double total_penetration_depth_;
+	double bounding_box_volume_;
 
 	//! Called with each contact for your own processing (e.g. test if contacts fall in within sensor parameters)
 	virtual btScalar addSingleResult(btManifoldPoint& cp,
@@ -188,25 +189,30 @@ struct OverlappingObjectSensor : public btCollisionWorld::ContactResultCallback
 
 		btVector3 pt; // will be set to point of collision relative to body
 		std::string other_id;
+		bool other_object_is_1;
 		if (colObj0->m_collisionObject==&body_)
 		{
 			pt = cp.m_localPointA;
+			other_object_is_1 = true;
 			other_id = getObjectIDFromCollisionObject(obj_1);
 		}
 		else
 		{
 			assert(colObj1->m_collisionObject==&body_ && "body does not match either collision object");
 			pt = cp.m_localPointB;
+			other_object_is_1 = false;
 			other_id = getObjectIDFromCollisionObject(obj_0);
 		}
 
+        btAABB shapeAABB_0 = getCollisionAABB(colObj0->getCollisionObject(), cp, true, index0);
+        btAABB shapeAABB_1 = getCollisionAABB(colObj1->getCollisionObject(), cp, false, index1);
+		bounding_box_volume_ = other_object_is_1 ? getBoundingBoxVolume(shapeAABB_0) : getBoundingBoxVolume(shapeAABB_1);
 		if (other_id == "unrecognized_object" || other_id == "background" || other_id == object_id_) return 0;
 
 		// do stuff with the collision point
 		total_penetration_depth_ += cp.getDistance() < 0 ? -cp.getDistance() : 0;
-        btAABB shapeAABB_0 = getCollisionAABB(colObj0->getCollisionObject(), cp, true, index0);
-        btAABB shapeAABB_1 = getCollisionAABB(colObj1->getCollisionObject(), cp, false, index1);
-
+        
+		
         total_intersecting_volume_ += getIntersectingVolume(shapeAABB_0,shapeAABB_1);
 
 		return 0; // There was a planned purpose for the return value of addSingleResult, but it is not used so you can ignore it.

include/scene_physics_support.h

@@ -124,6 +124,8 @@ void assignAllConnectedToParentVertices(SceneSupportGraph &input_graph, const ve
 
 btAABB getCollisionAABB(const btCollisionObject* obj, const btManifoldPoint &pt, const bool &is_body_0, int &shape_index);
 
+double getBoundingBoxVolume(const btAABB &shapeAABB);
+
 double getIntersectingVolume(const btAABB &shapeAABB_a, const btAABB &shapeAABB_b);
 
 SceneSupportGraph generateObjectSupportGraph(btDynamicsWorld *world, 

src/scene_physics_support.cpp

@@ -155,7 +155,13 @@ double getIntersectingVolume(const btAABB &shapeAABB_a, const btAABB &shapeAABB_
 {
     btAABB intersecting_box;
     shapeAABB_a.find_intersection(shapeAABB_b,intersecting_box);
-    btVector3 &m_min = intersecting_box.m_min, &m_max = intersecting_box.m_max;
+
+    return getBoundingBoxVolume(intersecting_box);
+}
+
+double getBoundingBoxVolume(const btAABB &shapeAABB)
+{
+    const btVector3 &m_min = shapeAABB.m_min, &m_max = shapeAABB.m_max;
     double volume = 1;
     for (int i = 0; i < 3 ; i++) volume *= m_max[i] - m_min[i];
     return std::abs(volume);

src/sequential_scene_hypothesis.cpp

@@ -214,7 +214,7 @@ bool SequentialSceneHypothesis::checkObjectObstruction(const std::string &model_
 	}
 
 	std::cerr << " number of obstructed points: " << num_point_obstructed << "/" << model_cloud->size() << std::endl;
-	return num_point_obstructed > 0.6 * model_cloud->size();
+	return num_point_obstructed > 0.4 * model_cloud->size();
 }
 
 bool SequentialSceneHypothesis::checkObjectReplaced(const std::string &object_name,
@@ -247,14 +247,15 @@ 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);
 
 	delete col_obj;
-	std::cerr << "Overlaps: " << result.total_penetration_depth_ << "; " << result.total_intersecting_volume_ << std::endl;
+	std::cerr << "Overlaps: " << result.total_penetration_depth_ << "; " << result.total_intersecting_volume_ 
+		<< " >? " << 0.3 * result.bounding_box_volume_ << std::endl;
 
-	return false;
+	return result.total_intersecting_volume_ > 0.3 * result.bounding_box_volume_;
 }