Closed laoshiweiyu closed 2 years ago
Hi, thank you for your interest. I published 2 versions of REVO (i) the purely edge-based method at IROS and (ii) the edge- and ICP-based version at BMVC. Since getting code into a releasable version is a lot of work and the edge-based part is the most interesting one, I released only the purely edge-based method from IROS with a few improvements such as edge enhancement from BMVC. Point-to-plane ICP is well-known and has been used for a long time. You could look at systems such as InfiniTAM and KinectFusion for the respective implementation or you could try to implement it yourself.
Here are a few hints about how to add it to REVO: You need to fill the buffers similar to the edge-based error. You can reuse all of the buffers but an additional one is necessary. The Jacobian of the ICP point-to-plane error is simply J = [normal, cross_product(Wxp,normal), where Wxp is the warped 3D point and kfPclElem the corresponding 3D point.
//JACOBIANS
*(buf_warped_x+idx) = normInterp[0];
*(buf_warped_y+idx) = normInterp[1];
*(buf_warped_z+idx) = normInterp[2];
const Eigen::Vector3f tmp = Wxp.cross(normInterp.head<3>());
*(buf_warped_dx+idx) = tmp[0];
*(buf_warped_dy+idx) = tmp[1];
*(buf_warped_elem6+idx) = tmp[2];
*(buf_warped_residual+idx) = residual;
//true if it is an ICP term, false if it is an edge term
*(buf_warped_residual_depth+idx) = true;
In calculateWarpUpdate you have to add the ICP term. Note that you already computed the Jacobians when filling the buffers!
v[0] = px; v[1] = py; v[2] = pz;
v[3] = gx; v[4] = gy; v[5] = *(buf_warped_elem6+i);
thank you for your reply,I will take a try.
please tell me where did it? thanks