/*
* Camera.cpp
*
* Copyright (c) 2014-2015 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 "Camera.h"

using namespace MVS;


// D E F I N E S ///////////////////////////////////////////////////


// S T R U C T S ///////////////////////////////////////////////////

const Camera Camera::IDENTITY(Matrix3x3::eye(), Point3(REAL(0), REAL(0), REAL(0)));

Camera::Camera(const Matrix3x4& _P, bool bUpdate/*=true*/)
	:
	P(_P)
{
	if (bUpdate)
		DecomposeP();
} // Camera
Camera::Camera(const Matrix3x3& _R, const Point3& _C, bool bUpdate/*=true*/)
	:
	CameraIntern(_R, _C)
{
	if (bUpdate)
		ComposeP_RC();
} // Camera
Camera::Camera(const Matrix3x3& _K, const Matrix3x3& _R, const Point3& _C, bool bUpdate/*=true*/)
	:
	CameraIntern(_K, _R, _C)
{
	if (bUpdate)
		ComposeP();
} // Camera
/*----------------------------------------------------------------*/


Camera& Camera::operator= (const CameraIntern& camera)
{
	K = camera.K;
	R = camera.R;
	C = camera.C;
	return *this;
}
/*----------------------------------------------------------------*/


void Camera::ComposeP_RC()
{
	AssembleProjectionMatrix(R, C, P);
} // ComposeP_RC
/*----------------------------------------------------------------*/

void Camera::ComposeP()
{
	AssembleProjectionMatrix(K, R, C, P);
} // ComposeP
/*----------------------------------------------------------------*/

void Camera::DecomposeP_RC()
{
	DecomposeProjectionMatrix(P, R, C);
} // DecomposeP_RC
/*----------------------------------------------------------------*/

void Camera::DecomposeP()
{
	DecomposeProjectionMatrix(P, K, R, C);
} // DecomposeP
/*----------------------------------------------------------------*/

void Camera::Transform(const Matrix3x3& _R, const Point3& _t, const REAL& _s)
{
	R = R * _R.t();
	C = _R*C*_s+_t;
	ComposeP();
} // Transform
/*----------------------------------------------------------------*/


REAL Camera::PointDepth(const Point3& X) const
{
	return P(2,0)*X.x + P(2,1)*X.y + P(2,2)*X.z + P(2,3);
} // PointDepth
/*----------------------------------------------------------------*/

bool Camera::IsInFront(const Point3& X) const
{
	return (PointDepth(X)>0);
} // IsInFront
/*----------------------------------------------------------------*/

REAL Camera::DistanceSq(const Point3& X) const
{
	const Point3 ray = X-C;
	return (SQUARE(ray.x)+SQUARE(ray.y)+SQUARE(ray.z));
} // Distance
/*----------------------------------------------------------------*/


// decomposition of projection matrix into KR[I|-C]: internal calibration ([3,3]), rotation ([3,3]) and translation ([3,1])
// (comparable with OpenCV: normalized cv::decomposeProjectionMatrix)
void MVS::DecomposeProjectionMatrix(const PMatrix& P, KMatrix& K, RMatrix& R, CMatrix& C)
{
	// extract camera center as the right null vector of P
	const Vec4 hC(P.RightNullVector());
	C = CMatrix(hC[0],hC[1],hC[2]) * INVERT(hC[3]);
	// perform RQ decomposition
	RQDecomp3x3<REAL>(cv::Mat(3,4,cv::DataType<REAL>::type,const_cast<REAL*>(P.val))(cv::Rect(0,0, 3,3)), K, R);
	// normalize calibration matrix
	K *= INVERT(K(2,2));
	// ensure positive focal length
	if (K(0,0) < 0) {
		ASSERT(K(1,1) < 0);
		NEGATE(K(0,0));
		NEGATE(K(1,1));
		NEGATE(K(0,1));
		NEGATE(K(0,2));
		NEGATE(K(1,2));
		(TMatrix<REAL,2,3>&)R *= REAL(-1);
	}
	ASSERT(R.IsValid());
} // DecomposeProjectionMatrix
void MVS::DecomposeProjectionMatrix(const PMatrix& P, RMatrix& R, CMatrix& C)
{
	#ifndef _RELEASE
	KMatrix K;
	DecomposeProjectionMatrix(P, K, R, C);
	ASSERT(K.IsEqual(Matrix3x3::IDENTITY, 1e-5));
	#endif
	// extract camera center as the right null vector of P
	const Vec4 hC(P.RightNullVector());
	C = CMatrix(hC[0],hC[1],hC[2]) * INVERT(hC[3]);
	// get rotation
	const cv::Mat mP(3,4,cv::DataType<REAL>::type,const_cast<REAL*>(P.val));
	mP(cv::Rect(0,0, 3,3)).copyTo(R);
	ASSERT(R.IsValid());
} // DecomposeProjectionMatrix
/*----------------------------------------------------------------*/

