67 lines
2.3 KiB
C++
67 lines
2.3 KiB
C++
/*
|
|
* SimilarityTransform.cpp
|
|
*
|
|
* Copyright (c) 2014-2022 SEACAVE
|
|
*
|
|
* Author(s):
|
|
*
|
|
* cDc <cdc.seacave@gmail.com>
|
|
*
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU Affero General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU Affero General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Affero General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*
|
|
*
|
|
* Additional Terms:
|
|
*
|
|
* You are required to preserve legal notices and author attributions in
|
|
* that material or in the Appropriate Legal Notices displayed by works
|
|
* containing it.
|
|
*/
|
|
|
|
#include "Common.h"
|
|
#include "SimilarityTransform.h"
|
|
|
|
|
|
// D E F I N E S ///////////////////////////////////////////////////
|
|
|
|
|
|
// S T R U C T S ///////////////////////////////////////////////////
|
|
|
|
// find the similarity transform that best aligns the given two sets of corresponding 3D points
|
|
bool SEACAVE::SimilarityTransform(const CLISTDEF0(Point3)& points, const CLISTDEF0(Point3)& pointsRef, Matrix4x4& transform)
|
|
{
|
|
ASSERT(points.size() == pointsRef.size());
|
|
typedef Eigen::Matrix<REAL,3,Eigen::Dynamic> PointsVec;
|
|
PointsVec p(3, points.size());
|
|
PointsVec pRef(3, pointsRef.size());
|
|
FOREACH(i, points) {
|
|
p.col(i) = static_cast<const Point3::EVec&>(points[i]);
|
|
pRef.col(i) = static_cast<const Point3::EVec&>(pointsRef[i]);
|
|
}
|
|
transform = Eigen::umeyama(p, pRef);
|
|
return true;
|
|
} // SimilarityTransform
|
|
/*----------------------------------------------------------------*/
|
|
|
|
void SEACAVE::DecomposeSimilarityTransform(const Matrix4x4& transform, Matrix3x3& R, Point3& t, REAL& s)
|
|
{
|
|
const Eigen::Transform<REAL,3,Eigen::Affine,Eigen::RowMajor> T(static_cast<const Matrix4x4::EMat&>(transform));
|
|
Eigen::Matrix<REAL,3,3> rotation, scaling;
|
|
T.computeRotationScaling(&rotation, &scaling);
|
|
R = rotation;
|
|
t = T.translation();
|
|
s = scaling.diagonal().mean();
|
|
} // DecomposeSimilarityTransform
|
|
/*----------------------------------------------------------------*/
|