VectorXd imgpos(6);
imgpos << 0, M_PI/2, -M_PI*16/18, 0, M_PI*5/18, 0;
arm.m_captenv.clear();
CAPT_POSE captenvpose;
captenvpose.got = false;
captenvpose.jointspos = imgpos;
arm.m_captenv.push_back(captenvpose);
if m_captenv not initialized, m_captenv[0] is null, will break with segmentation fault.
arm.m_d435i.getframe();
arm.m_d435i.getpoints();
arm.addpointsenv(arm.m_d435i.m_points, 0);
实际上的错误是OpenCV的深度图和彩色图对齐后得到的点云和原始的frame的直接计算点云不一样导致的,对点云的索引导致了段错误,具体原因未知,改成不对齐的深度图计算点云就可以了。