Example #1
0
void BoundingBox3DTest::testSimpleOrientedBox() {
	BoundingBox3DExtractor boundingBoxExtractor;
	brics_3d::Vector3D resultBoxDimensions;
	HomogeneousMatrix44 resultTransform;

	const double* matrixData;
	matrixData = resultTransform.getRawData();

	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, matrixData[12], maxTolerance); //preconditions
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, matrixData[13], maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, matrixData[14], maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, resultBoxDimensions.getX(), maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, resultBoxDimensions.getY(), maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, resultBoxDimensions.getZ(), maxTolerance);

	/* get bounding box */
	boundingBoxExtractor.computeOrientedBoundingBox(testCloudUnitCube, &resultTransform, resultBoxDimensions);
//	std::cout << "computeOrientedBoundingBox transform " << std::endl << resultTransform << std::endl;

	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, matrixData[12], maxTolerance); //postconditions
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, matrixData[13], maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, matrixData[14], maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, resultBoxDimensions.getX(), maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, resultBoxDimensions.getY(), maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, resultBoxDimensions.getZ(), maxTolerance);



	/* take the other point cloud */
	brics_3d::Vector3D resultBoxDimensions2;
	HomogeneousMatrix44 resultTransform2;
	const double* matrixData2;
	matrixData2 = resultTransform2.getRawData();

	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, matrixData2[12], maxTolerance); //preconditions
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, matrixData2[13], maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, matrixData2[14], maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, resultBoxDimensions2.getX(), maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, resultBoxDimensions2.getY(), maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, resultBoxDimensions2.getZ(), maxTolerance);

	boundingBoxExtractor.computeOrientedBoundingBox(testCloud, &resultTransform, resultBoxDimensions2);

//	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, matrixData2[12], maxTolerance); //postconditions
//	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, matrixData2[13], maxTolerance);
//	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, matrixData2[14], maxTolerance);
	CPPUNIT_ASSERT(resultBoxDimensions2.getX() < 10);
	CPPUNIT_ASSERT(resultBoxDimensions2.getY() < 10);
	CPPUNIT_ASSERT(resultBoxDimensions2.getZ() < 10);
//	CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, resultBoxDimensions2.getX(), maxTolerance);
//	CPPUNIT_ASSERT_DOUBLES_EQUAL(5.0, resultBoxDimensions2.getY(), maxTolerance);
//	CPPUNIT_ASSERT_DOUBLES_EQUAL(5.0, resultBoxDimensions2.getZ(), maxTolerance);

}
JNIEXPORT jdouble JNICALL Java_be_kuleuven_mech_rsg_jni_RsgJNI_getTransformElement
  (JNIEnv *, jclass, jint index, jlong transformPtr) {

	HomogeneousMatrix44* transform = reinterpret_cast<HomogeneousMatrix44*>(transformPtr);
	assert(transform != 0);
	const double* matrixData = transform->getRawData();
	if (index < 0 || index >= 16) {
		LOG(ERROR) << "Index for HomogeneousMatrix44 is out of bounds: " << index;
		return -1;
	} else {
		return matrixData[index];
	}
}