#include"pch.h"
///进行一次扫描,转换为大地坐标系,并作为参考数据
if(!m_client->scan_to_reference())//把基于大地坐标系的扫描数据放到参考窗口
return;
//删除底面
//delete_plane(m_gun_PlaneInfo);
bin::vector<techlego::pos6f>plane_points1;
bin::vector_h<techlego::h_point_info>pt_clouds1;
m_client->get_scan_data_by_index(pt_clouds1,0);
for(int i=0;i<pt_clouds1.m_size;i++)
{
bin::vector<double>pt;
techlego::pos6f pos6f;
pt_clouds1[i].get_point_info(pt);
pos6f.m_x=pt[0];
pos6f.m_y=pt[1];
pos6f.m_z=pt[2];
pos6f.m_nx=pt[3];
pos6f.m_ny=pt[4];
pos6f.m_nz=pt[5];
plane_points1.emplace_back(pos6f);
}
//把当前点作为平面上的点,点的法相作为平面的法向,删除点云(删除底面)
plane.filter_points_by_plane(plane_points1,-500,filter_height);
m_client->load_feature_align_data_from_memory(plane_points1,true);
//定义机器手移动位姿
techlego::pose_3d res;
techlego::pose_3d res1;
double error;
bin::vector<int>inde;
///调用feature_align
if(!m_client->feature_align(res,error,inde))//将移动数据移动到与参考数据重合,并得到移动位姿
{
if(filter_height==filter_height_max)
{
BCGPMessageBox(m_hWnd,L"未识别到目标!\n开始识别下一层",L"提示",MB_OK|MB_ICONHAND|MB_ICONASTERISK);
filter_height=3;
continue;
}
if(filter_height==filter_height_min)
{
BCGPMessageBox(m_hWnd,L"未识别到目标!停止扫描",L"提示",MB_OK|MB_ICONHAND|MB_ICONASTERISK);
break;
}
}