编辑FunnyMap对象

请找管理员授权。/funnyscript/edit_node_item.php

using Common_Robot2;
using ConverxHull;
using MathNet.Numerics.LinearAlgebra.Double;
using System.Text;
using Three.Net.Math;

namespace Test1
{
    public class Cloud_Dazu_S35_Camera_To_Arm : C_Node
    {
        public string key_read = "";
        public string key_save = "";
        public string six = "";
        public string key_robot="";

public Cloud_Dazu_S35_Camera_To_Arm(string name, C_Space space_parent, C_Space space) :
            base(name, space_parent, space)
        {
        }

public override void init()
        {
        }

public override Task run_sub()
        {
            run_sub_main();
            return Task.CompletedTask;
        }

/// <summary>
        /// 虚拟相机1，和机械臂只是一个平移关系
        /// 虚拟相机2，和虚拟相机1是一个旋转关系
        /// 虚拟相机2 到相机的变换为M
        /// </summary>
        public void run_sub_main()
        {

C_Robot_DaZu_S35 pRobot2 = (C_Robot_DaZu_S35)this.read_var(this.key_robot, "");

C_Camera_TuYang camera1 = new C_Camera_TuYang();

camera1.file = "D:\\Data\\calibration\\virtual_2_camera.txt";

C_Point3D p1;
            C_Point3D p2;
            DenseMatrix m11;
            (p1, p2, m11) = Main.读取标定文件(camera1.file);

camera1.p1_center = p1;
            camera1.p2_center = p2;
            camera1.camera1.p1_center = p1;
            camera1.camera1.p2_center = p2;

camera1.M_Rotate = m11; //坐标变换矩阵
            camera1.camera1.M_Rotate = m11;

//Main.计算标定数据误差(this, camera1.file, m11, p1, p2);

string line = this.read_string(six);
            string[] strSplit = line.Split(",");
            float[] angles = new float[6];

for (var i = 0; i < Math.Min(angles.Length, strSplit.Length); i++)
            {
                angles[i] = float.Parse(strSplit[i].Trim());
            }
            //angles[5] += 131;

(Matrix4 m1, Matrix4 m2, C_Point3D vX, C_Point3D vY, C_Point3D vZ)
                = Main_Robot.DaZu_S35_机械臂变换矩阵(pRobot2, angles);

//机械臂坐标 -233.723,-1286.058,-246.172

List<C_Point3D> list = (List<C_Point3D>)this.read_var(this.key_read, "List<C_Point3D>");
            if (list == null)
            {
                MessageBox.Show(this.Name+ "点云数据为null");
                return;
            }

List<C_Point3D> list2 = new List<C_Point3D>();
            for (var i = 0; i < list.Count; i++)
            {

C_Point3D x0 = list[i];
                if (x0 == null) continue;
                C_Point3D v_camera_v2 = Main_Robot.Convert_Matrix_Invert(camera1, x0);

C_Point3D v_camera_v1 = Main_Point.Multiply_Point(Matrix4.GetInverse(m2), v_camera_v2);
                
                C_Point3D arm = Main_Point.Multiply_Point(Matrix4.GetInverse(m1), v_camera_v1);
                arm.pExtend = x0.pExtend;
                list2.Add(arm);
            }
            this.save_var(this.key_save, "List<C_Point3D>", list2);

}

}
}

ID=10721 Cloud_Dazu_S35_Camera_To_Arm.cs
using Common_Robot2; using ConverxHull; using MathNet.Numerics.LinearAlgebra.Double; using System.Text; using Three.Net.Math; namespace Test1 { public class Cloud_Dazu_S35_Camera_To_Arm : C_Node { public string key_read = ""; public string key_save = ""; public string six = ""; public string key_robot=""; public Cloud_Dazu_S35_Camera_To_Arm(string name, C_Space space_parent, C_Space space) : base(name, space_parent, space) { } public override void init() { } public override Task run_sub() { run_sub_main(); return Task.CompletedTask; } /// <summary> /// 虚拟相机1，和机械臂只是一个平移关系 /// 虚拟相机2，和虚拟相机1是一个旋转关系 /// 虚拟相机2 到相机的变换为M /// </summary> public void run_sub_main() { C_Robot_DaZu_S35 pRobot2 = (C_Robot_DaZu_S35)this.read_var(this.key_robot, ""); C_Camera_TuYang camera1 = new C_Camera_TuYang(); camera1.file = "D:\\Data\\calibration\\virtual_2_camera.txt"; C_Point3D p1; C_Point3D p2; DenseMatrix m11; (p1, p2, m11) = Main.读取标定文件(camera1.file); camera1.p1_center = p1; camera1.p2_center = p2; camera1.camera1.p1_center = p1; camera1.camera1.p2_center = p2; camera1.M_Rotate = m11; //坐标变换矩阵 camera1.camera1.M_Rotate = m11; //Main.计算标定数据误差(this, camera1.file, m11, p1, p2); string line = this.read_string(six); string[] strSplit = line.Split(","); float[] angles = new float[6]; for (var i = 0; i < Math.Min(angles.Length, strSplit.Length); i++) { angles[i] = float.Parse(strSplit[i].Trim()); } //angles[5] += 131; (Matrix4 m1, Matrix4 m2, C_Point3D vX, C_Point3D vY, C_Point3D vZ) = Main_Robot.DaZu_S35_机械臂变换矩阵(pRobot2, angles); //机械臂坐标 -233.723,-1286.058,-246.172 List<C_Point3D> list = (List<C_Point3D>)this.read_var(this.key_read, "List<C_Point3D>"); if (list == null) { MessageBox.Show(this.Name+ "点云数据为null"); return; } List<C_Point3D> list2 = new List<C_Point3D>(); for (var i = 0; i < list.Count; i++) { C_Point3D x0 = list[i]; if (x0 == null) continue; C_Point3D v_camera_v2 = Main_Robot.Convert_Matrix_Invert(camera1, x0); C_Point3D v_camera_v1 = Main_Point.Multiply_Point(Matrix4.GetInverse(m2), v_camera_v2); C_Point3D arm = Main_Point.Multiply_Point(Matrix4.GetInverse(m1), v_camera_v1); arm.pExtend = x0.pExtend; list2.Add(arm); } this.save_var(this.key_save, "List<C_Point3D>", list2); } } }