hello,I don't find the computation of point-to-plane error Eicp from the code?

[laoshiweiyu]

please tell me where did it? thanks

[fabianschenk]

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);

[laoshiweiyu]

thank you for your reply,I will take a try.