icp_refined branch

[meiqua]

ICP module works well but still has some drawbacks. We improve it from those aspects.

Add kdtree to build faster

        // construct scene

        // buffer idx to query
//        Scene_edge scene;
//        vector<::Vec2f> pcd_buffer, normal_buffer;
//        scene.init_Scene_edge_cpu(detector.dx_, detector.dy_, pcd_buffer, normal_buffer);

        // use kdtree to query, expected to be faster
        Scene_kdtree scene;
        KDTree_cpu kdtree;
        scene.init_Scene_kdtree_cpu(detector.dx_, detector.dy_, kdtree);

Reuse dx dy of detector rather than recompute

// scene.init_Scene_edge_cpu(detector.dx_, detector.dy_, pcd_buffer, normal_buffer);
scene.init_Scene_kdtree_cpu(detector.dx_, detector.dy_, kdtree);

Move canny parameters to function for quick adjustments

    void init_Scene_kdtree_cpu(cv::Mat dx, cv::Mat dy, KDTree_cpu& kdtree, float max_dist_diff = 4.0f,
                             float low_thresh = 30, float high_thresh = 60);

merge subpixel / sim3 branch, so easy to switch as you like

            // subpixel
//            cuda_icp::RegistrationResult result = cuda_icp::ICP2D_Point2Plane_cpu(model_pcd, scene);

            // subpixel, also refine scale
            cuda_icp::RegistrationResult result = cuda_icp::sim3::ICP2D_Point2Plane_cpu(model_pcd, scene);

fusion branch has been added a new node to produce dx/dy, just set one bool value

        Timer timer;
        detector.set_produce_dxy = true; // produce dxy, for icp purpose maybe
        std::vector<line2Dup::Match> matches = detector.match(img, 90, ids);
        timer.out("match total time");

        // test dx, dy;
        cv::Mat canny_edge;
        cv::Canny(detector.dx_, detector.dy_, canny_edge, 30, 60);
        cv::imshow("canny edge", canny_edge);
        // cv::waitKey();

See icp_refined branch for details.

[DennisLiu-elogic]

Hi mequa，

關於icp_refine branch與fusion-byhand branch中，dx_ dy_的寫法不一樣

若要混用的話，是不是icp_refine branch中跟dx_ dy_相關的部分直接刪除就好了呢?

[meiqua]

是的，fusion branch是fusion的时候直接copy出来的