Demo entry 6626754

cpp

   

Submitted by asd on Jun 26, 2017 at 14:23
Language: C++. Code size: 3.7 kB.

#include "stdafx.h"
#include "RigidTransform.h"

void GetRigidTrans2D(cv::Point2f* srcPoints, cv::Point2f* dstPoints, int pointsNum, TRigidTrans2D& transform)
{
	double srcSumX = 0.0f;
	double srcSumY = 0.0f;

	double dstSumX = 0.0f;
	double dstSumY = 0.0f;

	for (int i = 0; i < pointsNum; ++ i)
	{
		srcSumX += srcPoints[i].x;
		srcSumY += srcPoints[i].y;

		dstSumX += dstPoints[i].x;
		dstSumY += dstPoints[i].y;
	}

	cv::Point2f centerSrc, centerDst;

	centerSrc.x = float(srcSumX / pointsNum);
	centerSrc.y = float(srcSumY / pointsNum);

	centerDst.x = float(dstSumX / pointsNum);
	centerDst.y = float(dstSumY / pointsNum);

	cv::Mat srcMat(2, pointsNum, CV_64FC1);
	cv::Mat dstMat(2, pointsNum, CV_64FC1);

	double* srcDat = (double*)(srcMat.data);
	double* dstDat = (double*)(dstMat.data);
	for (int i = 0; i < pointsNum; ++ i)
	{
		srcDat[i] = srcPoints[i].x - centerSrc.x;
		srcDat[pointsNum + i] = srcPoints[i].y - centerSrc.y;

		dstDat[i] = dstPoints[i].x - centerDst.x;
		dstDat[pointsNum + i] = dstPoints[i].y - centerDst.y;
	}

	cv::Mat matS = srcMat * dstMat.t();

	cv::Mat matU, matW, matV;
	cv::SVDecomp(matS, matW, matU, matV);

	cv::Mat matTemp = matU * matV;
	double det = cv::determinant(matTemp);

	double datM[] = {1, 0, 0, det};
	cv::Mat matM(2, 2, CV_64FC1, datM);

	cv::Mat matR = matV.t() * matM * matU.t();

	memcpy(transform.matR, matR.data, sizeof(double) * 4);

	double* datR = (double*)(matR.data);
	transform.X = centerDst.x - (centerSrc.x * datR[0] + centerSrc.y * datR[1]);
	transform.Y = centerDst.y - (centerSrc.x * datR[2] + centerSrc.y * datR[3]);
}

void GetRigidTrans3D(cv::Point3f* srcPoints, cv::Point3f* dstPoints, int pointsNum, TRigidTrans3D& transform)
{
	double srcSumX = 0.0f;
	double srcSumY = 0.0f;
	double srcSumZ = 0.0f;

	double dstSumX = 0.0f;
	double dstSumY = 0.0f;
	double dstSumZ = 0.0f;

	for (int i = 0; i < pointsNum; ++ i)
	{
		srcSumX += srcPoints[i].x;
		srcSumY += srcPoints[i].y;
		srcSumZ += srcPoints[i].z;

		dstSumX += dstPoints[i].x;
		dstSumY += dstPoints[i].y;
		dstSumZ += dstPoints[i].z;
	}

	cv::Point3f centerSrc, centerDst;

	centerSrc.x = float(srcSumX / pointsNum);
	centerSrc.y = float(srcSumY / pointsNum);
	centerSrc.z = float(srcSumZ / pointsNum);

	centerDst.x = float(dstSumX / pointsNum);
	centerDst.y = float(dstSumY / pointsNum);
	centerDst.z = float(dstSumZ / pointsNum);

	cv::Mat srcMat(3, pointsNum, CV_32FC1);
	cv::Mat dstMat(3, pointsNum, CV_32FC1);
	
	float* srcDat = (float*)(srcMat.data);
	float* dstDat = (float*)(dstMat.data);
	for (int i = 0; i < pointsNum; ++ i)
	{
		srcDat[i] = srcPoints[i].x - centerSrc.x;
		srcDat[pointsNum + i] = srcPoints[i].y - centerSrc.y;
		srcDat[pointsNum * 2 + i] = srcPoints[i].z - centerSrc.z;

		dstDat[i] = dstPoints[i].x - centerDst.x;
		dstDat[pointsNum + i] = dstPoints[i].y - centerDst.y;
		dstDat[pointsNum * 2 + i] = dstPoints[i].z - centerDst.z;
	}

	cv::Mat matS = srcMat * dstMat.t();

	cv::Mat matU, matW, matV;
	cv::SVDecomp(matS, matW, matU, matV);

	cv::Mat matTemp = matU * matV;
	double det = cv::determinant(matTemp);

	double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};
	cv::Mat matM(3, 3, CV_64FC1, datM);

	cv::Mat matR = matV.t() * matM * matU.t();

	memcpy(transform.matR, matR.data, sizeof(double) * 9);

	double* datR = (double*)(matR.data);
	transform.X = centerDst.x - (centerSrc.x * datR[0] + centerSrc.y * datR[1] + centerSrc.z * datR[2]);
	transform.Y = centerDst.y - (centerSrc.x * datR[3] + centerSrc.y * datR[4] + centerSrc.z * datR[5]);
	transform.Z = centerDst.z - (centerSrc.x * datR[6] + centerSrc.y * datR[7] + centerSrc.z * datR[8]);

This snippet took 0.02 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).