void HomogeneousMatrixTest::testIsIdentity() {
HomogeneousMatrix44 identity;
HomogeneousMatrix44 noIdentity(1,0,0, 0,1,0, 0,0,1, 1,2,3);
HomogeneousMatrix44 nearlyIdentity(1.001,0,0, 0,1,0, 0,0,0.999, 0,0,0.001);
CPPUNIT_ASSERT(identity.isIdentity() == true);
CPPUNIT_ASSERT(noIdentity.isIdentity() == false);
CPPUNIT_ASSERT(nearlyIdentity.isIdentity() == false);
CPPUNIT_ASSERT(nearlyIdentity.isIdentity(0.01) == true); // check with less precision
}
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];
}
}
void BoundingBox3DExtractor::computeOrientedBoundingBox(PointCloud3D* inputPointCloud, IHomogeneousMatrix44* resultTransform, Vector3D& resultBoxDimensions) {
assert(resultTransform != 0);
lowerBound.setX(std::numeric_limits<brics_3d::Coordinate>::max());
lowerBound.setY(std::numeric_limits<brics_3d::Coordinate>::max());
lowerBound.setZ(std::numeric_limits<brics_3d::Coordinate>::max());
upperBound.setX(-std::numeric_limits<brics_3d::Coordinate>::max());
upperBound.setY(-std::numeric_limits<brics_3d::Coordinate>::max());
upperBound.setZ(-std::numeric_limits<brics_3d::Coordinate>::max());
Eigen::MatrixXd eigenvectors;
Eigen::VectorXd eigenvalues;
pcaExtractor.computePrincipleComponents(inputPointCloud, eigenvectors, eigenvalues); //actually we compute a centroid twice...
pcaExtractor.computeRotationMatrix(eigenvectors, eigenvalues, resultTransform);
/* get min/max for PCA rotated points */
HomogeneousMatrix44* inverseRotation = new HomogeneousMatrix44();
*inverseRotation = *(resultTransform);
inverseRotation->inverse();
for (unsigned int i = 0; i < inputPointCloud->getSize(); ++i) {
Point3D tmpPoint = (*inputPointCloud->getPointCloud())[i];
tmpPoint.homogeneousTransformation(inverseRotation); // move _all_ points to new frame
Point3D* currentPoint = &tmpPoint;
/* adjust lower bound if necessary */
if (currentPoint->getX() <= lowerBound.getX()) {
lowerBound.setX(currentPoint->getX());
}
if (currentPoint->getY() <= lowerBound.getY()) {
lowerBound.setY(currentPoint->getY());
}
if (currentPoint->getZ() <= lowerBound.getZ()) {
lowerBound.setZ(currentPoint->getZ());
}
/* adjust upper bound if necessary */
if ( currentPoint->getX() >= upperBound.getX()) {
upperBound.setX(currentPoint->getX());
}
if (currentPoint->getY() >= upperBound.getY()) {
upperBound.setY(currentPoint->getY());
}
if (currentPoint->getZ() >= upperBound.getZ()) {
upperBound.setZ(currentPoint->getZ());
}
}
delete inverseRotation;
/* get centroid */
Centroid3D centroidExtractor;
Eigen::Vector3d centroid;
centroid = centroidExtractor.computeCentroid(inputPointCloud);
/* as centroid as translation */
double* matrixData;
matrixData = resultTransform->setRawData();
//djd RC5 - don't use centroid as centre of bounding box if only returning dimensions - you will not be able to reconstruct it
matrixData[12] = centroid[0];
matrixData[13] = centroid[1];
matrixData[14] = centroid[2];
LOG(DEBUG) << "Estimated transform for oriented bounding box: "<< std::endl << *resultTransform;
resultBoxDimensions.setX(fabs(upperBound.getX() - lowerBound.getX()));
resultBoxDimensions.setY(fabs(upperBound.getY() - lowerBound.getY()));
resultBoxDimensions.setZ(fabs(upperBound.getZ() - lowerBound.getZ()));
// matrixData[12]=resultBoxDimensions.getX()/2+lowerBound.getX(); //FIXME
// matrixData[13]=resultBoxDimensions.getY()/2+lowerBound.getY();
// matrixData[14]=resultBoxDimensions.getZ()/2+lowerBound.getZ();
}