// assemble projection matrix: P=KR[I|-C]
void MVS::AssembleProjectionMatrix(const KMatrix& K, const RMatrix& R, const CMatrix& C, PMatrix& P)
{
	// compute temporary matrices
	cv::Mat mP(3,4,cv::DataType<REAL>::type,const_cast<REAL*>(P.val));
	cv::Mat M(mP, cv::Rect(0,0, 3,3));
	cv::Mat(K * R).copyTo(M); //3x3
	mP.col(3) = M * cv::Mat(-C); //3x1
} // AssembleProjectionMatrix
void MVS::AssembleProjectionMatrix(const RMatrix& R, const CMatrix& C, PMatrix& P)
{
	Eigen::Map<Matrix3x3::EMat,0,Eigen::Stride<4,0> >(P.val) = (const Matrix3x3::EMat)R;
	Eigen::Map<Point3::EVec,0,Eigen::Stride<0,4> >(P.val+3) = ((const Matrix3x3::EMat)R) * (-((const Point3::EVec)C));
} // AssembleProjectionMatrix
/*----------------------------------------------------------------*/

// compute the focus of attention of a set of cameras; only cameras
// that have the focus of attention in front of them are considered
Point3 MVS::ComputeCamerasFocusPoint(const CameraArr& cameras, const Point3* pInitialFocus)
{
	// set initial focus point
	Point3 focus;
	if (pInitialFocus == NULL) {
		// initialize focus point to the average camera center
		focus = Point3::ZERO;
		for (const Camera& camera: cameras)
			focus += camera.C;
		focus /= cameras.size();
		// compute average distance to the cameras
		REAL avgDist = 0;
		for (const Camera& camera: cameras)
			avgDist += SQRT(camera.DistanceSq(focus));
		avgDist /= cameras.size();
		// move focus point to the average distance in front of first camera
		focus = cameras.front().C + cameras.front().Direction()*avgDist;
	} else
		focus = *pInitialFocus;
	// compute focus point as the point in front of the active cameras,
	// closest to the view direction of each camera
	uint32_t numActive = 0;
	Point3::EVec smp(Point3::EVec::Zero());
	Matrix3x3::EMat smTm(Matrix3x3::EMat::Zero());
	for (const Camera& camera: cameras) {
		if (!camera.IsInFront(focus))
			continue;
		// https://en.wikipedia.org/wiki/Line–line_intersection#In_more_than_two_dimensions
		const Point3::EVec dir = camera.Direction();
		const Matrix3x3::EMat m(Matrix3x3::EMat::Identity() - dir * dir.transpose());
		const Matrix3x3::EMat mTm(m.transpose() * m);
		const Point3::EVec s = mTm * Point3::EVec(camera.C);
		smp += s;
		smTm += mTm;
		++numActive;
	}
	if (numActive >= 2)
		focus = smTm.inverse() * smp;
	return focus;
} // ComputeCamerasFocusPoint
/*----------------------------------------------------------------*/



namespace MVS {

namespace RECTIFY {
	
// compute the ROIs for the two images based on the corresponding points
void GetImagePairROI(const Point3fArr& points1, const Point3fArr& points2, const Matrix3x3& K1, const Matrix3x3& K2, const Matrix3x3& R1, const Matrix3x3& R2, const Matrix3x3& invK1, const Matrix3x3& invK2, AABB2f& roi1h, AABB2f& roi2h)
{
	ASSERT(!points1.empty() && points1.size() && points2.size());

	// compute rectification homography (from original to rectified image)
	const Matrix3x3 H1(K1 * R1 * invK1);
	const Matrix3x3 H2(K2 * R2 * invK2);

	// determine the ROIs in rectified images
	roi1h.Reset(); roi2h.Reset();
	FOREACH(i, points1) {
		Point2f xh;
		const Point3f& x1 = points1[i];
		ProjectVertex_3x3_2_2(H1.val, x1.ptr(), xh.ptr());
		roi1h.InsertFull(xh);
		const Point3f& x2 = points2[i];
		ProjectVertex_3x3_2_2(H2.val, x2.ptr(), xh.ptr());
		roi2h.InsertFull(xh);
	}
}

void SetCameraMatricesROI(const AABB2f& roi1h, const AABB2f& roi2h, cv::Size& size1, cv::Size& size2, Matrix3x3& K1, Matrix3x3& K2)
{
	// set the new image sizes such that they are equal and contain the entire ROI
	const Point2f size1h(roi1h.GetSize());
	const Point2f size2h(roi2h.GetSize());
	const int maxSize(MAXF(size1.width+size2.width, size1.height+size2.height)/2);
	size1.width = size2.width = MINF(ROUND2INT(MAXF(size1h.x,size2h.x)), maxSize);
	size1.height = size2.height = MINF(ROUND2INT(MAXF(size1h.y,size2h.y)), maxSize);

	// set the new camera matrices such that the ROI is centered
	const Point2f center1h(roi1h.GetCenter());
	const Point2f center2h(roi2h.GetCenter());
	K1(0,2) += size1.width /2-center1h[0];
	K1(1,2) += size1.height/2-center1h[1];
	K2(0,2) += size2.width /2-center2h[0];
	K2(1,2) += size2.height/2-center2h[1];
}

} // namespace RECTIFY

} // namespace MVS

