static void
computeBestFitOBB(TheaArray<Vector3> const & points, Box3 & result, bool has_up, Vector3 const & up)
{
if (points.empty())
{
result = Box3();
return;
}
else if (points.size() == 1)
{
result = Box3(AxisAlignedBox3(points[0]));
return;
}
Vector3 centroid;
CoordinateFrame3 cframe;
if (has_up)
{
centroid = CentroidN<Vector3, 3>::compute(points.begin(), points.end());
Vector3 u, v;
up.createOrthonormalBasis(u, v);
cframe = CoordinateFrame3::_fromAffine(AffineTransform3(basisMatrix(u, v, up), centroid));
}
else
{
Plane3 plane;
LinearLeastSquares3<Vector3>::fitPlane(points.begin(), points.end(), plane, ¢roid);
planeToCFrame(plane, centroid, cframe);
}
OBB best_obb;
computeOBB(points, cframe, best_obb, true);
Matrix3 rot = Matrix3::rotationAxisAngle((has_up ? up : Vector3::unitY()), Math::degreesToRadians(10));
for (int a = 10; a < 180; a += 10)
{
cframe._setRotation(cframe.getRotation() * rot);
computeOBB(points, cframe, best_obb, false);
}
result = Box3(AxisAlignedBox3(best_obb.lo, best_obb.hi), best_obb.cframe);
}
RigidTransform3
closestRigidTransform(std::vector<CoordinateFrame3> const & src, std::vector<CoordinateFrame3> const & dst)
{
alwaysAssertM(src.size() == dst.size(), "Source and destination sets must have same number of frames");
if (src.empty())
return RigidTransform3::identity();
else if (src.size() == 1)
return RigidTransform3(dst[0]) * RigidTransform3(src[0].inverse());
else if (src.size() == 2)
{
// First map line segment to line segment...
Vector3 src_mean = 0.5f * (src[0].getTranslation() + src[1].getTranslation());
Vector3 dst_mean = 0.5f * (dst[0].getTranslation() + dst[1].getTranslation());
static Real const MIN_SQLEN = 1.0e-8f;
Vector3 src_axis = src[1].getTranslation() - src[0].getTranslation();
Vector3 dst_axis = dst[1].getTranslation() - dst[0].getTranslation();
if (src_axis.squaredLength() > MIN_SQLEN && dst_axis.squaredLength() > MIN_SQLEN)
{
Matrix3 rot = Matrix3::rotationArc(src_axis, dst_axis);
// Now rotate around the mapped segment to align the z axes of the frames...
Vector3 src_dir = rot * (src[0].getRotation().getColumn(2) + src[1].getRotation().getColumn(2));
Vector3 dst_dir = dst[0].getRotation().getColumn(2) + dst[1].getRotation().getColumn(2);
// Transform src_dir and dst_dir to be perpendicular to dst_axis
dst_axis = dst_axis.fastUnit();
src_dir = src_dir - src_dir.dot(dst_axis) * dst_axis;
dst_dir = dst_dir - dst_dir.dot(dst_axis) * dst_axis;
if (src_dir.squaredLength() > MIN_SQLEN && dst_dir.squaredLength() > MIN_SQLEN)
{
src_dir = src_dir.fastUnit();
dst_dir = dst_dir.fastUnit();
Vector3 src_perp = dst_axis.cross(src_dir);
Vector3 dst_perp = dst_axis.cross(dst_dir);
Matrix3 src_basis = basisMatrix(src_dir, src_perp, dst_axis);
Matrix3 dst_basis = basisMatrix(dst_dir, dst_perp, dst_axis);
Matrix3 extra_rot = dst_basis * src_basis.transpose(); // inverse == tranpose for rotation matrices
rot = extra_rot * rot;
}
CoordinateFrame3 cf(RigidTransform3::_fromAffine(AffineTransform3(rot, dst_mean - rot * src_mean)));
// qDebug().nospace() << "R = " << cf.getRotation().toString() << ", T = " << cf.getTranslation().toString();
return RigidTransform3(cf);
}
else
return RigidTransform3::translation(dst_mean - src_mean);
}
// ICP algorithm of Arun et al. 1987.
size_t num_frames = src.size();
Vector3 src_mean = Vector3::zero(), dst_mean = Vector3::zero();
for (size_t i = 0; i < num_frames; ++i)
{
CoordinateFrame3 const & src_frame = src[i];
CoordinateFrame3 const & dst_frame = dst[i];
// qDebug().nospace() << "src[" << i << "] = R: " << src_frame.getRotation().toString() << ", T: "
// << src_frame.getTranslation().toString();
// qDebug().nospace() << "dst[" << i << "] = R: " << dst_frame.getRotation().toString() << ", T: "
// << dst_frame.getTranslation().toString();
src_mean += src_frame.getTranslation();
dst_mean += dst_frame.getTranslation();
}
src_mean /= num_frames;
dst_mean /= num_frames;
Matrix3 corr = Matrix3::zero();
Vector3 src_pt, dst_pt;
for (size_t i = 0; i < num_frames; ++i)
{
CoordinateFrame3 const & src_frame = src[i];
CoordinateFrame3 const & dst_frame = dst[i];
src_pt = src_frame.getTranslation() - src_mean;
dst_pt = dst_frame.getTranslation() - dst_mean;
for (int r = 0; r < 3; ++r)
for (int c = 0; c < 3; ++c)
corr(r, c) += src_pt[r] * dst_pt[c];
}
Matrix3 U, V;
TheaArray<Real> diag;
Algorithms::SVD::compute(corr, U, diag, V);
Matrix3 U_T = U.transpose();
Matrix3 rotation = V * U_T;
if (rotation.determinant() < 0)
{
// FIXME: One of the columns (which?) of V should be negated. See Eggert et al. '97, "Estimating 3D rigid transformations: a
// comparison of four major algorithms".
//
// For now, we'll do the dumb but safe thing and test each option for the minimum error.
THEA_WARNING << "Estimated rigid transform involves reflection, fixing by negating a column of V";
Real min_sqerr = -1;
for (int i = 0; i < 3; ++i)
{
// Swap i'th column of V
Matrix3 V_i = V;
V_i(0, i) = -V_i(0, i);
V_i(1, i) = -V_i(1, i);
V_i(2, i) = -V_i(2, i);
Matrix3 candidate_rot = V_i * U_T;
Real sqerr = 0;
for (size_t j = 0; j < num_frames; ++j)
{
CoordinateFrame3 const & src_frame = src[j];
CoordinateFrame3 const & dst_frame = dst[j];
src_pt = src_frame.getTranslation() - src_mean;
dst_pt = dst_frame.getTranslation() - dst_mean;
sqerr += (candidate_rot * src_pt - dst_pt).squaredLength();
}
if (min_sqerr < 0 || sqerr < min_sqerr)
{
rotation = candidate_rot;
min_sqerr = sqerr;
}
}
}
Vector3 translation = dst_mean - rotation * src_mean;
CoordinateFrame3 cf(RigidTransform3::_fromAffine(AffineTransform3(rotation, translation)));
// qDebug().nospace() << "R = " << rotation.toString() << ", T = " << translation.toString();
return RigidTransform3(cf);
}