zjlooojoe
commented
5 years ago 你好，想问一下你知道为什么这个误差项里面为什么会这么计算吗？ ` class EncoderFrame2FrameError { public: EncoderFrame2FrameError ( const double& delta_x, const double& delta_y, const double& delta_theta, const Eigen::Matrix3d sqrt_info ) : deltax ( delta_x ), deltay ( delta_y ), deltatheta ( delta_theta ), sqrtinfo ( sqrt_info ) {}
template <typename T>
bool operator() ( const T* const ref_x, const T* const ref_y, const T* const ref_th,
const T* const cur_x, const T* const cur_y, const T* const cur_th,
T* residuals ) const {
// ref pose
T xr = ref_x[0];
T yr = ref_y[0];
T thr = ref_th[0];
// cur pose
T xc = cur_x[0];
T yc = cur_y[0];
T thc = cur_th[0];
T ob_dx = T ( delta_x_ );
T ob_dy = T ( delta_y_ );
T ob_dth = T ( delta_theta_ );
T tmp_dx = xc - xr;
T tmp_dy = yc - yr;
T dx = cos ( thr ) *tmp_dx + sin ( thr ) *tmp_dy;
T dy = -sin ( thr ) *tmp_dx + cos ( thr ) *tmp_dy;
T ex = dx - ob_dx;
T ey = dy - ob_dy;
T eth = NormalizeAngle ( thc - thr - ob_dth );// [(cur_th - ref_th) - delta_enc_th]
residuals[0] = T ( sqrt_info_ ( 0,0 ) ) * ex + T ( sqrt_info_ ( 0,1 ) ) * ey + T ( sqrt_info_ ( 0,2 ) ) * eth ;
residuals[1] = T ( sqrt_info_ ( 1,0 ) ) * ex + T ( sqrt_info_ ( 1,1 ) ) * ey + T ( sqrt_info_ ( 1,2 ) ) * eth ;
residuals[2] = T ( sqrt_info_ ( 2,0 ) ) * ex + T ( sqrt_info_ ( 2,1 ) ) * ey + T ( sqrt_info_ ( 2,2 ) ) * eth ;
return true;
}`比较不明白的是这个 T dx = cos ( thr ) tmp_dx + sin ( thr ) tmp_dy; T dy = -sin ( thr ) tmp_dx + cos ( thr ) tmp_dy; 不是直接xc-xr就可以作为视觉方面的delta项了吗？为什么还要再乘以他们的cos？这是因为什么啊？
BloodLemonS
您好: 首先非常感谢您的开源.论文和代码都写的非常棒, 简洁明了. 用您的数据集测试的结果也比较好.
我用我们自己的robot采集的数据集进行测试,走一个大圈,大概60-70米之后,回到初始位置,关闭闭环之后,漂移比较大x:-0.178m, y:0.945m, theta:9.42度. 打开回环之后: x: -0.0118m, y:0.00549m, theta: 2.8度. 而单里程计计算的结果为: x: -0.014675, y: 0.1735, theta: 4.68. 也就是说,如果没有回环, 系统计算的结果没有单里程计计算的结果准确.
想咨询下, 您有没有做过类似的测试,是否也有如此的情况. 我现在怀疑有几个原因, 一是,一路上深度相机的有效部分比较少,因为这一圈是一个走廊,两边很多玻璃,好几处地方的高度有几cm的差别. 二是, rgbd相机与odom的外参标定不够准确.
谢谢,麻烦您了