一，ARSceneTransformer类的关键函数
1.arSceneTransform函数：
输入值是后端返回给前端的字符串，字符串中包含图片id，pose，inliners，timestamp
返回值一个字符串给后端（用来做一个记录，可以不管）
public string arSceneTransform(ref string receiveMsg)
{
returnMsg = "";
returnMsg += receiveMsg;
if (receiveMsg.StartsWith("success"))
{
int imgId;
Pose pose;
int inliners;
float timestamp;
if(parseReceiveMsg(ref receiveMsg, out imgId, out pose, out inliners, out timestamp,';',' '))
{
if (queryWaitList[imgId % queryWaitListLength].id == imgId)
{
pose = ARTools.poseChangeChirality(pose); //后端位姿，右->左
//Matrix4x4 Tcw1 = Matrix4x4.TRS(pose.position, pose.rotation, Vector3.one);
Matrix4x4 Tcw1 = ARTools.pose2Matrix(pose,true); //转为矩阵进行操作
Pose frontPose = queryWaitList[imgId % queryWaitListLength].pose; //前端位姿
//Matrix4x4 Tcw2 = Matrix4x4.TRS(frontPose.position, frontPose.rotation, Vector3.one);
Matrix4x4 Tcw2 = ARTools.pose2Matrix(frontPose, true); ;
computeTransMatrix(ref Tcw1, ref Tcw2, predictScale); // Change
transformSceneObject();
returnMsg += ";";
returnMsg += "frontPose " + ARTools.pose2Str(ref frontPose);
returnMsg += ";";
returnMsg += "transMatrix " + ARTools.matrix2Str(ref transformMatrix);
returnMsg += ";";
returnMsg += "predictScale " + predictScale.ToString();
// 返回current_camera
Pose transServerPose = pose;
transServerPose = ARTools.getInversePose(transServerPose, true);
transServerPose = transformPose(ref transformMatrix, transServerPose); //变换后端位姿
returnMsg += ";";
returnMsg += "transServerPose " + ARTools.pose2Str(ref transServerPose);
}
}
}
return returnMsg;
}2.、computeTransMatrix：用来计算转换矩阵，将场景坐标转换为前端世界坐标，输入参数Tcw1是后端相机坐标系下的查询图像的位姿，输入参数Tcw2是前端相机坐标系下的查询图像位姿
void computeTransMatrix(ref Matrix4x4 Tcw1, ref Matrix4x4 Tcw2, float scale21)
{
//w1后端世界坐标系
//w2前端世界坐标系
//相机的位姿Tcw，世界坐标到相机坐标的变换
//相机的坐标Twc，是相机坐标到世界坐标的变换，col(3)即为相机在世界坐标系下的坐标（Twc*原点=世界坐标系下的相机原点坐标）
Matrix4x4 Tw1c = Tcw1.inverse;
Matrix4x4 Tw2c = Tcw2.inverse;
Matrix4x4 Tscale21 = Matrix4x4.identity;
Tscale21.m00 = scale21;
Tscale21.m11 = scale21;
Tscale21.m22 = scale21;
Vector3 sceneCameraPos = Tw1c.GetColumn(3);
Vector3 worldCameraPos = Tw2c.GetColumn(3);
if (firstLocalize)
{
firstLocalize = false;
firstScenePos = sceneCameraPos;
firstWorldPos = worldCameraPos;
}
else
{
updateScale(ref sceneCameraPos, ref worldCameraPos);
}
transformMatrix = Tw2c * Tscale21 * Tcw1; //Tw2w1，场景坐标到世界坐标
}3. updateScale：估计前后端地图的尺度比例，Scale = WorldDist/SceneDist
void updateScale(ref Vector3 currentScenePos, ref Vector3 currentWorldPos, int updateMethod=0)
{
// 用最近两个相机位姿的距离估计比例
float localSceneDist = Vector3.Distance(lastScenePos, currentScenePos);
float localWorldDist = Vector3.Distance(lastWorldPos, currentWorldPos);
if (localWorldDist < minSceneDistanceToUpdateScale)
return;
localComputeScale = localWorldDist / localSceneDist;
//if(localWorldDist > minSceneDistanceToUpdateScale)
// localComputeScale = localWorldDist / localSceneDist;
// 用First和Last两个相机位姿的距离估计比例
float fGlobalSceneDist = Vector3.Distance(firstScenePos, currentScenePos);
float fGlobalWorldDist = Vector3.Distance(firstWorldPos, currentWorldPos);
if (fGlobalSceneDist > minSceneDistanceToUpdateScale)
globalComputeScale = fGlobalWorldDist / fGlobalSceneDist;
lastScenePos = currentScenePos;
lastWorldPos = currentWorldPos;
// 更新数据（当前使用局部距离计算,0.1更新率）
if (lockUpdateScale)
return;
float computeScale = localComputeScale;
if (!usePreComputeScale) // 无预计算尺度比例时，用第一次估算的比例初始化predictScale
{
if(localSceneDist > minSceneDistanceToUpdateScale)
{
usePreComputeScale = true;
predictScale = computeScale;
}
}
else
{
predictScale = scaleChangeFactor * computeScale + (1.0f - scaleChangeFactor) * predictScale;
}
debugText.text = "";
debugText.text += "\nlocalSceneDist " + localSceneDist.ToString();
debugText.text += "\nlocalWorldDist " + localWorldDist.ToString();
debugText.text += "\nlocalComputeScale " + localComputeScale.ToString();
debugText.text += "\npredictScale " + predictScale.ToString();
debugText.text += "\nFaList[0].localScale " + FaList[0].localScale.ToString();
debugText.text += "\nVOList[0].transform.localScale " + VOList[0].transform.localScale.ToString();
//if (i == 0)
//{
// Debug.Log("uuu predictScale " + predictScale.ToString());
// Debug.Log("uuu FaList[i].localScale" + FaList[i].localScale);
// Debug.Log("uuu VOList[i].transform.localScale" + VOList[i].transform.localScale);
//}
}二、ARTools类
该类主要是工具类。
主要包括pose与矩阵数据结构的转换，做右手系的转换、定义请求节点的数据结构，放置的虚拟物体的数据结构。
主要内容：
1.ARTools类中有一个QueryNode类，用来定义请求节点的数据结构，每一次请求都会记录图片id与图片位姿
public class QueryNode
{
public QueryNode(int img_id, Pose img_pose)
{
pose = img_pose;
id = img_id;
timestamp = 0f;
}
public QueryNode(int img_id, Pose img_pose, float time_stamp)
{
pose = img_pose;
id = img_id;
timestamp = time_stamp;
}
public Pose pose { get; set; }
public int id { get; set; }
public float timestamp { get; set; }
}2.ModelInfo，用来存储预制体的信息
public class ModelInfo
{
public int id;
public string filepath;
public string suffix;
public float[] position;
public float[] rotation;
public float[] scale;
public bool rightHand;
public ModelInfo(int _id,string _filepath,string _suffix, Vector3 _position,Quaternion _rotation,Vector3 _scale, bool _rightHand)
{
id = _id;
filepath = _filepath;
suffix = _suffix;
position = new float[] { _position.x, _position.y, _position.z };
rotation = new float[] { _rotation.x, _rotation.y, _rotation.z, _rotation.w };
scale = new float[] { _scale.x, _scale.y, _scale.z };
rightHand = _rightHand;
}
}
[System.Serializable]
public class ModelInfoList
{
public List<ModelInfo> model_config;
}