public void IndexAssignedTo()
{
Matrix3x3f m = new Matrix3x3f();
for (int i = 0; i < SIZE; i++)
{
m[i] = i;
}
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(i, m[i]);
}
}
/// <summary>
/// Create the corresponding <see cref="LightsState.Light"/> for this object.
/// </summary>
/// <returns>
/// It returns the <see cref="LightsState.Light"/> equivalent to this SceneObjectLight.
/// </returns>
public override LightsState.Light ToLight(GraphicsContext ctx, SceneGraphContext sceneCtx)
{
LightsState.LightDirectional light = new LightsState.LightDirectional();
SetLightParameters(sceneCtx, light);
TransformState transformState = (TransformState)sceneCtx.GraphicsStateStack.Current[TransformState.StateSetIndex];
Matrix3x3f normalMatrix = transformState.NormalMatrix;
light.Direction = (normalMatrix * (Vertex3f)Direction).Normalized;
light.HalfVector = (Vertex3f.UnitZ + light.Direction).Normalized;
return(light);
}
public void Transpose()
{
Matrix3x3f m = new Matrix3x3f();
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
m[j, i] = i + j * HALF_SIZE;
}
}
Assert.AreEqual(Indexed3x3().Transpose, m);
}
Matrix3x3f Indexed3x3()
{
Matrix3x3f m = new Matrix3x3f();
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
m[i, j] = i + j * HALF_SIZE;
}
}
return(m);
}
public void CreatedFromArray2()
{
float[,] d = new float[, ] {
{ 0, 3, 6 },
{ 1, 4, 7 },
{ 2, 5, 8 }
};
Matrix3x3f m = new Matrix3x3f(d);
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(i, m[i]);
}
}
Matrix3x3f Random3x3(int seed)
{
Random rnd = new Random(seed);
Matrix3x3f m = new Matrix3x3f();
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
m[i, j] = (float)rnd.NextDouble();
}
}
return(m);
}
public void UpdateMoI()
{
if (vessel == null || vessel.rootPart.Rigidbody == null)
{
return;
}
InertiaTensor = new Matrix3x3f();
var vesselTransform = vessel.GetTransform();
var inverseVesselRotation = Quaternion.Inverse(vesselTransform.rotation);
for (int pi = 0, vesselpartsCount = vessel.parts.Count; pi < vesselpartsCount; pi++)
{
Part p = vessel.parts[pi];
var rb = p.Rigidbody;
if (rb == null)
{
continue;
}
//Compute the contributions to the vessel inertia tensor due to the part inertia tensor
Vector3 principalMoments = rb.inertiaTensor;
Quaternion principalAxesRot = inverseVesselRotation * p.transform.rotation * rb.inertiaTensorRotation;
Quaternion invPrincipalAxesRot = Quaternion.Inverse(principalAxesRot);
for (int j = 0; j < 3; j++)
{
Vector3 partInertiaTensorTimesjHat = principalAxesRot * Vector3.Scale(principalMoments, invPrincipalAxesRot * unitVectors[j]);
for (int i = 0; i < 3; i++)
{
InertiaTensor.Add(i, j, Vector3.Dot(unitVectors[i], partInertiaTensorTimesjHat));
}
}
//Compute the contributions to the vessel inertia tensor due to the part mass and position
float partMass = rb.mass;
Vector3 partPosition = vesselTransform.InverseTransformDirection(rb.worldCenterOfMass - wCoM);
for (int i = 0; i < 3; i++)
{
InertiaTensor.Add(i, i, partMass * partPosition.sqrMagnitude);
for (int j = 0; j < 3; j++)
{
InertiaTensor.Add(i, j, -partMass * partPosition[i] * partPosition[j]);
}
}
}
MoI = new Vector3(InertiaTensor[0, 0], InertiaTensor[1, 1], InertiaTensor[2, 2]);
MoI = refT.InverseTransformDirection(vessel.transform.TransformDirection(MoI)).AbsComponents();
}
/// <summary>
/// Rotates A through phi in pq-plane to set A(p,q) = 0
/// Rotation stored in R whose columns are eigenvectors of A
/// </summary>
public static void JacobiRotate(ref Matrix3x3f A, ref Matrix3x3f R, int p, int q)
{
if (A[p, q] == 0.0f)
{
return;
}
float d = (A[p, p] - A[q, q]) / (2.0f * A[p, q]);
float t = 1.0f / (Math.Abs(d) + (float)Math.Sqrt(d * d + 1.0f));
if (d < 0.0f)
{
t = -t;
}
float c = 1.0f / (float)Math.Sqrt(t * t + 1);
float s = t * c;
A[p, p] += t * A[p, q];
A[q, q] -= t * A[p, q];
A[p, q] = A[q, p] = 0.0f;
// transform A
int k;
for (k = 0; k < 3; k++)
{
if (k != p && k != q)
{
float Akp = c * A[k, p] + s * A[k, q];
float Akq = -s * A[k, p] + c * A[k, q];
A[k, p] = A[p, k] = Akp;
A[k, q] = A[q, k] = Akq;
}
}
// store rotation in R
for (k = 0; k < 3; k++)
{
float Rkp = c * R[k, p] + s * R[k, q];
float Rkq = -s * R[k, p] + c * R[k, q];
R[k, p] = Rkp;
R[k, q] = Rkq;
}
}
/// <summary>
/// Return the inf norm of the matrix.
/// </summary>
private static float InfNorm(Matrix3x3f A)
{
float sum1 = Math.Abs(A.m00) + Math.Abs(A.m01) + Math.Abs(A.m02);
float sum2 = Math.Abs(A.m10) + Math.Abs(A.m11) + Math.Abs(A.m12);
float sum3 = Math.Abs(A.m20) + Math.Abs(A.m21) + Math.Abs(A.m22);
float maxSum = sum1;
if (sum2 > maxSum)
{
maxSum = sum2;
}
if (sum3 > maxSum)
{
maxSum = sum3;
}
return(maxSum);
}
public void Index2AssignedTo()
{
Matrix3x3f m = new Matrix3x3f();
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
m[i, j] = i + j * HALF_SIZE;
}
}
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
Assert.AreEqual(i + j * HALF_SIZE, m[i, j]);
}
}
}
public static void EigenDecomposition(Matrix3x3f A, out Matrix3x3f eigenVecs, out Vector3f eigenVals)
{
const int numJacobiIterations = 10;
const float epsilon = 1e-15f;
Matrix3x3f D = A;
// only for symmetric matrices!
eigenVecs = Matrix3x3f.Identity; // unit matrix
int iter = 0;
while (iter < numJacobiIterations)
{
// 3 off diagonal elements
// find off diagonal element with maximum modulus
int p, q;
float a, max;
max = Math.Abs(D.m01);
p = 0; q = 1;
a = Math.Abs(D.m02);
if (a > max)
{
p = 0; q = 2; max = a;
}
a = Math.Abs(D.m12);
if (a > max)
{
p = 1; q = 2; max = a;
}
// all small enough -> done
if (max < epsilon)
{
break;
}
// rotate matrix with respect to that element
JacobiRotate(ref D, ref eigenVecs, p, q);
iter++;
}
eigenVals = new Vector3f(D.m00, D.m11, D.m22);
}
/// <summary>
/// Set uniform state variable (variant type).
/// </summary>
/// <param name="ctx">
/// A <see cref="GraphicsContext"/> used for operations.
/// </param>
/// <param name="uniformName">
/// A <see cref="String"/> that specify the variable name in the shader source.
/// </param>
/// <param name="m">
/// A <see cref="Matrix3x3f"/> holding the uniform variabile data.
/// </param>
public void SetVariantUniform(GraphicsContext ctx, string uniformName, Matrix3x3f m)
{
if (ctx == null)
{
throw new ArgumentNullException("ctx");
}
UniformBinding uniform = GetUniform(ctx, uniformName);
switch (uniform.UniformType)
{
case ShaderUniformType.Mat3x3:
SetUniform(ctx, uniformName, m);
break;
#if !MONODROID
case ShaderUniformType.DoubleMat3x3:
SetUniform(ctx, uniformName, (Matrix3x3d)m);
break;
#endif
default:
throw new ShaderException("unable to set single-precision floating-point matrix 3x3 data to uniform of type {0}", uniform.UniformType);
}
}
public SVGFontProvider ImportSVG(XmlDocument document)
{
XmlNode font = document["svg"]["defs"]["font"];
if (font == null)
{
throw new ArgumentException("The document does not include any fonts.");
}
// We write basic properties (for normalization).
string name = font["font-face"]["font-family"] != null ? font["font-face"]["font-family"].InnerText : "unknown";
// We extract data.
if (font.Attributes["horiz-origin-x"] != null)
{
horizOriginX = int.Parse(font.Attributes["horiz-origin-x"].InnerText);
}
if (font.Attributes["horiz-origin-y"] != null)
{
horizOriginY = int.Parse(font.Attributes["horiz-origin-y"].InnerText);
}
if (horizOriginX != 0 || horizOriginY != 0)
{
throw new NotSupportedException();
}
if (font.Attributes["horiz-adv-x"] != null)
{
horizAdvX = int.Parse(font.Attributes["horiz-adv-x"].InnerText);
}
// We now extract glyphs.
// TODO: extract missing gylph.
SVGGlyph def = ProcessDefGlyph(font["missing-glyph"]);
// Extract each glyph.
foreach (XmlNode node in font)
{
if (node.Name == "glyph")
{
ProcessGlyph(node);
}
}
// We now rescale each glyph to range in y to [0,1].
float scale = 1.0f / maxDimensions.Y;
Matrix3x3f matrix = Matrix3x3f.CreateScale(new Vector3f(scale, scale, scale));
foreach (KeyValuePair <string, SVGGlyph> pair in glyphs)
{
// We scale outline.
pair.Value.Outline.Transform(matrix);
pair.Value.Advance *= scale;
}
// Transform default.
def.Advance *= scale;
def.Outline.Transform(matrix);
ImportKernellings(font);
SortedList <ComparablePair <char, char>, float> r = new SortedList <ComparablePair <char, char>, float>();
// We normalize kernellig (also negatie, since the + meaning is shrink in SVG, in our format,
// it is enlarge).
for (int z = 0; z < kernelling.Count; z++)
{
r.Add(kernelling.Keys[z], kernelling.Values[z] * -scale);
}
return(new SVGFontProvider(name, glyphs, r, def));
}
public void Init()
{
m_pipelineManager = new SolARPluginPipelineManager();
#if UNITY_EDITOR
// If in editor mode, the pipeline configuration file are stored in the unity assets folder but not in the streaminAssets folder
if (!m_pipelineManager.init(Application.dataPath + m_configurationPath, m_uuid))
{
Debug.Log("Cannot init pipeline manager " + Application.dataPath + m_configurationPath + " with uuid " + m_uuid);
return;
}
#elif UNITY_ANDROID
Screen.sleepTimeout = SleepTimeout.NeverSleep;
Android.ReplacePathToApp(Application.persistentDataPath + "/StreamingAssets" + m_configurationPath);
// When the application is built, only the pipeline configuration files used by the application are moved to the an external folder on terminal
Debug.Log("[ANDROID] Load pipeline : " + Application.persistentDataPath + "/StreamingAssets" + m_configurationPath);
if (!m_pipelineManager.init(Application.persistentDataPath + "/StreamingAssets" + m_configurationPath, m_uuid))
{
Debug.Log("Cannot init pipeline manager " + Application.persistentDataPath + "/StreamingAssets" + m_configurationPath + " with uuid " + m_uuid);
return;
}
Debug.Log("[ANDROID] Pipeline initialization successful");
//m_Unity_Webcam = true;
#else
// When the application is built, only the pipeline configuration files used by the application are moved to the streamingAssets folder
if (!m_pipelineManager.init(Application.streamingAssetsPath + m_configurationPath, m_uuid))
{
Debug.Log("Cannot init pipeline manager " + Application.streamingAssetsPath + m_configurationPath + " with uuid " + m_uuid);
return;
}
//m_Unity_Webcam = true;
#endif
if (m_Unity_Webcam)
{
//m_webCamTexture = new WebCamTexture(WebCamTexture.devices[m_webCamNum].name, width, height);
m_webCamTexture = deviceCameraScript.activeCameraTexture;
m_webCamTexture.Play();
data = new Color32[width * height];
m_vidframe_byte = new byte[width * height * 3];
GetPhysicalCameraFrame();
}
else
{
Matrix3x3f camParams = m_pipelineManager.getCameraParameters().intrinsic;
width = Screen.width;
height = Screen.height;
focalX = camParams.coeff(0, 0); // focalX;
focalY = camParams.coeff(1, 1); // focalY;
centerX = camParams.coeff(0, 2); // centerX;
centerY = camParams.coeff(1, 2); // centerY;
}
SendParametersToCameraProjectionMatrix();
array_imageData = new byte[width * height * 3];
IntPtr ptr = Marshal.UnsafeAddrOfPinnedArrayElement(array_imageData, 0);
m_pipelineManager.start(ptr); //IntPtr
//deviceCameraScript.UpdateScreenParams();
//StartCoroutine(deviceCameraScript.UpdateScreenParamsCoroutine());
UpdateReady = true;
}
public virtual void filter(DescriptorMatchVector inputMatches, DescriptorMatchVector outputMatches, KeypointArray inputKeyPoints1, KeypointArray inputKeyPoints2, Transform3Df pose1, Transform3Df pose2, Matrix3x3f intrinsicParams)
{
solar_api_featuresPINVOKE.IMatchesFilter_filter__SWIG_1(swigCPtr, DescriptorMatchVector.getCPtr(inputMatches), DescriptorMatchVector.getCPtr(outputMatches), KeypointArray.getCPtr(inputKeyPoints1), KeypointArray.getCPtr(inputKeyPoints2), Transform3Df.getCPtr(pose1), Transform3Df.getCPtr(pose2), Matrix3x3f.getCPtr(intrinsicParams));
if (solar_api_featuresPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_featuresPINVOKE.SWIGPendingException.Retrieve();
}
}
void IShaderUniformContainer.SetUniform(GraphicsContext ctx, string uniformName, Matrix3x3f m)
{
throw new NotImplementedException();
}
public override void SetCameraParameters(Matrix3x3f intrinsics, Vector5f distorsion)
{
// initialize pose estimation
poseEstimationPlanar.setCameraParameters(intrinsics, distorsion);
}
public override void Transform(Matrix3x3f m)
{
Position = m * Position;
}
/// <summary>
/// Perform a polar decomposition of matrix M and return the rotation matrix R. This method handles the degenerated cases.
/// </summary>am>
public static void PolarDecompositionStable(Matrix3x3f M, float tolerance, out Matrix3x3f R)
{
Matrix3x3f Mt = M.Transpose;
float Mone = OneNorm(M);
float Minf = InfNorm(M);
float Eone;
Matrix3x3f MadjTt = new Matrix3x3f();
Matrix3x3f Et = new Matrix3x3f();
do
{
MadjTt.SetRow(0, Mt.GetRow(1).Cross(Mt.GetRow(2)));
MadjTt.SetRow(1, Mt.GetRow(2).Cross(Mt.GetRow(0)));
MadjTt.SetRow(2, Mt.GetRow(0).Cross(Mt.GetRow(1)));
float det = Mt.m00 * MadjTt.m00 + Mt.m01 * MadjTt.m01 + Mt.m02 * MadjTt.m02;
if (Math.Abs(det) < 1.0e-12f)
{
int index = int.MaxValue;
for (int i = 0; i < 3; i++)
{
float len = MadjTt.GetRow(i).SqrMagnitude;
if (len > 1.0e-12f)
{
// index of valid cross product
// => is also the index of the vector in Mt that must be exchanged
index = i;
break;
}
}
if (index == int.MaxValue)
{
R = Matrix3x3f.Identity;
return;
}
else
{
Mt.SetRow(index, Mt.GetRow((index + 1) % 3).Cross(Mt.GetRow((index + 2) % 3)));
MadjTt.SetRow((index + 1) % 3, Mt.GetRow((index + 2) % 3).Cross(Mt.GetRow(index)));
MadjTt.SetRow((index + 2) % 3, Mt.GetRow(index).Cross(Mt.GetRow((index + 1) % 3)));
Matrix3x3f M2 = Mt.Transpose;
Mone = OneNorm(M2);
Minf = InfNorm(M2);
det = Mt.m00 * MadjTt.m00 + Mt.m01 * MadjTt.m01 + Mt.m02 * MadjTt.m02;
}
}
float MadjTone = OneNorm(MadjTt);
float MadjTinf = InfNorm(MadjTt);
float gamma = (float)Math.Sqrt(Math.Sqrt((MadjTone * MadjTinf) / (Mone * Minf)) / Math.Abs(det));
float g1 = gamma * 0.5f;
float g2 = 0.5f / (gamma * det);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
Et[i, j] = Mt[i, j];
Mt[i, j] = g1 * Mt[i, j] + g2 * MadjTt[i, j];
Et[i, j] -= Mt[i, j];
}
}
Eone = OneNorm(Et);
Mone = OneNorm(Mt);
Minf = InfNorm(Mt);
}while (Eone > Mone * tolerance);
// Q = Mt^T
R = Mt.Transpose;
//end of function
}
/// <summary>
/// Perform a singular value decomposition of matrix A: A = U * sigma * V^T.
/// This function returns two proper rotation matrices U and V^T which do not
/// contain a reflection. Reflections are corrected by the inversion handling
/// proposed by Irving et al. 2004.
/// </summary>
public static void SVDWithInversionHandling(Matrix3x3f A, out Vector3f sigma, out Matrix3x3f U, out Matrix3x3f VT)
{
Vector3f S;
Matrix3x3f AT_A, V;
AT_A = A.Transpose * A;
// Eigen decomposition of A^T * A
EigenDecomposition(AT_A, out V, out S);
int pos;
// Detect if V is a reflection .
// Make a rotation out of it by multiplying one column with -1.
float detV = V.Determinant;
if (detV < 0.0f)
{
float minLambda = float.PositiveInfinity;
pos = 0;
for (int l = 0; l < 3; l++)
{
if (S[l] < minLambda)
{
pos = l;
minLambda = S[l];
}
}
V[0, pos] = -V[0, pos];
V[1, pos] = -V[1, pos];
V[2, pos] = -V[2, pos];
}
if (S.x < 0.0f)
{
S.x = 0.0f; // safety for sqrt
}
if (S.y < 0.0f)
{
S.y = 0.0f;
}
if (S.z < 0.0f)
{
S.z = 0.0f;
}
sigma.x = (float)Math.Sqrt(S.x);
sigma.y = (float)Math.Sqrt(S.y);
sigma.z = (float)Math.Sqrt(S.z);
VT = V.Transpose;
// Check for values of hatF near zero
int chk = 0;
pos = 0;
for (int l = 0; l < 3; l++)
{
if (Math.Abs(sigma[l]) < 1.0e-4f)
{
pos = l;
chk++;
}
}
if (chk > 0)
{
if (chk > 1)
{
U = Matrix3x3f.Identity;
}
else
{
U = A * V;
for (int l = 0; l < 3; l++)
{
if (l != pos)
{
for (int m = 0; m < 3; m++)
{
U[m, l] *= 1.0f / sigma[l];
}
}
}
Vector3f[] v = new Vector3f[2];
int index = 0;
for (int l = 0; l < 3; l++)
{
if (l != pos)
{
v[index++] = new Vector3f(U[0, l], U[1, l], U[2, l]);
}
}
Vector3f vec = v[0].Cross(v[1]);
vec.Normalize();
U[0, pos] = vec[0];
U[1, pos] = vec[1];
U[2, pos] = vec[2];
}
}
else
{
Vector3f sigmaInv = new Vector3f(1.0f / sigma.x, 1.0f / sigma.y, 1.0f / sigma.z);
U = A * V;
for (int l = 0; l < 3; l++)
{
for (int m = 0; m < 3; m++)
{
U[m, l] *= sigmaInv[l];
}
}
}
float detU = U.Determinant;
// U is a reflection => inversion
if (detU < 0.0f)
{
//std::cout << "Inversion!\n";
float minLambda = float.PositiveInfinity;
pos = 0;
for (int l = 0; l < 3; l++)
{
if (sigma[l] < minLambda)
{
pos = l;
minLambda = sigma[l];
}
}
// invert values of smallest singular value
sigma[pos] = -sigma[pos];
U[0, pos] = -U[0, pos];
U[1, pos] = -U[1, pos];
U[2, pos] = -U[2, pos];
}
//end of function
}
// Compute the two intersecting lines of a cone with a corner at (0,0,0) with a plane passing by (0,0,0)
// If there is no intersection, return the line on the plane closest to the cone (assumes cone_angle > pi/2)
static void IntersectConePlane(Vector3d cone_direction, double cone_angle, Vector3d plane_normal, out Vector3d intersect_1, out Vector3d intersect_2)
{
intersect_1 = Vector3d.zero;
intersect_2 = Vector3d.zero;
cone_direction.Normalize();
plane_normal.Normalize();
// Change the frame of reference:
// x = cone_direction
// y = plane_normal projected on the plane normal to cone_direction
// z = x * y
Vector3d x = cone_direction;
Vector3d z = Vector3d.Cross(x, plane_normal).normalized;
Vector3d y = Vector3d.Cross(z, x);
// transition matrix from the temporary frame to the global frame
Matrix3x3f M_i_tmp = new Matrix3x3f();
for (int i = 0; i < 3; i++)
{
M_i_tmp[i, 0] = (float)x[i];
M_i_tmp[i, 1] = (float)y[i];
M_i_tmp[i, 2] = (float)z[i];
}
Vector3d n = M_i_tmp.transpose() * plane_normal;
double cos_phi = -n.x / (n.y * Math.Tan(cone_angle));
if (Math.Abs(cos_phi) <= 1.0f)
{
intersect_1.x = Math.Cos(cone_angle);
intersect_1.y = Math.Sin(cone_angle) * cos_phi;
intersect_1.z = Math.Sin(cone_angle) * Math.Sqrt(1 - cos_phi * cos_phi);
}
else
{
intersect_1.x = -n.y;
intersect_1.y = n.x;
intersect_1.z = 0.0;
}
intersect_2.x = intersect_1.x;
intersect_2.y = intersect_1.y;
intersect_2.z = -intersect_1.z;
intersect_1 = M_i_tmp * intersect_1;
intersect_2 = M_i_tmp * intersect_2;
}
public virtual void Transform(Matrix3x3f m)
{
}
/// <summary>
/// Perform polar decomposition A = (U D U^T) R
/// </summary>
public static void PolarDecomposition(Matrix3x3f A, out Matrix3x3f R, out Matrix3x3f U, out Matrix3x3f D)
{
// A = SR, where S is symmetric and R is orthonormal
// -> S = (A A^T)^(1/2)
// A = U D U^T R
Matrix3x3f AAT = new Matrix3x3f();
AAT.m00 = A.m00 * A.m00 + A.m01 * A.m01 + A.m02 * A.m02;
AAT.m11 = A.m10 * A.m10 + A.m11 * A.m11 + A.m12 * A.m12;
AAT.m22 = A.m20 * A.m20 + A.m21 * A.m21 + A.m22 * A.m22;
AAT.m01 = A.m00 * A.m10 + A.m01 * A.m11 + A.m02 * A.m12;
AAT.m02 = A.m00 * A.m20 + A.m01 * A.m21 + A.m02 * A.m22;
AAT.m12 = A.m10 * A.m20 + A.m11 * A.m21 + A.m12 * A.m22;
AAT.m10 = AAT.m01;
AAT.m20 = AAT.m02;
AAT.m21 = AAT.m12;
R = Matrix3x3f.Identity;
Vector3f eigenVals;
EigenDecomposition(AAT, out U, out eigenVals);
float d0 = (float)Math.Sqrt(eigenVals.x);
float d1 = (float)Math.Sqrt(eigenVals.y);
float d2 = (float)Math.Sqrt(eigenVals.z);
D = new Matrix3x3f();
D.m00 = d0;
D.m11 = d1;
D.m22 = d2;
const float eps = 1e-15f;
float l0 = eigenVals.x; if (l0 <= eps)
{
l0 = 0.0f;
}
else
{
l0 = 1.0f / d0;
}
float l1 = eigenVals.y; if (l1 <= eps)
{
l1 = 0.0f;
}
else
{
l1 = 1.0f / d1;
}
float l2 = eigenVals.z; if (l2 <= eps)
{
l2 = 0.0f;
}
else
{
l2 = 1.0f / d2;
}
Matrix3x3f S1 = new Matrix3x3f();
S1.m00 = l0 * U.m00 * U.m00 + l1 * U.m01 * U.m01 + l2 * U.m02 * U.m02;
S1.m11 = l0 * U.m10 * U.m10 + l1 * U.m11 * U.m11 + l2 * U.m12 * U.m12;
S1.m22 = l0 * U.m20 * U.m20 + l1 * U.m21 * U.m21 + l2 * U.m22 * U.m22;
S1.m01 = l0 * U.m00 * U.m10 + l1 * U.m01 * U.m11 + l2 * U.m02 * U.m12;
S1.m02 = l0 * U.m00 * U.m20 + l1 * U.m01 * U.m21 + l2 * U.m02 * U.m22;
S1.m12 = l0 * U.m10 * U.m20 + l1 * U.m11 * U.m21 + l2 * U.m12 * U.m22;
S1.m10 = S1.m01;
S1.m20 = S1.m02;
S1.m21 = S1.m12;
R = S1 * A;
// stabilize
Vector3f c0, c1, c2;
c0 = R.GetColumn(0);
c1 = R.GetColumn(1);
c2 = R.GetColumn(2);
if (c0.SqrMagnitude < eps)
{
c0 = c1.Cross(c2);
}
else if (c1.SqrMagnitude < eps)
{
c1 = c2.Cross(c0);
}
else
{
c2 = c0.Cross(c1);
}
R.SetColumn(0, c0);
R.SetColumn(1, c1);
R.SetColumn(2, c2);
}
public virtual void setCameraParameters(Matrix3x3f intrinsicParams, Vector5f distorsionParams)
{
solar_api_solver_posePINVOKE.I2Dto3DTransformDecomposer_setCameraParameters(swigCPtr, Matrix3x3f.getCPtr(intrinsicParams), Vector5f.getCPtr(distorsionParams));
if (solar_api_solver_posePINVOKE.SWIGPendingException.Pending)
{
throw solar_api_solver_posePINVOKE.SWIGPendingException.Retrieve();
}
}
public virtual double solve(KeyframeList framesToAdjust, CloudPointVector mapToAdjust, Matrix3x3f K, Vector5f D, IntVector selectKeyframes, Transform3DfList poseAdjusted, CloudPointVector mapAdjusted, Matrix3x3f KAdjusted, Vector5f DAdjusted)
{
double ret = solar_api_solver_mapPINVOKE.IBundler_solve(swigCPtr, KeyframeList.getCPtr(framesToAdjust), CloudPointVector.getCPtr(mapToAdjust), Matrix3x3f.getCPtr(K), Vector5f.getCPtr(D), IntVector.getCPtr(selectKeyframes), Transform3DfList.getCPtr(poseAdjusted), CloudPointVector.getCPtr(mapAdjusted), Matrix3x3f.getCPtr(KAdjusted), Vector5f.getCPtr(DAdjusted));
if (solar_api_solver_mapPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_solver_mapPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
/// <summary>
/// Creates matrix fixed.
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public Float3x3 Fixed(Matrix3x3f m)
{
ConstantOperation c = dag.CreateFixed(m);
return(new Float3x3(c.Outputs[0], this));
}
/// <summary>
/// The mapper with transform.
/// </summary>
/// <param name="mapper"></param>
/// <param name="transform"></param>
public TransformMapper([NotNull] IMapper mapper, Matrix3x3f transform)
{
this.internalMapper = mapper;
this.transform = transform;
}
public abstract void SetCameraParameters(Matrix3x3f intrinsic, Vector5f distorsion);
public override void SetCameraParameters(Matrix3x3f intrinsics, Vector5f distorsion)
{
PnP.setCameraParameters(intrinsics, distorsion);
}
