public SE3 Inverse()
{
SE3 result = new SE3();
result.SO3 = SO3.Inverse();
result.Translation = -1 * result.SO3.TransformVector(Translation);
return(result);
}
public Sim3 Inverse()
{
Sim3 result = new Sim3();
result.SE3 = SE3.Inverse();
result.Scale = 1 / Scale;
result.SE3.Translation = new Vector3(result.Scale * result.SE3.Translation.Vector);
return(result);
}
public Sim3(double[,] matrix)
{
Scale = matrix[3, 3];
_SE3 = new SE3(matrix);
}
public Sim3(double[][] matrix)
{
Scale = matrix[3][3];
_SE3 = new SE3(matrix);
}
public Sim3(double scale, SE3 se3)
{
Scale = scale;
_SE3 = se3;
}