23 lines
954 B
C#
23 lines
954 B
C#
|
using Intel.RealSense;
|
|||
|
|
using System;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using System.Linq;
|
|||
|
|
using System.Text;
|
|||
|
|
using System.Threading.Tasks;
|
|||
|
|
using Vector = Intel.RealSense.Math.Vector;
|
|||
|
|
namespace WindowsFormsApp1
|
|||
|
|
{
|
|||
|
|
public static class ExtrinsicsExtensions
|
|||
|
|
{
|
|||
|
|
public static Vector Transform(this Extrinsics extrin, Vector from_point)
|
|||
|
|
{
|
|||
|
|
Vector to_point;
|
|||
|
|
// 使用外部參數執行坐標轉換
|
|||
|
|
to_point.x = extrin.rotation[0] * from_point.x + extrin.rotation[3] * from_point.y + extrin.rotation[6] * from_point.z + extrin.translation[0];
|
|||
|
|
to_point.y = extrin.rotation[1] * from_point.x + extrin.rotation[4] * from_point.y + extrin.rotation[7] * from_point.z + extrin.translation[1];
|
|||
|
|
to_point.z = extrin.rotation[2] * from_point.x + extrin.rotation[5] * from_point.y + extrin.rotation[8] * from_point.z + extrin.translation[2];
|
|||
|
|
return to_point;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|