mirror of
https://github.com/lucaspalomodevelop/meshlab.git
synced 2026-03-13 08:09:39 +00:00
137 lines
4.3 KiB
C++
137 lines
4.3 KiB
C++
/****************************************************************************
|
|
* MeshLab o o *
|
|
* An extendible mesh processor o o *
|
|
* _ O _ *
|
|
* Copyright(C) 2005, 2009 \/)\/ *
|
|
* Visual Computing Lab /\/| *
|
|
* ISTI - Italian National Research Council | *
|
|
* \ *
|
|
* All rights reserved. *
|
|
* *
|
|
* This program is free software; you can redistribute it and/or modify *
|
|
* it under the terms of the GNU General Public License as published by *
|
|
* the Free Software Foundation; either version 2 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 General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
|
* for more details. *
|
|
* *
|
|
****************************************************************************/
|
|
|
|
#include <vector>
|
|
#include <vcg/math/matrix33.h>
|
|
#include <vcg/math/matrix44.h>
|
|
#include <vcg/space/point3.h>
|
|
|
|
using namespace vcg;
|
|
using namespace std;
|
|
|
|
#include "radial_distortion.h"
|
|
#include "arc3D_camera.h"
|
|
|
|
|
|
/*
|
|
Arc3D Camera
|
|
|
|
|
|
*/
|
|
// This function take in input a point in image space (e.g. with coords in range [0..1024]x[0..768]
|
|
// a depth value and it returns the point in absolute 3D coords
|
|
//
|
|
Point3m Arc3DCamera::DepthTo3DPoint(double x, double y, double depth, Point3d &M) const
|
|
{
|
|
Point3d m_temp = Kinv * Point3d(x,y,1);
|
|
|
|
double oldx, oldy;
|
|
rd.ComputeOldXY(m_temp[0] / m_temp[2], m_temp[1] / m_temp[2], oldx, oldy);
|
|
|
|
m_temp=Point3d(oldx,oldy,1);
|
|
Point3d fp=t;
|
|
Point3d end = TRinv*m_temp;
|
|
Point3d dir =fp-end;
|
|
dir.Normalize();
|
|
M = fp-dir*depth;
|
|
|
|
Point3d Mundist;
|
|
|
|
m_temp = Kinvundist * Point3d(x,y,1);
|
|
|
|
rd.ComputeOldXY(m_temp[0] / m_temp[2], m_temp[1] / m_temp[2], oldx, oldy);
|
|
|
|
m_temp=Point3d(oldx,oldy,1);
|
|
fp=t;
|
|
end = TRinv*m_temp;
|
|
dir =fp-end;
|
|
dir.Normalize();
|
|
Mundist = fp-dir*depth;
|
|
|
|
Point3m correct(Point3m::ScalarType(Mundist[0]-M[0]),Point3m::ScalarType(Mundist[1]-M[1]),Point3m::ScalarType(Mundist[2]-M[2]));
|
|
return correct;
|
|
}
|
|
|
|
|
|
|
|
|
|
bool Arc3DCamera::Open(const char *filename)
|
|
{
|
|
FILE *fp=fopen(filename,"rb");
|
|
if(!fp) return false;
|
|
|
|
fscanf(fp,"%lf %lf %lf",&(K[0][0]),&(K[0][1]),&(K[0][2]));
|
|
fscanf(fp,"%lf %lf %lf",&(K[1][0]),&(K[1][1]),&(K[1][2]));
|
|
fscanf(fp,"%lf %lf %lf",&(K[2][0]),&(K[2][1]),&(K[2][2]));
|
|
|
|
k.resize(3);
|
|
fscanf(fp,"%lf %lf %lf",&(k[0]),&(k[1]),&(k[2]));
|
|
|
|
fscanf(fp,"%lf %lf %lf",&(R[0][0]),&(R[0][1]),&(R[0][2]));
|
|
fscanf(fp,"%lf %lf %lf",&(R[1][0]),&(R[1][1]),&(R[1][2]));
|
|
fscanf(fp,"%lf %lf %lf",&(R[2][0]),&(R[2][1]),&(R[2][2]));
|
|
|
|
fscanf(fp,"%lf %lf %lf",&(t[0]),&(t[1]),&(t[2]));
|
|
|
|
fscanf(fp,"%i %i",&width,&height);
|
|
|
|
fclose(fp);
|
|
|
|
Kundist=K;
|
|
|
|
///////
|
|
Kundist[0][2]=(double)width/2.0;
|
|
Kundist[1][2]=(double)height/2.0;
|
|
|
|
///////
|
|
Kinv=Inverse(K);
|
|
Kinvundist=Inverse(Kundist);
|
|
|
|
rd.SetParameters(k);
|
|
|
|
// TR = [R | -Rt] 4x4 matrix with upperleft a 3x3 rotation
|
|
// on the right the rotated translation -Rt and 0001 in the
|
|
// lower line.
|
|
|
|
R=Inverse(R);
|
|
#ifndef VCG_USE_EIGEN
|
|
for(int i=0;i<3;++i)
|
|
for(int j=0;j<3;++j)
|
|
TR[i][j]= R[i][j];
|
|
#else
|
|
TR.corner<3,3>(Eigen::TopLeft) = R.transpose();
|
|
#endif
|
|
|
|
Point3d rt= R*(-t);
|
|
|
|
for(int i=0;i<3;++i)
|
|
TR[i][3]=rt[i];
|
|
|
|
for(int j=0;j<3;++j)
|
|
TR[3][j]=0;
|
|
|
|
TR[3][3]=1;
|
|
TRinv=Inverse(TR);
|
|
return true;
|
|
}
|