// Compute stereo rectification homographies that transform two images,
// such that corresponding pixels in one image lie on the same scan-line in the other image;
// note: this function assumes that the two cameras are already undistorted
REAL Camera::StereoRectify(const cv::Size& size1, const Camera& camera1, const cv::Size& size2, const Camera& camera2, Matrix3x3& R1, Matrix3x3& R2, Matrix3x3& K1, Matrix3x3& K2)
{
	// compute relative pose
	RMatrix poseR;
	CMatrix poseC;
	ComputeRelativePose(camera1.R, camera1.C, camera2.R, camera2.C, poseR, poseC);

	// compute the average rotation between the first and the second camera
	const Point3 r(poseR.GetRotationAxisAngle());
	reinterpret_cast<RMatrix&>(R2).SetFromAxisAngle(r*REAL(-0.5));
	R1 = R2.t();

	// compute the translation, such that it coincides with the X-axis
	int idx(0);
	Point3 t(R2 * (poseR*(-poseC)));
	#if 0
	const Point3 unitX(t.x<0?-1:1, 0, 0);
	const Point3 axis(t.cross(unitX));
	const REAL normAxis(norm(axis));
	if (normAxis > EPSILONTOLERANCE<REAL>()) {
		ASSERT(t.dot(unitX) >= 0);
		const REAL angle(ACOS(ComputeAngle(t.ptr(), unitX.ptr())));
		const RMatrix Rx(axis*angle/normAxis);
	#else
	const REAL nt(norm(t));
	//idx = (ABS(t.x) > ABS(t.y) ? 0 : 1);
	Point3 axis(Point3(0,0,1).cross(t));
	const REAL na(norm(axis));
	if (na < EPSILONTOLERANCE<REAL>())
		return 0;
	{
		const Point3 w(normalized(t.cross(axis)));
		RMatrix Rx;
		for (int i = 0; i < 3; ++i) {
			Rx(idx,i) = -t[i]/nt;
			Rx(idx^1,i) = -axis[i]/na;
			Rx(2,i) = w[i]*(1-2*idx); // if idx == 1 -> opposite direction
		}
	#endif
		R1 = Rx * R1;
		R2 = Rx * R2;
		t = R2 * (poseR*(-poseC));
	}
	ASSERT(ISEQUAL(-t.x, norm(camera2.C-camera1.C)) && ISZERO(t.y) && ISZERO(t.z));

	// determine the intrinsic calibration matrix;
	// vertical focal length must be the same for both images to keep the epipolar constraint
	K1 = Matrix3x3::IDENTITY;
	K1(0,0) = (camera1.K(0,0)+camera2.K(0,0))/2;
	K1(1,1) = (camera1.K(1,1)+camera2.K(1,1))/2;
	K2 = K1;
	// set ROI to contain the whole transformed image
	for (int k = 0; k < 2; k++) {
		const Matrix3x3 H(k == 0 ? K1*R1*camera1.GetInvK() : K2*R2*camera2.GetInvK());
		Point2f ptMin(std::numeric_limits<float>::max());
		for (int i = 0; i < 4; i++) {
			const Point2f pt(
				(float)((i%2)*size1.width),
				(float)((i<2?0:1)*size1.height)
			);
			Point2f ptH;
			ProjectVertex_3x3_2_2(H.val, pt.ptr(), ptH.ptr());
			ptMin.x = MINF(ptMin.x,ptH.x);
			ptMin.y = MINF(ptMin.y,ptH.y);
		}
		Matrix3x3& KNew(k == 0 ? K1 : K2);
		KNew(0,2) = -ptMin.x;
		KNew(1,2) = -ptMin.y;
	}
	K1(idx^1,2) = K2(idx^1,2) = MAXF(K1(idx^1,2), K2(idx^1,2));
	return t.x;
} // StereoRectify

// see: "A compact algorithm for rectification of stereo pairs", A. Fusiello, E. Trucco, and A. Verri, 2000
REAL Camera::StereoRectifyFusiello(const cv::Size& size1, const Camera& camera1, const cv::Size& size2, const Camera& camera2, Matrix3x3& R1, Matrix3x3& R2, Matrix3x3& K1, Matrix3x3& K2)
{
	// compute relative pose
	RMatrix poseR;
	CMatrix poseC;
	ComputeRelativePose(camera1.R, camera1.C, camera2.R, camera2.C, poseR, poseC);

	// new x axis (baseline, from C1 to C2)
	const Point3 v1(camera2.C-camera1.C);
	// new y axes (orthogonal to old z and new x)
	const Point3 v2(camera1.Direction().cross(v1));
	// new z axes (no choice, orthogonal to baseline and y)
	const Point3 v3(v1.cross(v2));

	// new extrinsic (translation unchanged)
	RMatrix R;
	R.SetFromRowVectors(normalized(v1), normalized(v2), normalized(v3));

	// new intrinsic (arbitrary)
	K1 = camera1.K; K1(0,1) = 0;
	K2 = camera2.K; K2(0,1) = 0;
	K1(1,1) = K2(1,1) = (camera1.K(1,1)+camera2.K(1,1))/2;

	// new rotations
	R1 = R*camera1.R.t();
	R2 = R*camera2.R.t();

	#if 0
	// new projection matrices
	PMatrix P1, P2;
	AssembleProjectionMatrix(K1, R, camera1.C, P1);
	AssembleProjectionMatrix(K2, R, camera2.C, P2);

	// rectifying image transformation
	#if 0
	const Matrix3x3 H1((PMatrix::EMat(P1).leftCols<3>()*(PMatrix::EMat(camera1.P).leftCols<3>().inverse())).eval());
	const Matrix3x3 H2((PMatrix::EMat(P2).leftCols<3>()*(PMatrix::EMat(camera2.P).leftCols<3>().inverse())).eval());
	#else
	const Matrix3x3 H1(K1*R*camera1.R.t()*camera1.GetInvK());
	const Matrix3x3 H2(K2*R*camera2.R.t()*camera2.GetInvK());
	#endif
	#endif

	const Point3 t(R2 * (poseR*(-poseC)));
	ASSERT(ISEQUAL(-t.x, norm(v1)) && ISZERO(t.y) && ISZERO(t.z));
	return t.x;
} // StereoRectifyFusiello

// adjust rectified camera matrices such that the entire area common to both source images is contained in the rectified images;
// as long as the focal-length on x and the skewness are equal in both cameras
// the point-scale is also equal in both images;
// note however that the surface-scale can still be different (most likely is)
// as it depends on its (unknown) normal too;
//  - points1 and points2: contain the pairs of corresponding pairs of image projections and their depth
//  - size1 and size2: input the size of the source images, output the size of the rectified images (rectified image sizes are equal)
void Camera::SetStereoRectificationROI(const Point3fArr& points1, cv::Size& size1, const Camera& camera1, const Point3fArr& points2, cv::Size& size2, const Camera& camera2, const Matrix3x3& R1, const Matrix3x3& R2, Matrix3x3& K1, Matrix3x3& K2)
{
	ASSERT(!points1.empty() && points1.size() && points2.size());

	#if 1
	// ignore skewness
	K1(0,1) = K2(0,1) = 0;
	#else
	// set same skewness
	K1(0,1) = K2(0,1) = (K1(0,1)+K2(0,1))/2;
	#endif

	// set same focal-length on x too
	K1(0,0) = K2(0,0) = (K1(0,0)+K2(0,0))/2;
	ASSERT(ISEQUAL(K1(1,1), K2(1,1)));

	// determine the ROIs in rectified images
	AABB2f roi1h, roi2h;
	RECTIFY::GetImagePairROI(points1, points2, K1, K2, R1, R2, camera1.GetInvK(), camera2.GetInvK(), roi1h, roi2h);

	// set the new camera matrices such that the ROI is centered
	RECTIFY::SetCameraMatricesROI(roi1h, roi2h, size1, size2, K1, K2);
} // SetStereoRectificationROI
/*----------------------------------------------------------------*/