public SE3(double[,] matrix)
{
_Translation.X = matrix[0, 3];
_Translation.Y = matrix[1, 3];
_Translation.Z = matrix[2, 3];
_SO3 = new SO3(matrix);
}
public SE3(double[][] matrix)
{
_Translation.X = matrix[0][3];
_Translation.Y = matrix[1][3];
_Translation.Z = matrix[2][3];
_SO3 = new SO3(matrix);
}
public SO3 Inverse()
{
SO3 result = new SO3();
result.Quaternion = Quaternion.Inverse();
return(result);
}
public SE3 Inverse()
{
SE3 result = new SE3();
result.SO3 = SO3.Inverse();
result.Translation = -1 * result.SO3.TransformVector(Translation);
return(result);
}
public Sim3(double scale, Vector3 translation, SO3 so3) : this(scale, new SE3(translation, so3))
{
}
public SE3(Vector3 translation, SO3 so3)
{
Translation = translation;
SO3 = so3;
}