text:
#include "poseg2.h"
#include "features.h"
#include "math_modules.h"
#include "utilities.h"
#include <Eigen/src/Core/Matrix.h>
#include <Open3D/Geometry/PointCloud.h>
#include <Open3D/Visualization/Utility/DrawGeometry.h>
#include <boost/core/use_default.hpp>
#include <cstdint>
#include <functional>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/types.hpp>
#include <stdexcept>
#include <string>
#include <sys/types.h>
#include <utility>
#include <vector>

/**
 *  TODO:
 *      - cache landmark descriptors, update only when added to landmark
 *      - compute pointcloud scale as soon as possible
 *          --> can use metric units
 */

namespace stitcher3d
{
    PoseGraph::PoseGraph() :
        cameras(std::vector<Camera*>()),
        // f_matcher(std::make_unique<cv::BFMatcher>(cv::BFMatcher(cv::NORM_HAMMING)))
        // NORM_HAMMING for ORB, AKAZE. NORM_L2 for SIFT
        f_matcher_crosscheck(std::make_unique<cv::BFMatcher>(*cv::BFMatcher::create(cv::NORM_L2, true))),
        f_matcher(std::make_unique<cv::BFMatcher>(*cv::BFMatcher::create(cv::NORM_L2, false)))
    { }

    PoseGraph::~PoseGraph()
    {
        for (Camera* cam : cameras)
            delete cam;
    }

    Camera* PoseGraph::add_camera(const Eigen::Vector3d& position, const Eigen::Matrix3d& rotation,
        const std::shared_ptr<o3d::geometry::Image>& rgb, const std::shared_ptr<o3d::geometry::Image>& depth,
        const o3d::camera::PinholeCameraIntrinsic& intr, const std::vector<double>& distortion_coeffs,
        const std::string rgb_name, const std::string depth_name, const bool offline)
    {
        // const auto [cv_keypoints, descriptor] = features::detect_akaze_features(rgb, &intr, &distortion_coeffs);
        Camera* tcam;

        auto cv_rgb = nullptr; //std::make_shared<cv::Mat>(rgbd::o3d_image_to_cv_image(rgb));
        auto cv_depth = nullptr; //std::make_shared<cv::Mat>(rgbd::o3d_image_to_cv_image(depth, true));

        // cv::imshow("depth", *cv_depth);
        // cv::waitKey(0);

        if (!offline)
        {
            tcam = new Camera (
                position,
                rotation,
                UNIT_SCALE,
                position,
                rotation,
                Eigen::Matrix4d::Identity(),
                intr.intrinsic_matrix_ * Eigen::Matrix<double, 3, 4>::Identity(),
                distortion_coeffs,
                nullptr,
                std::vector<cv::KeyPoint>(),
                cv::Mat(),
                rgb,
                depth,
                cv_rgb,
                cv_depth,
                intr,
                NOT_SET_CAM_ID,
                0.0f,
                rgb_name,
                depth_name
            );
        }
        else
        {
            throw std::runtime_error("unsupported feature, aborting");
        }

        // dR.transpose(), -dR.transpose() * dt
        // set_camera_T_and_P(tcam, Eigen::Matrix3d::Identity().transpose(), -Eigen::Matrix3d::Identity().transpose() * Eigen::Vector3d::Zero());

        cameras.push_back(tcam);
        cameras.back()->id = cameras.size() - 1;
        return cameras.back();
    }

    void PoseGraph::add_camera_offline(const std::shared_ptr<o3d::geometry::Image>& rgb, const std::shared_ptr<o3d::geometry::Image>& depth,
        const o3d::camera::PinholeCameraIntrinsic& intr, const std::vector<double>& distortion_coeffs,
        const std::string rgb_name, const std::string depth_name)
    {
        Camera* cam = add_camera(Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity(),
            rgb, depth, intr, distortion_coeffs, rgb_name, depth_name, true);

        const auto [cv_keypoints, descriptor] = features::detect_features(rgb, reg_params.max_feature_count, &intr, &distortion_coeffs);
        cam->kp_features = cv_keypoints;
        cam->feature_descriptors = descriptor;
    }

    bool PoseGraph::add_camera_and_compute_pose_realtime(const std::shared_ptr<o3d::geometry::Image>& rgb, const std::shared_ptr<o3d::geometry::Image>& depth,
        const o3d::camera::PinholeCameraIntrinsic& intr, const std::vector<double>& distortion_coeffs,
        const uint32_t time_id, const std::string rgb_name, const std::string depth_name)
    {
        // if (landmarks.size() > 200000)
        //     visualize_tracks();

        Timer t;

        #ifdef DEBUG_VERBOSE
        std::cout << "\n";
        #endif

        Camera* cam = add_camera(Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity(), rgb, depth, intr, distortion_coeffs, rgb_name, depth_name);

        const auto [cv_keypoints, descriptor] = features::detect_features(rgb, reg_params.max_feature_count, &intr, &distortion_coeffs);
        cam->kp_features = cv_keypoints;
        cam->feature_descriptors = descriptor;

        #ifdef DEBUG_VERBOSE
        t.stop("feature detection");
        #endif

        if (cameras.size() == 2)
        {
            #ifdef DEBUG_VERBOSE
        	std::cout << "less than 3 cameras, using essential matrix\n";
            #endif

        	const bool status = compute_initial_pose(cam, cameras.at(0));

            #ifdef DEBUG_VERBOSE
        	t.stop("computing initial pose");
            #endif

            if (status)
                pose_predictor.add_good_pose(time_id, cameras.back()->position, cameras.back()->rotation);

        	return status;
        }
        else if (cameras.size() == 1)
        {
            #ifdef DEBUG_VERBOSE
            std::cout << "only one camera, returning\n";
            #endif

            pose_predictor.add_good_pose(time_id, cam->position, cam->rotation);
            return true;
        }
        // visualize_tracks();

        const bool status = compute_pose_realtime(cam, time_id);

        #ifdef DEBUG_VERBOSE
        t.stop("computing the entire pose");
        #endif

        if (!status)
        {
            #ifdef DEBUG_VERBOSE
            std::cout << "adding camera failed, removing\n";
            #endif

            remove_last_camera(cameras.back());
        }

        return status;
    }

    void PoseGraph::compute_camera_poses_accurate()
    {
        if (cameras.size() < 2)
            throw std::runtime_error("not enough cameras to compute poses, aborting");

        Timer t;

        // pair-wise match all cameras
        for (int ii = 0; ii < cameras.size(); ii++)
        {
            OfflineCamera* refcam = dynamic_cast<OfflineCamera*>(cameras.at(ii));

            for (int jj = ii + 1; jj < cameras.size(); jj++)
            {
                OfflineCamera* cam = dynamic_cast<OfflineCamera*>(cameras.at(jj));

                const std::vector<std::pair<uint32_t, uint32_t>> rc_corr =
                    match_features_hgraphy(refcam->feature_descriptors, cam->feature_descriptors, refcam, cam);

                    // DEBUG_visualize_matches(refcam, cam, rc_corr);

                // loop through matches and add to Camera match-map for each feature index (fi)
                for (int fi = 0; fi < rc_corr.size(); fi++)
                {
                    // add feature chains
                    refcam->kp_match_index_map[rc_corr[fi].first][jj] = rc_corr[fi].second;
                    cam->kp_match_index_map[rc_corr[fi].second][ii] = rc_corr[fi].first;
                }
            }
        }

        #ifdef DEBUG_VERBOSE
        t.stop("pair-wise correspondences and feature matches");
        #endif

        /**
         *  TODO:
         *      - find corresponding candidates in each image, pair-wise -> map
         *      - loop through all cameras, for each: loop through all matched frames
         *      - after local pose recovery, triangulate points
         *      - scale points
         *      - global pose recovery
         *      - triangulate good points
         *      - average landmarks
         *          - fix camera poses with PnP?
         *
         *  ALGORITHM:
         *      - pairwise match features and add to corresponding frame lookups
         *      - fix 0th camera as origin
         *      - iterate cameras:
         *          - recover pose
         *          - when featurecount < threshold (0.5?), stop and set as unit transformation
         *          - this is makes a fragment, a camera sequence
         *      - refcam +1 and repeat
         *      - match fragments
         *          - 2nd of first, 1st of second are the same frame
         *          - set 1st of 2nd transformation
         *          - propagate transformations (relative to 1st of 2nd)
         */

        std::vector<FrameFragment> frame_fragments;

        cv::Mat intr_cv;
        cv::eigen2cv(cameras.at(0)->intr.intrinsic_matrix_, intr_cv);

        /**
         *  ii is the fragment's 1st camera
         *  jj is the fragment's last camera
         */
        for (int ii = 0; ii < cameras.size() - 1; ii++)
        {
            OfflineCamera* refcam = dynamic_cast<OfflineCamera*>(cameras.at(ii));

            // set the refcam to be at origin, dealing with relative fragments
            set_camera_T_and_P(refcam, Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero());

            const uint32_t next_cam_id = refcam->id + 1;

            // <feature index, indexes in consequent frames>
            // feature must be present in all of them, otherwise delete
            std::map<uint32_t, std::vector<uint32_t>> feature_tracks;

            // for (int i = 0; i < refcam->kp_match_index_map.size(); i++)

            // construct feature trails

            // populate reference feature id lookup
            for (auto a : refcam->kp_match_index_map)
            {
                if (a.second.count(next_cam_id) != 0)
                {
                    feature_tracks[a.first] = { a.second.at(next_cam_id) };
                }
            }

            uint32_t next_cam_loop_id = next_cam_id;
            std::map<uint32_t, std::map<uint32_t, uint32_t>>& frame_kp_matches = dynamic_cast<OfflineCamera*>(cameras.at(next_cam_loop_id))->kp_match_index_map;
            uint32_t current_match_count = feature_tracks.size();
            const uint32_t initial_match_count = feature_tracks.size();

            while ((float)current_match_count > (float)initial_match_count * MATCH_RATIO_TRAIL_THRESHOLD )
                // || current_match_count > MATCH_COUNT_TRAIL_THRESHOLD)
            {
                std::vector<uint32_t> to_remove_tracks;

                for (auto& track : feature_tracks)
                {
                    const uint32_t ref_f_id = track.first;
                    const uint32_t fid = track.second.back();

                    // check if feature f is found also in the next_cam's matches
                    if (frame_kp_matches.at(fid).count(next_cam_loop_id + 1) != 0)
                    {
                        track.second.push_back(frame_kp_matches[fid][next_cam_loop_id + 1]);
                    }
                    // feature doesn't exist in next, mark for delete
                    else
                        to_remove_tracks.push_back(ref_f_id);
                }

                // remove tracks which weren't long enough
                for (uint32_t to_remove_id : to_remove_tracks)
                    feature_tracks.erase(to_remove_id);

                current_match_count = feature_tracks.size();

                // NOTE: throw away the last frame, should be fixed at some point
                // if (next_cam_loop_id >= cameras.size() - 1)
                //     break;

                next_cam_loop_id++;
                frame_kp_matches = dynamic_cast<OfflineCamera*>(cameras.at(next_cam_loop_id))->kp_match_index_map;

                // std::cout << "next camera: " << next_cam_loop_id << ", track size: "
                //     << current_match_count << ", out of " << initial_match_count << "\n";
            }

            std::cout << "feature trail feature count: " << feature_tracks.size() <<
                " and length: " << (feature_tracks.size() > 0 ? feature_tracks.begin()->second.size() : 0) << "\n";

            // for (auto a : feature_tracks)
            // {
            //     for (auto b : a.second)
            //         std::cout << b << " ";
            //     std::cout << "\n";
            // }

            // recover relative poses between frames
            // for (int jj = ii + 1; jj < cameras.size(); jj++)

            OfflineCamera* cam = dynamic_cast<OfflineCamera*>(cameras.at(ii + feature_tracks.begin()->second.size()));

            const std::vector<uint32_t> refcam_track_features = utilities::get_keys_as_vec(feature_tracks);
            const std::vector<uint32_t> cam_track_features = utilities::get_column_values_at_as_vec(feature_tracks.begin()->second.size() - 1, feature_tracks);

            /**
             *  - get 2d cv points
             *  - recover poses for first and last
             *  - triangulate
             *  - PnP for mid frames
             */

            std::vector<cv::Point2f> ref_points, cam_points;
            ref_points.reserve(refcam_track_features.size());
            cam_points.reserve(cam_track_features.size());

            std::vector<std::pair<uint32_t, uint32_t>> fmatches;

            for (int i = 0; i < refcam_track_features.size(); i++)
            {
                const cv::Point2f ref_pt = refcam->kp_features[refcam_track_features[i]].pt;
                const cv::Point2f cam_pt = cam->kp_features[cam_track_features[i]].pt;

                fmatches.push_back(std::make_pair(refcam_track_features[i], cam_track_features[i]));

                ref_points.push_back(ref_pt);
                cam_points.push_back(cam_pt);

                // std::cout << ref_pt << " : " << cam_pt << "\n";
            }

            cv::Mat good_feature_mask;

            const cv::Mat essential = cv::findEssentialMat(ref_points, cam_points, intr_cv, cv::RANSAC, 0.999, 1.0, good_feature_mask);
            cv::Mat R_local, t_local;
            cv::recoverPose(essential, ref_points, cam_points, intr_cv, R_local, t_local, good_feature_mask);

            Eigen::Matrix3d dR;
            Eigen::Vector3d dt;
            cv::cv2eigen(R_local, dR);
            cv::cv2eigen(t_local, dt);

            cam->dR = dR;
            cam->dt = dt;

            // DEBUG_visualize_matches(refcam, cam, fmatches);

            set_camera_T_and_P(cam, dR.transpose(), -dR.transpose() * dt);
            set_camera_T_and_P(cam, refcam->T * cam->T);

            // std::cout << cam->rotation << "\n\n" << cam->position.transpose() << "\n\n";

            cv::Mat refcam_P, cam_P;
            cv::eigen2cv(refcam->P, refcam_P);
            cv::eigen2cv(cam->P, cam_P);

            cv::Mat p4d;
            cv::triangulatePoints(refcam_P, cam_P, ref_points, cam_points, p4d);

            std::vector<Eigen::Vector3d> points3d;

            std::vector<cv::Point3f> new_points, existing_points;

            // don't scale the points if no landmarks are found
            if (landmarks.size() != 0)
            {
                for (int jj = 0; jj < ref_points.size(); jj++)
                {
                    const cv::Point3f p3d (
                        p4d.at<float>(0, jj) / p4d.at<float>(3, jj),
                        p4d.at<float>(1, jj) / p4d.at<float>(3, jj),
                        p4d.at<float>(2, jj) / p4d.at<float>(3, jj));

                    if (good_feature_mask.at<uint32_t>(jj))
                    {
                        // points3d.push_back(Eigen::Vector3d(p3d.x, p3d.y, p3d.z));

                        // landmark

                        const uint32_t ref_f_id = refcam_track_features[jj];
                        // const uint32_t cam_f_id = cam_track_features[jj];

                        // check if landmark exists -> add to / create new
                        if (refcam->landmark_kp_exists(ref_f_id) && refcam->match_exists(ref_f_id, ii))
                        {
                            const uint32_t lm_id = refcam->landmark_lookup[ref_f_id];
                            const cv::Point3f avg_3d = landmarks[lm_id].sum_point / (float)(landmarks[lm_id].view_data.size() - 1);

                            // save the points for scale calculations
                            new_points.push_back(p3d);
                            existing_points.push_back(avg_3d);
                        }
                    }
                }

                // calculate the scale

                double scale = 0.0;
                uint32_t count = 0;

                for (uint32_t jj = 0; jj < new_points.size() - 1; jj++)
                {
                    for (uint32_t kk = jj + 1; kk < new_points.size(); kk++)
                    {
                        const double denominator = norm(new_points[jj] - new_points[kk]);
                        if (math::is_close(denominator, 0.f))
                            continue;

                        const double s = norm(existing_points[jj] - existing_points[kk]) / denominator;

                        scale += s;
                        count++;
                    }
                }

                assert(count);

                scale /= (double)count;
                std::cout << "for image " << ii << " final scale is " << scale << "\n";

                dt *= scale;

                cam->dR = dR;
                cam->dt = dt;

                // DEBUG_visualize_matches(refcam, cam, fmatches);

                set_camera_T_and_P(cam, dR.transpose(), -dR.transpose() * dt);
                set_camera_T_and_P(cam, refcam->T * cam->T);
                cv::eigen2cv(cam->P, cam_P);

                // re-triangulate with new projection matrix
                cv::triangulatePoints(refcam_P, cam_P, ref_points, cam_points, p4d);
            }

            std::vector<cv::Point3f> tr_3d_points;

            for (int jj = 0; jj < ref_points.size(); jj++)
            {
                const cv::Point3f p3d (
                    p4d.at<float>(0, jj) / p4d.at<float>(3, jj),
                    p4d.at<float>(1, jj) / p4d.at<float>(3, jj),
                    p4d.at<float>(2, jj) / p4d.at<float>(3, jj));

                // add to or create new landmarks
                if (good_feature_mask.at<uint32_t>(jj))
                {
                    points3d.push_back(Eigen::Vector3d(p3d.x, p3d.y, p3d.z));
                    tr_3d_points.push_back(p3d);

                    const uint32_t ref_f_id = refcam_track_features[jj];
                    const uint32_t cam_f_id = cam_track_features[jj];

                    // landmark exists, add to it
                    if (refcam->landmark_kp_exists(ref_f_id))
                    {
                        cam->landmark_lookup[cam_f_id] = refcam->landmark_lookup[ref_f_id];

                        Landmark& lm = landmarks[cam->landmark_lookup[cam_f_id]];
                        lm.add_view_simple(cam->id, cam_f_id, p3d);
                    }
                    // landmark doesn't exist, create new
                    else
                    {
                        Landmark lm;
                        lm.add_view_simple(refcam->id, ref_f_id, p3d);
                        lm.add_view_simple(cam->id, cam_f_id, p3d);

                        landmarks.emplace_back(lm);

                        refcam->landmark_lookup[ref_f_id] = landmarks.size() - 1;
                        cam->landmark_lookup[cam_f_id] = landmarks.size() - 1;
                    }
                }
            }

            std::shared_ptr<o3d::geometry::TriangleMesh> camera_mesh = std::make_shared<o3d::geometry::TriangleMesh>(o3d::geometry::TriangleMesh());
            o3d::io::ReadTriangleMeshFromOBJ(reg_params.assets_path + DEBUG_CAMERA_PATH, *camera_mesh, false);
            camera_mesh->Transform(cam->T);

            auto cloud = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(points3d));
            o3d::visualization::DrawGeometries({ cloud, camera_mesh });

            // create frame fragment, lambdas ftw
            frame_fragments.push_back(FrameFragment {
                [&]{ std::vector<Camera*> cams; for (int i = ii; i < ii + feature_tracks.begin()->second.size(); i++) cams.push_back(cameras[i]); return cams; } (),
                feature_tracks,
                [&] { std::vector<Eigen::Matrix4d> ts; for (int i = ii; i < ii + feature_tracks.begin()->second.size(); i++) ts.push_back(cameras[i]->T); return ts; }(),
                tr_3d_points
            });
        }

        // PnP for first fragment
        FrameFragment& initial = frame_fragments[0];
        for (int ii = 1; ii < initial.frag_cameras.size() - 2; ii++)
        {
            // take 2d feature points, PnP to initial.tr_3d_points
            const auto f_ids = utilities::get_column_values_at_as_vec(ii, initial.frag_feature_tracks);

            Camera* cam = initial.frag_cameras[ii];

            std::vector<cv::Point2f> view_features_points;

            for (int jj = 0; jj < f_ids.size(); jj++)
                view_features_points.push_back(cam->kp_features[f_ids[jj]].pt);

            const cv::Mat cv_intr = [&]{
                cv::Mat intr;
                cv::eigen2cv(cam->intr.intrinsic_matrix_, intr);
                return intr;
            }();

            cv::Mat rcv, tcv;
            cv::solvePnPRansac(initial.feature_3d_points, view_features_points, cv_intr, cam->distortion_coeffs, rcv, tcv, true, 1000, 4.0, 0.987);

            Eigen::Matrix4d R;
            Eigen::Vector3d t;

            cv::cv2eigen(rcv, R);
            cv::cv2eigen(tcv, t);

            // NOTE: this doesn't work anymore!
            assert(false);
            // set_camera_T_and_P(cam, R.transpose(), -R.transpose() * t);
        }

        // match fragments and propagate transformations

        // 0th fragment is origin, skip it
        for (int ii = 1; ii < frame_fragments.size(); ii++)
        {
            // take the 1st of the previous fragment, and the 0th of the ii fragment
            // and use PnP to match them
        }

        // average landmarks

        // PnP to average landmarks

        /**
         *  e.g.
         *
         *   0-4
         *   1-5
         *   2-7
         *   3-8
         *
         *  - edges with essential
         *  - intraframes with PnP
         *  - match fragments 0:1 -> 1:0
         *
         *  - lastly, PnP to averaged landmarks
         */

        for (Landmark& lm : landmarks)
        {
            if (lm.view_data.size() >= 3)
                lm.point3d = lm.sum_point / float(lm.view_data.size() - 1);
        }

        #ifdef DEBUG_VERBOSE
        t.stop("camera motion recovery");
        #endif
    }

    bool PoseGraph::compute_initial_pose(Camera* cam, const Camera* ref_cam)
    {
    	/**
		 * 	- first 2 frames special case
    	 * 		* essential matrix, same as before
    	 * 		* reject 2nd if over 50% features not over threshold
    	 *
    	 * 	TODO: more early termination reasons?
         *
         *  TODO: currently only-manual initial checking,
         *          can essential matrix be used?
         *
         *  TODO: add pointcloud registration for the initial cloud
    	 */

    	// const auto rc_corr = correspondence_between_cameras(refcam, cam);
    	// std::pair<std::vector<uint32_t>, std::vector<uint32_t>> rc_corr =
        std::vector<std::pair<uint32_t, uint32_t>> rc_corr =
    		match_features_hgraphy(ref_cam->feature_descriptors, cam->feature_descriptors, ref_cam, cam);
            // match_features_bf_knn(ref_cam->feature_descriptors, cam->feature_descriptors);

        // DEBUG_visualize_feature_matches_custom(ref_cam, cam, rc_corr);

        /*
        {
            std::vector<cv::KeyPoint> inliers1, inliers2;
            std::vector<uint32_t> inliers1_i, inliers2_i;
            std::vector<cv::DMatch> good_matches;

            for (int i = 0; i < rc_corr.size(); i++)
            {
                inliers1.emplace_back(ref_cam->kp_features[rc_corr[i].first]);
                inliers2.emplace_back(cam->kp_features[rc_corr[i].second]);

                good_matches.emplace_back(cv::DMatch(i, i, 0));
            }

            cv::Mat matchimg;
            cv::drawMatches(features::o3d_image_to_cv_image(ref_cam->rgb), inliers1, features::o3d_image_to_cv_image(cam->rgb), inliers2, good_matches, matchimg);
            cv::imshow("img", matchimg);
            cv::waitKey(0);
        }
        */

    	if (rc_corr.size() < MIN_FEATURE_COUNT)
        {
            #ifdef DEBUG_VERBOSE
            std::cout << "not enough matches: " << rc_corr.size() << ", aborting!\n";
            #endif

            remove_last_camera(cam);
    		return false;
        }

    	relative_pose_for_camera(ref_cam, cam, rc_corr);

        // refine_pose_with_ICP(cam, ref_cam);
    	landmarks_from_feature_matches(rc_corr, ref_cam, cam);

        const std::vector<uint32_t> features_negative =
            create_match_negative(rc_corr, cam->kp_features.size());

        // camera_candidates.insert(std::make_pair(cam->id, features_negative));
        candidates_from_unmatched(features_negative, cam);

    	return true;
    }

    void PoseGraph::refine_pose_with_ICP(Camera* cam, const Camera* ref_cam)
    {
        // this method doesn't work correctly!
        assert(false);

        auto cam_cloud = rgbd::create_pcloud_from_rgbd(*cam->rgb, *cam->depth, DEFAULT_DEPTH_SCALE, DEFAULT_CLOUD_FAR_CLIP, false, cam->intr);
        // cam_cloud->Scale(pointcloud_scale, false);
        cam_cloud->Transform(cam->T);

        auto ref_cam_cloud = rgbd::create_pcloud_from_rgbd(*ref_cam->rgb, *ref_cam->depth, DEFAULT_DEPTH_SCALE, DEFAULT_CLOUD_FAR_CLIP, false, ref_cam->intr);
        // ref_cam_cloud->Scale(pointcloud_scale, false);
        ref_cam_cloud->Transform(ref_cam->T);

        // Colored ICP refinement
        const float voxel_size = ICP_VOXEL_SIZE;
        const float radius = ICP_NORMAL_RADIUS;

        const float distance_threshold = voxel_size * 0.4f;
        const Eigen::Matrix4d initial_transform = cam->T;

        auto cam_cloud_downsampled = cam_cloud->VoxelDownSample(voxel_size);
        auto ref_cam_cloud_downsampled = ref_cam_cloud->VoxelDownSample(voxel_size);

        cam_cloud_downsampled->EstimateNormals(o3d::geometry::KDTreeSearchParamHybrid(radius, 30));
        ref_cam_cloud_downsampled->EstimateNormals(o3d::geometry::KDTreeSearchParamHybrid(radius, 30));

        // o3d::visualization::DrawGeometries({cam_cloud_downsampled, ref_cam_cloud_downsampled});

        auto result = o3d::registration::RegistrationICP(
            *cam_cloud_downsampled, *ref_cam_cloud_downsampled, distance_threshold, initial_transform,
            o3d::registration::TransformationEstimationPointToPoint()); //,
            // o3d::registration::ICPConvergenceCriteria(1e-6, 1e-6, ICP_ITER_COUNT));

        if (result.fitness_ >= ICP_FITNESS_THRESHOLD)
        {
            set_camera_T_and_P(cam, result.transformation_);
            std::cout << "camera transform differences: \n" << result.transformation_ << "\n\n" << initial_transform << "\n\n" << result.fitness_ << "\n\n";
        }
        else
        {
            // cam_cloud_downsampled->Transform(result.transformation_);
            // o3d::visualization::DrawGeometries({cam_cloud_downsampled, ref_cam_cloud_downsampled});
            std::cout << "bad fitness: " << result.fitness_ << "\n";
        }
    }

    std::vector<uint32_t> PoseGraph::create_match_negative(
        const std::vector<std::pair<uint32_t, uint32_t>> matched_features,
        const uint32_t all_features_count) const
    {
        std::vector<uint32_t> feature_negative;
        feature_negative.reserve(all_features_count - matched_features.size());

        std::vector<uint32_t> matched;
        matched.reserve(matched_features.size());

        for (int i = 0; i < matched_features.size(); i++)
        {
            matched.emplace_back(matched_features[i].second);
        }
        // std::sort(matched.begin(), matched.end());

        std::vector<uint32_t> all_features;
        all_features.reserve(all_features_count);
        for (uint32_t i = 0; i < all_features_count; i++)
        {
            all_features.emplace_back(i);
        }

        // // https://stackoverflow.com/questions/15758680/get-all-vector-elements-that-dont-belong-to-another-vector
        // std::remove_copy_if(all_features.begin(), all_features.end(), std::back_inserter(feature_negative),
        //     [&matched](const uint32_t& arg)
        //     { return (std::find(matched.begin(), matched.end(), arg) != matched.end());});

        // std::cout << "all features: " << all_features_count << ", fnegative: " << feature_negative.size() << ", matched. " << matched_features.size() << "\n";

        return set_negative_intersection(matched, all_features);
    }

    void PoseGraph::candidates_from_unmatched(const std::vector<uint32_t> unmatched_features, const Camera* cam)
    {
        const uint32_t cam_id = cam->id;
        const std::vector<cv::KeyPoint> keypoints = cam->kp_features;
        const cv::Mat descriptors = cam->feature_descriptors;
        cv::Mat cam_P;
        cv::eigen2cv(cam->P, cam_P);

        for (int i = 0; i < unmatched_features.size(); i++)
        {
            Landmark lm;
            lm.add_as_candidate(
                cam_id,
                unmatched_features[i],
                keypoints[unmatched_features[i]],
                descriptors.row(unmatched_features[i]),
                cam_P
            );

            candidates.push_back(lm);
        }
    }

    std::vector<cv::Point3f> PoseGraph::triangulate_matches(
        const std::vector<std::pair<uint32_t, uint32_t>> matches,
        const std::vector<uint32_t> first_cams, const Camera* second_cam) const
    {
        std::vector<cv::Point3f> triangulated;
        triangulated.reserve(matches.size());

        const std::vector<cv::KeyPoint> second_kps = second_cam->kp_features;
        const cv::Mat second_cam_P = [&]
        {
            cv::Mat p;
            cv::eigen2cv(second_cam->P, p);
            return p;
        }();

        for (int i = 0; i < matches.size(); i++)
        {
            const std::vector<cv::Point2f> x1 { cameras[first_cams[i]]->kp_features[matches[i].first].pt };
            const std::vector<cv::Point2f> x2 { second_kps[matches[i].second].pt };

            cv::Mat first_cam_P;
            cv::eigen2cv(cameras[first_cams[i]]->P, first_cam_P);

            cv::Mat p4d;
            cv::triangulatePoints(first_cam_P, second_cam_P, x1, x2, p4d);

            triangulated.emplace_back(cv::Point3f(
                p4d.at<float>(0, 0) / p4d.at<float>(3, 0),
                p4d.at<float>(1, 0) / p4d.at<float>(3, 0),
                p4d.at<float>(2, 0) / p4d.at<float>(3, 0)
            ));
        }

        // debug visualization stuff
        // std::vector<Eigen::Vector3d> points;
        // for (int i = 0; i < triangulated.size(); i++)
        //     points.emplace_back(Eigen::Vector3d(triangulated[i].x, triangulated[i].y, triangulated[i].z));

        // auto pcloud = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(points));
        // const auto a = o3d::geometry::TriangleMesh::CreateCoordinateFrame();
        // o3d::visualization::DrawGeometries( { pcloud, a } );
        // std::exit(0);

        return triangulated;
    }

    void PoseGraph::landmarks_from_feature_matches(
        const std::vector<std::pair<uint32_t, uint32_t>> matches,
        const Camera* first_cam, const Camera* second_cam)
    {
        /**
         *  triangulate matches and create new landmarks
         */

        // std::vector<cv::DMatch> good_matches;
        // std::vector<cv::KeyPoint> inliers1, inliers2;
        // for (int i = 0; i < matches.size(); i++)
        // {
        //     inliers1.emplace_back(first_cam->kp_features[matches[i].first]);
        //     inliers2.emplace_back(second_cam->kp_features[matches[i].second]);

        //     good_matches.emplace_back(cv::DMatch(i, i, 0));
        // }

        // cv::Mat matchimg;
        // cv::drawMatches(features::o3d_image_to_cv_image(first_cam->rgb), inliers1, features::o3d_image_to_cv_image(second_cam->rgb), inliers2, good_matches, matchimg);
        // cv::imshow("img", matchimg);
        // cv::waitKey(0);

        // std::vector<Eigen::Vector3d> trpoints;

        landmarks.reserve(matches.size());

        // extend the vector to be of the same size
        const std::vector<uint32_t> first_cam_extended (matches.size(), (uint32_t)first_cam->id);
        const std::vector<cv::Point3f> triangulated = triangulate_matches(matches, first_cam_extended, second_cam);

        cv::Mat fcam_P, scam_P;
        cv::eigen2cv(first_cam->P, fcam_P);
        cv::eigen2cv(second_cam->P, scam_P);

        const std::vector<uint32_t> valid_points = filter_points_behind_camera(second_cam, triangulated);

        // create new landmarks
        for (int j = 0; j < valid_points.size(); j++)
        {
            const uint32_t i = valid_points[j];

            const cv::Point3f p3d = triangulated[i];

            const uint32_t ref_feature_id = matches[i].first;
            const uint32_t feature_id = matches[i].second;

            const uint32_t ref_cam_id = first_cam->id;
            const uint32_t cam_id = second_cam->id;

            const cv::KeyPoint ref_keypoint = first_cam->kp_features[ref_feature_id];
            const cv::KeyPoint keypoint = second_cam->kp_features[feature_id];

            const cv::Mat ref_descriptor = first_cam->feature_descriptors.row(ref_feature_id);
            const cv::Mat descriptor = second_cam->feature_descriptors.row(feature_id);

            Landmark lm;

            lm.add_view(ref_cam_id, ref_feature_id, ref_keypoint, ref_descriptor, fcam_P);
            lm.add_view(cam_id, feature_id, keypoint, descriptor, scam_P);
            lm.set_3d_point(p3d);
            lm.triangulation_view_pair = std::make_pair(0, 1);

            landmarks.emplace_back(lm);

            // trpoints.emplace_back(Eigen::Vector3d(p3d.x, p3d.y, p3d.z));
        }

        // auto cloud = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(trpoints));
        // o3d::visualization::DrawGeometries({ cloud });
    }

    std::vector<uint32_t> PoseGraph::filter_points_behind_camera(const Camera* cam, const std::vector<cv::Point3f> points3d) const
    {
        std::vector<uint32_t> valid_points;
        valid_points.reserve(points3d.size());

        // depth test
        // return (R*X)(2) + t(2);

        for (int i = 0; i < points3d.size(); i++)
        {
            const cv::Point3f pcv = points3d[i];
            const Eigen::Vector3d p = Eigen::Vector3d(pcv.x, pcv.y, pcv.z);
            const Eigen::Vector3d pp = (cam->rotation * p) + cam->position;

            // if point has the depth of < 0 or is distance outlier
            if (pp(2) <= 0 || pp(2) > DEPTH_DISTANCE_OUTLIER)
                continue;

            valid_points.push_back(i);
        }

        #ifdef DEBUG_VERBOSE
        std::cout << "valid points: " << valid_points.size() << "; original points: " << points3d.size() << "\n";
        #endif

        return valid_points;
    }

    std::tuple<Eigen::Matrix3d, Eigen::Vector3d> PoseGraph::relative_pose_for_camera(const Camera* ref_cam, Camera* cam,
        const std::vector<std::pair<uint32_t, uint32_t>> feature_matches)
    {
        /**
         *  NOTE: for some reason x1 and x2 are flipped, i.e. feature_matches
         *      order is inverted.
         */

        std::vector<cv::Point2f> x1, x2;
        x1.reserve(feature_matches.size());
        x2.reserve(feature_matches.size());

        for (int i = 0; i < feature_matches.size(); i++)
        {
            const cv::KeyPoint& kp0 = ref_cam->kp_features.at(feature_matches[i].first);
            const cv::KeyPoint& kp1 = cam->kp_features.at(feature_matches[i].second);

            x1.emplace_back(kp0.pt);
            x2.emplace_back(kp1.pt);
        }

        cv::Mat mask;
        cv::Mat cv_intr;
        cv::eigen2cv(cam->intr.intrinsic_matrix_, cv_intr);

        cv::Mat E = cv::findEssentialMat(x1, x2, cv_intr, cv::RANSAC, 0.999, 0.11, mask);

        cv::Mat local_R, local_t;
        cv::recoverPose(E, x1, x2, cv_intr, local_R, local_t, mask);

        Eigen::Matrix3d dR;
        Eigen::Vector3d dt;
        cv::cv2eigen(local_R, dR);
        cv::cv2eigen(local_t, dt);

        // dt *= 0.31505707;

        cam->dR = dR;
        cam->dt = dt;

        set_camera_T_and_P(cam, dR.transpose(), -dR.transpose() * dt);

        #ifdef DEBUG_VERBOSE
        std::cout << "\033[1;31m";

        std::cout << "relative dR and dt for camera: " << ref_cam->id << "->" << cam->id << "\n";
        std::cout << "dt: " << dt.transpose() << "\n";
        #endif

        Eigen::AngleAxisd ax(dR);

        #ifdef DEBUG_VERBOSE
        std::cout << "rodrigues axis: " << ax.axis().transpose() << ", angle: " << ax.angle() * (180.0/3.14159);
        #endif

        cam->rodriques_angle = ax.angle() * (180.0/3.14159);

        #ifdef DEBUG_VERBOSE
        std::cout << "\033[0m\n\n";
        #endif

        return std::make_tuple(cam->dR, cam->dt);
    }

    bool PoseGraph::compute_pose_realtime(Camera* cam, const uint32_t time_id)
    {
        Timer t;

        std::map<std::pair<uint32_t, uint32_t>, cv::Mat> camera_homography_pairs;

        // match features against landmarks, <landmark_id, feature_id>
        std::vector<std::pair<uint32_t, uint32_t>> landmarks_in_view =
            find_landmarks_in_view(cam, camera_homography_pairs);

        #ifdef DEBUG_VERBOSE
        t.stop("find landmarks in view");
        #endif

        #ifdef DEBUG_VISUALIZE
        DEBUG_visualize_landmark_matches_against_last(cam, landmarks_in_view, 1);
        #endif
        // t.stop("draw matches visualization");

        #ifdef DEBUG_VERBOSE
        std::cout << "landmarks in view: " << landmarks_in_view.size() << "\n";
        std::cout << "landmarks: " << landmarks.size() << "\n";
        std::cout << "candidates: " << candidates.size() << "\n";
        #endif

        // create 3d-2d correspondences
        std::vector<cv::Point3f> landmark_points;
        std::vector<cv::Point2f> view_features;
        landmark_points.reserve(landmarks_in_view.size());
        view_features.reserve(landmarks_in_view.size());

        // std::vector<Eigen::Vector3d> lmdebug;

        if (landmarks_in_view.size() < reg_params.landmark_min_count)
        {
            #ifdef DEBUG_VERBOSE
            std::cout << "\033[1;31m";
            std::cout << "not enough landmarks in view: " << landmarks_in_view.size() << ", returning false\n";
            std::cout << "\033[0m\n";
            #endif

            return false;
        }

        for (int i = 0; i < landmarks_in_view.size(); i++)
        {
            landmark_points.emplace_back(landmarks[landmarks_in_view[i].first].point3d);
            view_features.emplace_back(cam->kp_features[landmarks_in_view[i].second].pt);

            // lmdebug.push_back(Eigen::Vector3d(landmark_points.back().x, landmark_points.back().y, landmark_points.back().z));
        }

        // const auto debug = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(lmdebug));
        // o3d::visualization::DrawGeometries({ debug });

        const cv::Mat cv_intr = [&]{
            cv::Mat intr;
            cv::eigen2cv(cam->intr.intrinsic_matrix_, intr);
            return intr;
        }();

        // DEBUG_visualize_landmarks(landmark_points);

        cv::Mat rcv, tcv, rcv_mat;
        std::vector<int> inliers;
        try
        {
            // use last frame as a guess
            const Camera* refcam = cameras.at(cam->id - 1);
            const auto [t_pred, r_pred, confidence] = pose_predictor.predict_next_pose(time_id);
            cv::eigen2cv(r_pred, rcv);
            cv::eigen2cv(t_pred, tcv);
            // these parameters have been figured out by painstakingly trial-and-erroring
            cv::solvePnPRansac(landmark_points, view_features, cv_intr, cam->distortion_coeffs, rcv, tcv, true, 1000, 4.0, 0.987, inliers);
        }
        catch (cv::Exception& e) {
            #ifdef DEBUG_VERBOSE
            std::cout << e.msg << "\n";
            std::cout << "lm points: " << landmark_points.size() << ", view_features: " << view_features.size() << "\n";
            for (int i = 0; i < landmark_points.size(); i++)
            {
                std::cout << landmark_points[i] << ", " << view_features[i] << "\n";
            }
            #endif

            visualize_tracks();
            std::exit(0);
        }

        #ifdef DEBUG_VERBOSE
        t.stop("PnPRansac");
        std::cout << "inliers size: " << inliers.size() << ", original size: " << view_features.size() << "\n";
        #endif

        if (inliers.size() < reg_params.landmark_min_count)
        {
            #ifdef DEBUG_VERBOSE
            std::cout << "\033[1;31m";
            std::cout << "adding camera failed, not enough PnP inliers for camera " << cam->id << "\n";
            std::cout << "\033[0m\n";
            #endif

            return false;
        }

        std::vector<cv::Point3f> inlier_points;
        std::vector<cv::Point2f> inlier_features;
        std::vector<std::pair<uint32_t, uint32_t>> good_lms_in_view;

        inlier_points.reserve(inliers.size());
        inlier_features.reserve(inliers.size());
        good_lms_in_view.reserve(inliers.size());

        for (int i : inliers)
        {
            inlier_points.push_back(landmark_points[i]);
            inlier_features.push_back(view_features[i]);
            good_lms_in_view.push_back(landmarks_in_view[i]);
        }

        // cv::solvePnPRefineLM(inlier_points, inlier_features, cv_intr, {}, rcv, tcv);
        // t.stop("Levenberg-Marquardt refinement");

        // extract and set the camera R and t
        {
            Eigen::Matrix3d R;
            Eigen::Vector3d t;

            cv::Rodrigues(rcv, rcv_mat);
            cv::cv2eigen(rcv_mat, R);
            cv::cv2eigen(tcv, t);

            set_camera_T_and_P(cam, R.transpose(), -R.transpose() * t);

            #ifdef DEBUG_VERBOSE
            std::cout << "\033[1;32m";
            std::cout << "absolute world position for camera PnP: " << cam->id << "\n";
            std::cout << "t: " << cam->position.transpose() << ", t norm: " << cam->position.norm() << "\n";
            #endif

            Eigen::AngleAxisd ax(cam->rotation);

            #ifdef DEBUG_VERBOSE
            std::cout << "rodrigues axis: " << ax.axis().transpose() << ", angle: " << ax.angle() * (180.0/3.14159) << "\n";
            std::cout << "\033[0m\n";
            #endif

            cam->rodriques_angle = ax.angle() * (180.0/3.14159);
        }

        const auto [pose_score, pose_confidence]  = pose_predictor.verify_pose(time_id, cam->position, cam->rotation);

        /**
         *  low score & high confidence --> discard
         *  high score & high confidence --> keep
         *
         *  low score & low confidence --> abort
         *  high score & low confidence --> abort
         *
         *  reject also if pose difference is too great, compared to 1st camera position
         *      --> rejects occasional outliers due to projection error
         */
        if ((pose_score < MIN_POSE_SCORE && pose_confidence >= POSE_CONFIDENCE_THRESHOLD) ||
            (pose_confidence < POSE_CONFIDENCE_THRESHOLD) ||
            (cam->position - cameras.at(cam->id - 1)->position).norm() > cameras.at(1)->position.norm() * CAMERA_POSITION_OUTLIER_MULTIPLIER)
        {
            #ifdef DEBUG_MINIMAL
            std::cout << "\033[1;31m";
            std::cout << "adding camera failed. pose score: " << pose_score << ", pose confidence: " << pose_confidence << "\n";
            std::cout << "\033[0m\n";
            #endif

            return false;
        }

        pose_predictor.add_good_pose(time_id, cam->position, cam->rotation);

        // append landmark-feature matches to landmarks
        add_track_to_landmarks(cam, landmarks_in_view, landmark_points, inliers);

        #ifdef DEBUG_VERBOSE
        t.stop("add track to landmarks");
        #endif

        // retriangulate_landmarks(good_lms_in_view);
        // t.stop("retriangulate landmarks");

        /**
         *  NOTE:
         *      track current candidates
         *          - find unmatched non-lm features
         *          - match against candidates
         *              - add matched to candidates
         *          - remove old candidates (no track for N frames), if cannot be triangulated
         *          - triangulate succesful candidates and add to lms
         *          - add all the rest unmatched features to candidates
         */

        const std::vector<uint32_t> unmatched =
            create_match_negative(landmarks_in_view, cam->kp_features.size());

        #ifdef DEBUG_VERBOSE
        t.stop("create match negative");
        #endif

        /**
         *  TODO:
         *      optimize:
         *      - match_features_against_candidates                 (234 ms)
         *          - candidate match_features_bf since last took   (180 ms)
         *      - handle_feature_candidate_matches                  (264 ms)
         *          - to_landmark
         */

        // std::vector<uint32_t>* unmatched_features = new std::vector<uint32_t>();
        // <candidate id, current cam feature id>
        const std::vector<std::pair<uint32_t, uint32_t>> feature_candidates =
            match_features_against_candidates(cam, unmatched, camera_homography_pairs);

        #ifdef DEBUG_VERBOSE
        t.stop("match features against candidates");
        #endif

        /**
         *  TODO:
         *      track candidates until no match
         *          --> create landmark if enough views
         *          --> delete if invalid
         */

        handle_feature_candidate_matches(feature_candidates, cam, false);
        remove_old_candidates(cam->id);

        #ifdef DEBUG_VERBOSE
        t.stop("handling candidates");
        #endif

        // collect the subset features not yet used anywhere
        std::vector<uint32_t> subset;
        {
            subset.reserve(feature_candidates.size());
            for (const auto& elem : feature_candidates)
                subset.push_back(elem.second);
        }

        const std::vector<uint32_t> new_candidate_features = set_negative_intersection(subset, unmatched);
        candidates_from_unmatched(new_candidate_features, cam);

        #ifdef DEBUG_VERBOSE
        t.stop("candidate intersection and new");
        #endif

        return true;
    }

    void PoseGraph::DEBUG_visualize_landmarks(const std::vector<cv::Point3f> points) const
    {
        if (points.size() < 3)
            return;

        std::vector<Eigen::Vector3d> epoints;
        epoints.reserve(points.size());
        for (const auto p : points)
        {
            epoints.emplace_back(Eigen::Vector3d(p.x, p.y, p.z));
            std::cout << "point: " << epoints.back().transpose() << "\n";
        }

        const auto debug_cloud = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(epoints));
        o3d::visualization::DrawGeometries({ debug_cloud });
    }

    void PoseGraph::DEBUG_visualize_landmarks(const std::vector<uint32_t> lms) const
    {
        if (lms.size() == 0)
            return;

        std::vector<cv::Point3f> points;
        points.reserve(lms.size());

        for (uint32_t i : lms)
        {
            const Landmark& lm = landmarks[i];
            points.emplace_back(lm.point3d);
        }

        DEBUG_visualize_landmarks(points);
    }

    std::vector<uint32_t> PoseGraph::set_negative_intersection(const std::vector<uint32_t> subset, const std::vector<uint32_t> all) const
    {
        if (subset.size() > all.size())
            throw std::runtime_error("subset was larger than all, aborting");

        std::vector<uint32_t> ret_set = all;

        for (const uint32_t s : subset)
        {
            auto iter = std::find(ret_set.begin(), ret_set.end(), s);
            if (iter != ret_set.end())
                ret_set.erase(iter);
        }

        return ret_set;
    }

    void PoseGraph::all_candidates_to_landmarks()
    {
        for (Landmark lm : candidates)
        {
            lm.to_landmark(cameras);
            lm.retriangulate_full(cameras, 0.0);
            landmarks.emplace_back(lm);
        }
    }

    void PoseGraph::retriangulate_landmarks(const std::vector<std::pair<uint32_t, uint32_t>> landmark_features)
    {
        const double lm_avg_magnitude = landmark_average_magnitude();

        #ifdef _OPENMP
        #pragma omp parallel for
        #endif
        for (int i = 0; i < landmark_features.size(); i++)
        {
            Landmark& lm = landmarks[landmark_features[i].first];
            lm.retriangulate_full(cameras, lm_avg_magnitude);
        }

/*
        for (int i = 0; i < landmark_features.size(); i++)
        {
            Landmark& lm = landmarks[landmark_features[i].first];
            cv::Point3f sum_point(0.f, 0.f, 0.f);
            int count = 0;

            for (int j = 0; j < lm.view_data.size(); j++)
            {
                const Camera* cam = cameras.at(lm.view_data[j].camera_id);

                for (int k = j; k < lm.view_data.size(); k++)
                {
                    const cv::Point3f p3d = lm.get_triangulated(j, k);

                    // point is out of bounds
                    // if (sqrt(p3d.dot(p3d)) > TRIANGULATED_POINT_OUTLIER_NORM)
                    // const double point_depth = math::depth_from_RtX(cam->rotation, cam->position, Eigen::Vector3d(p3d.x, p3d.y, p3d.z));
                    // if (point_depth <= 0 || point_depth > DEPTH_DISTANCE_OUTLIER)
                    //     continue;

                    if (!lm.to_landmark(cameras, lm_avg_magnitude))
                    {
                        continue;
                    }

                    sum_point += p3d;
                    count++;
                }
            }

            if (count > 0)
            {
                // std::cout << "original: " << lm.point3d << "; new point: " << sum_point / (float)count << "\n";
                lm.set_3d_point(sum_point / (float)count);
            }
        } */
    }

    void PoseGraph::retriangulate_landmarks()
    {
        const double lm_avg_3d = landmark_average_magnitude();

        #ifdef _OPENMP
        #pragma omp parallel for
        #endif
        for (int i = 0; i < landmarks.size(); i++)
        {
            landmarks[i].retriangulate_full(cameras, lm_avg_3d);
        }
    }

    void PoseGraph::remove_old_candidates(const uint32_t cam_id)
    {
        std::vector<uint32_t> to_remove_candidates;
        std::vector<uint32_t> debug_landmarks;

        const double lm_avg_dist = landmark_average_magnitude();

        for (int i = 0; i < candidates.size(); i++)
        {
            Landmark& cd = candidates[i];

            // candidate lost tracking and didn't reach required length --> delete
            if (abs((int)cd.view_data.back().camera_id - (int)cam_id) >= INVALID_CANDIDATE_CAMERA_TEMPORAL_DIFF &&
                cd.view_data.size() < reg_params.candidate_chain_len)
            {
                to_remove_candidates.push_back(i);
            }
            // candidate lost tracking but reached the required length --> to landmark
            // or enough movement between first and last
            else if ((abs((int)cd.view_data.back().camera_id - (int)cam_id) >= INVALID_CANDIDATE_CAMERA_TEMPORAL_DIFF &&
                cd.view_data.size() >= reg_params.candidate_chain_len) ||
                candidate_movement_large_enough(cd))
            {
                cd.to_landmark(cameras);
                cd.retriangulate_full(cameras, lm_avg_dist);
                landmarks.push_back(cd);
                debug_landmarks.push_back(landmarks.size() - 1);

                // const cv::Point2f front_pt = cd.view_data.front().feature_kp.pt;
                // const cv::Point2f back_pt = cd.view_data.back().feature_kp.pt;

                // // triangulate and create a landmark
                // if (cd.to_landmark(cameras, lm_avg_dist))
                // {
                //     cd.retriangulate_full(cameras, lm_avg_dist);
                //     landmarks.push_back(cd);
                //     debug_landmarks.push_back(landmarks.size() - 1);
                // }

                to_remove_candidates.push_back(i);
            }
        }

        // DEBUG_visualize_majority_candidate_matches(debug_landmarks, 100, false);
        // DEBUG_visualize_landmarks(debug_landmarks);
        remove_candidates(to_remove_candidates);
    }

    void PoseGraph::handle_feature_candidate_matches(const std::vector<std::pair<uint32_t, uint32_t>> matches, const Camera* cam, const bool check_for_lm)
    {
        cv::Mat cam_P;
        cv::eigen2cv(cam->P, cam_P);
        const uint32_t cam_id = cam->id;

        std::vector<uint32_t> to_remove_candidates;
        if (check_for_lm)
            to_remove_candidates.reserve(matches.size());

        std::vector<uint32_t> debug_landmarks;

        const double lm_avg_dist = landmark_average_magnitude();

        for (int i = 0; i < matches.size(); i++)
        {
            const uint32_t candidate_id = matches[i].first;
            const uint32_t feature_id = matches[i].second;

            Landmark& cd = candidates[candidate_id];
            cd.add_view(
                cam_id,
                feature_id,
                cam->kp_features[feature_id],
                cam->feature_descriptors.row(feature_id),
                cam_P,
                cameras,
                lm_avg_dist,
                false
            );

            if (cd.view_data.size() >= reg_params.candidate_chain_len && check_for_lm)
            {
                cd.to_landmark(cameras);
                cd.retriangulate_full(cameras, lm_avg_dist);
                landmarks.push_back(cd);
                debug_landmarks.push_back(landmarks.size() - 1);

                // const cv::Point2f front_pt = cd.view_data.front().feature_kp.pt;
                // const cv::Point2f back_pt = cd.view_data.back().feature_kp.pt;

                // // const float feature_diff_magnitude = sqrt((back_pt - front_pt).dot(back_pt - front_pt));

                // // triangulate and create a landmark
                // if (cd.to_landmark(cameras))
                // {
                //     landmarks.push_back(cd);
                //     debug_landmarks.push_back(landmarks.size() - 1);
                // }

                // either way, the candidate will be removed
                to_remove_candidates.push_back(matches[i].first);
            }
        }

        // DEBUG_visualize_majority_candidate_matches(debug_landmarks);
        // DEBUG_visualize_landmarks(debug_landmarks);

        if (check_for_lm)
            remove_candidates(to_remove_candidates);
    }

    std::vector<std::pair<uint32_t, uint32_t>> PoseGraph::filter_candidate_matches_homography(
        const std::vector<std::pair<uint32_t, uint32_t>> unfiltered,
        const Camera* cam)
    {
        // group the candidate by camera (min n (4) lms per view?), compute homography matrix
        // by-view, filter outliers by-view

        // <camera_id, <corresponding candidate_match_ids>>
        // the indexes in .second are indexes to unfiltered
        std::map<uint32_t, std::vector<uint32_t>> cam_grouped_matches;

        // if any candidate's views camera in cam_grouped_matches
        // --> add, else create from 0th
        for (uint32_t i = 0; i < unfiltered.size(); i++)
        {
            const Landmark& lm = candidates[unfiltered[i].first];

            std::map<uint32_t, std::vector<uint32_t>>::iterator cam_iter =
                cam_grouped_matches.find(lm.view_data.front().camera_id);

            // found, append the candidate_id
            if (cam_iter != cam_grouped_matches.end())
            {
                cam_iter->second.push_back(i);
            }
            // not found, create new
            else
            {
                cam_grouped_matches.insert(
                    std::pair<uint32_t, std::vector<uint32_t>>(
                        lm.view_data.front().camera_id,
                        { i }
                    )
                );
            }
        }

        std::vector<std::pair<uint32_t, uint32_t>> filtered_matches;
        filtered_matches.reserve(unfiltered.size());

        // filter with homography
        for (const auto it : cam_grouped_matches)
        {
            if (it.second.size() < INVALID_CANDIDATE_CAMERA_TEMPORAL_DIFF)
                continue;

            std::vector<cv::Point2f> fpoints1, fpoints2;
            fpoints1.reserve(it.second.size());
            fpoints2.reserve(it.second.size());

            std::vector<cv::DMatch> good_matches;
            std::vector<cv::KeyPoint> fkp1, fkp2;

            // collect fpoints1 and fpoints2
            for (uint32_t i : it.second)
            {
                fpoints1.push_back(candidates[unfiltered[i].first].view_data.front().feature_kp.pt);
                fpoints2.push_back(cam->kp_features[unfiltered[i].second].pt);

                fkp1.push_back(candidates[unfiltered[i].first].view_data.front().feature_kp);
                fkp2.push_back(cam->kp_features[unfiltered[i].second]);
            }

            cv::Mat inlier_mask, homography;
            std::vector<cv::DMatch> inlier_matches;

            homography = findHomography(fpoints1, fpoints2, cv::RANSAC, ransac_thresh, inlier_mask);

            int l_i = 0;
            for (size_t i = 0; i < fpoints1.size(); i++)
            {
                cv::Mat col = cv::Mat::ones(3, 1, CV_64F);
                col.at<double>(0) = fpoints1[i].x;
                col.at<double>(1) = fpoints2[i].y;
                col = homography * col;
                col /= col.at<double>(2);

                const double dist = sqrt(pow(col.at<double>(0) - fpoints2[i].x, 2) + pow(col.at<double>(1) - fpoints2[i].y, 2));
                if (dist < CANDIDATE_INLIER_THRESHOLD)
                {
                    filtered_matches.push_back(unfiltered[it.second[i]]);

                    good_matches.emplace_back(cv::DMatch(i, i, 0));
                    l_i++;
                }
            }

            // debug visualize matches
            {
                if (good_matches.size() == 0)
                    continue;

                auto ref_rgb = cameras[it.first]->rgb;

                cv::Mat matchimg;
                cv::drawMatches(rgbd::o3d_image_to_cv_image(ref_rgb), fkp1, rgbd::o3d_image_to_cv_image(cam->rgb), fkp2, good_matches, matchimg);
                cv::imshow("img", matchimg);
                cv::waitKey(0);
            }
        }

        filtered_matches.shrink_to_fit();

        #ifdef DEBUG_VERBOSE
        std::cout << "sizes : " << filtered_matches.size() << ", " << unfiltered.size() << "\n";
        #endif

        return filtered_matches;
    }

    std::vector<uint32_t> PoseGraph::find_negative_features_from_unmatched(
            const std::vector<std::pair<uint32_t, uint32_t>> feature_candidate_matches,
            const std::vector<uint32_t> landmark_unmatched_features) const
    {
        /**
         *  finds the features not in landmark_unmatched_features but in
         *  feature_candidate_matches
         *
         *  feature_candidate_matches should always be smaller
         */

        if (landmark_unmatched_features.size() < feature_candidate_matches.size())
        {
            std::cout << "lm unmatched features size: " << landmark_unmatched_features.size() << ", fcm size: " << feature_candidate_matches.size() << "\n";
            throw std::runtime_error("landmark_unmatched_features was larger than feature_candidate_matches, impossible case! aborting");
        }

        std::vector<uint32_t> negative;
        negative.reserve(landmark_unmatched_features.size() - feature_candidate_matches.size());

        std::vector<uint32_t> feature_candidate_matches_collected;
        feature_candidate_matches_collected.reserve(feature_candidate_matches.size());
        for (int i = 0; i < feature_candidate_matches.size(); i++)
            feature_candidate_matches_collected.emplace_back(feature_candidate_matches[i].second);

        // https://stackoverflow.com/questions/15758680/get-all-vector-elements-that-dont-belong-to-another-vector
        std::remove_copy_if(landmark_unmatched_features.begin(), landmark_unmatched_features.end(), std::back_inserter(negative),
            [&feature_candidate_matches_collected](const uint32_t& arg)
            { return (std::find(feature_candidate_matches_collected.begin(), feature_candidate_matches_collected.end(), arg) != feature_candidate_matches_collected.end());});

        return negative;
    }

    void PoseGraph::remove_candidates(std::vector<uint32_t> matched_candidates)
    {
        // sort and reverse the candidates for easier removing
        std::sort(matched_candidates.begin(), matched_candidates.end());
        std::reverse(matched_candidates.begin(), matched_candidates.end());

        for (int i : matched_candidates)
            candidates.erase(candidates.begin() + i);
    }

    std::vector<uint32_t> PoseGraph::add_landmarks_from_candidates(const Camera* cam,
        const std::vector<std::pair<uint32_t, uint32_t>> candidate_matches)
    {
        /**
         *  if possible, triangulate and add to landmarks
         *  else add view to landmark
         */

        std::vector<uint32_t> succesfull_candidates;
        succesfull_candidates.reserve(candidate_matches.size());

        const cv::Mat cam_P = [&]{
            cv::Mat p;
            cv::eigen2cv(cam->P, p);
            return p;
        }();
        const uint32_t cam_id = cam->id;

        std::vector<Eigen::Vector3d> debug_points;
        std::vector<cv::Point2f> debug_features;

        const double lm_avg_magnitude = landmark_average_magnitude();

        for (int i = 0; i < candidate_matches.size(); i++)
        {
            const uint32_t candidate_id = candidate_matches[i].first;
            Landmark& candidate = candidates[candidate_id];
            const uint32_t feature_id = candidate_matches[i].second;

            const std::vector<cv::Point2f> x1 = { candidate.view_data[0].feature_kp.pt };
            const std::vector<cv::Point2f> x2 = { cam->kp_features[feature_id].pt };

            // skip triangulation and do not add if deviation not big enough
            if (abs(x1[0].x - x2[0].x) < FEATURE_TRIANGULATION_MIN_DIFF &&
                abs(x1[0].y - x2[0].y) < FEATURE_TRIANGULATION_MIN_DIFF)
            {
                candidate.add_view(
                    cam_id,
                    feature_id,
                    cam->kp_features[feature_id],
                    cam->feature_descriptors.row(feature_id),
                    cam_P,
                    cameras,
                    lm_avg_magnitude,
                    true
                );
                continue;
            }
            succesfull_candidates.emplace_back(candidate_id);

            const cv::Mat firstcam_P = candidate.view_data[0].camera_P;

            // std::cout << "id: " << candidate.view_data[0].camera_id << "\n" << firstcam_P << "\n\n" << cam_P << "\n\n\n";

            cv::Mat p4d;
            cv::triangulatePoints(firstcam_P, cam_P, x1, x2, p4d);

            const cv::Point3f p3d(
                p4d.at<float>(0, 0) / p4d.at<float>(3, 0),
                p4d.at<float>(1, 0) / p4d.at<float>(3, 0),
                p4d.at<float>(2, 0) / p4d.at<float>(3, 0)
            );

            // discard the candidate/landmark if triangulation failed
            if (sqrt(p3d.dot(p3d)) > TRIANGULATED_POINT_OUTLIER_NORM)
                continue;

            Landmark lm = candidate;

            lm.add_view(
                cam_id,
                feature_id,
                cam->kp_features[feature_id],
                cam->feature_descriptors.row(feature_id),
                cam_P,
                cameras,
                lm_avg_magnitude,
                true
            );

            lm.set_3d_point(p3d);
            landmarks.emplace_back(lm);

            debug_points.emplace_back(Eigen::Vector3d(p3d.x, p3d.y, p3d.z));
            debug_features.emplace_back(x2[0]);
        }

        /* if (debug_points.size() > 10)
        {
            auto debug_cloud = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(debug_points));
            std::shared_ptr<o3d::geometry::TriangleMesh> camera_mesh = std::make_shared<o3d::geometry::TriangleMesh>(o3d::geometry::TriangleMesh());
            o3d::io::ReadTriangleMeshFromOBJ(reg_params.assets_path + DEBUG_CAMERA_PATH, *camera_mesh, false);
            camera_mesh->Transform(cam->T);
            DEBUG_visualize_features(cam->rgb, debug_features);
            o3d::visualization::DrawGeometries({debug_cloud, camera_mesh});
        } */

        succesfull_candidates.shrink_to_fit();
        return succesfull_candidates;
    }

    std::vector<std::pair<uint32_t, uint32_t>> PoseGraph::match_features_against_candidates(const Camera* cam,
        const std::vector<uint32_t> matches_negative,
        std::map<std::pair<uint32_t, uint32_t>, cv::Mat>& camera_hgraphy_pairs) const
    {
        if (candidates.size() == 0 || matches_negative.size() == 0)
            return std::vector<std::pair<uint32_t, uint32_t>>();

        // create candidate descriptors
        const cv::Mat candidate_descriptors = create_candidate_descriptor();

        // check for empty
        if (candidate_descriptors.rows == 0)
            return std::vector<std::pair<uint32_t, uint32_t>>();

        // collect unmatched feature descriptors
        cv::Mat unmatched_descriptors (matches_negative.size(), cam->feature_descriptors.cols, CV_32F);
        const cv::Mat cam_kp_descriptors = cam->feature_descriptors;

        for (int i = 0; i < matches_negative.size(); i++)
        {
            const float* const vaptr = cam_kp_descriptors.ptr<float>(matches_negative[i], 0);

            float* fptr = unmatched_descriptors.ptr<float>(i, 0);
            for (int j = 0; j < unmatched_descriptors.cols; j++)
            {
                fptr[j] = vaptr[j];
            }
        }

        Timer t;
        // <candidate_id, index in matches_negative>
        std::vector<std::pair<uint32_t, uint32_t>> candidate_matches =
            match_features_bf(candidate_descriptors, unmatched_descriptors);

        #ifdef DEBUG_VERBOSE
        t.stop("candidate match_features_bf");
        #endif

        // unravel candidate_matches to be
        // <candidate_id, feature_id>
        for (int i = 0; i < candidate_matches.size(); i++)
        {
            const uint32_t second_id = candidate_matches[i].second;
            candidate_matches[i].second = matches_negative[second_id];
        }

        // homography filtering. Marginally slower, but yields consistently more robust results
        const auto good_matches = cdlm_pairwise_homography_filter(cam, candidate_matches, candidates, camera_hgraphy_pairs);

        #ifdef DEBUG_VERBOSE
        t.stop("candidate hgraphy filtering");
        #endif
        // DEBUG_visualize_candidate_matches(cam, good_matches, 1);

        return good_matches;
    }

    cv::Mat PoseGraph::create_candidate_descriptor() const
    {
        cv::Mat descriptor (candidates.size(), candidates[0].view_data[0].feature_descriptor.cols, CV_32F);

        // set the descriptor rows to be landmark descriptors
        for (int i = 0; i < candidates.size(); i++)
        {
            const cv::Mat view_descriptor = candidates[i].view_data.back().feature_descriptor;
            const float* vaptr = view_descriptor.ptr<float>(0, 0);

            float* fptr = descriptor.ptr<float>(i, 0);
            for (int j = 0; j < descriptor.cols; j++)
            {
                fptr[j] = vaptr[j];
            }
        }

        return descriptor;
    }

    void PoseGraph::add_track_to_landmarks(const Camera* cam,
        const std::vector<std::pair<uint32_t, uint32_t>> landmarks_in_view,
        const std::vector<cv::Point3f> points3d,
        const std::vector<int> inliers)
    {
        const uint32_t cam_id = cam->id;
        cv::Mat cam_P;
        cv::eigen2cv(cam->P, cam_P);

        const double lm_avg_magnitude = landmark_average_magnitude();

        for (int i : inliers)
        {
            const uint32_t landmark_id = landmarks_in_view[i].first;
            const uint32_t feature_id = landmarks_in_view[i].second;
            // const cv::Point3f p3d = points3d[i];
            const cv::KeyPoint kpf = cam->kp_features[feature_id];
            const cv::Mat descriptor = cam->feature_descriptors.row(feature_id);

            landmarks.at(landmark_id).add_view(
                cam_id,
                feature_id,
                kpf,
                descriptor,
                cam_P,
                cameras,
                lm_avg_magnitude,
                true
            );
        }
    }

    std::vector<std::pair<uint32_t, uint32_t>>
        PoseGraph::find_landmarks_in_view(const Camera* cam,
        std::map<std::pair<uint32_t, uint32_t>, cv::Mat>& camera_hgraphy_pairs,
        const uint32_t view_offset) const
    {
        Timer t;

        const cv::Mat landmark_descriptor = create_landmark_descriptor(view_offset);

        #ifdef DEBUG_VERBOSE
        t.stop("create landmark descriptor");
        #endif

        // < lm_id, f_id >
        const std::vector<std::pair<uint32_t, uint32_t>> landmarks_in_view =
            match_features_bf(landmark_descriptor, cam->feature_descriptors);

        #ifdef DEBUG_VERBOSE
        t.stop("match landmarks against features");
        #endif

        const auto good_landmarks_in_view = cdlm_pairwise_homography_filter(cam, landmarks_in_view, landmarks, camera_hgraphy_pairs, true);

        #ifdef DEBUG_VERBOSE
        t.stop("landmark homography filtering");
        #endif

        return good_landmarks_in_view;
    }

    cv::Mat PoseGraph::create_landmark_descriptor(const uint32_t view_offset) const
    {
        if (landmarks.size() == 0)
        {
            throw std::runtime_error("cannot create a descriptor because there are no landmarks!");
        }

        cv::Mat descriptor (landmarks.size(), landmarks[0].view_data[0].feature_descriptor.cols, CV_32F);

        // set the descriptor rows to be landmark descriptors
        for (int i = 0; i < landmarks.size(); i++)
        {

            const cv::Mat view_average_desc = landmarks[i].view_data[landmarks[i].view_data.size() - 1 -
                (landmarks[i].view_data.size() > view_offset ? view_offset : 0)
            ].feature_descriptor;
                // compute_view_average_descriptor();
            const float* vaptr = view_average_desc.ptr<float>(0, 0);

            float* fptr = descriptor.ptr<float>(i, 0);
            for (int j = 0; j < descriptor.cols; j++)
            {
                fptr[j] = vaptr[j];
            }
        }

        return descriptor;
    }

    std::shared_ptr<o3d::geometry::PointCloud> PoseGraph::landmarks_to_pointcloud() const
    {
        std::vector<Eigen::Vector3d> points3d, colors;
        points3d.reserve(landmarks.size());
        colors.reserve(landmarks.size());

        for (int i = 0; i < landmarks.size(); i++)
        {
            const cv::Point3f p3d = landmarks[i].point3d;
            const Eigen::Vector3d p3de (p3d.x, p3d.y, p3d.z);

            // if (p3de.norm() > 30.0)
            //     continue;

            points3d.emplace_back(p3de);
            colors.emplace_back(landmarks[i].point_color);
        }

        auto cloud = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(points3d));
        cloud->colors_ = colors;
        return cloud;
    }

    void PoseGraph::create_pointclouds_from_depths(const bool only_scale)
    {
        if (cameras.empty())
            throw std::runtime_error("cannot create pointclouds from non-existent cameras!");

        if (!only_scale)
            depth_pointclouds.clear();

        // calculate scale for depthclouds
    	// gather all landmarks which have .front().camera_index  == 0
        std::vector<cv::Point3f> zero_view_points;
        std::vector<cv::Point2f> zero_view_features;
        for (int i = 0; i < landmarks.size(); i++)
        {
            const Landmark& lm = landmarks[i];
            if (lm.view_data.front().camera_id == 0)
            {
                zero_view_features.push_back(lm.view_data.front().feature_kp.pt);
                zero_view_points.push_back(lm.point3d);
            }
        }

        uint32_t count = 0;
        double cloud_scale = 0.0;

        const Camera* cam = cameras.at(0);

        for (int i = 0; i < zero_view_points.size(); i++)
        {
            const double depth = (double)(*cam->depth->PointerAt<uint16_t>(
                (int)zero_view_features[i].x, (int)zero_view_features[i].y)) / 1000.0;

            // do not account for occluded pixels
            if (depth < DEPTH_NEAR_CLIPPING)
                continue;

            const Eigen::Vector3d depth_point = rgbd::triangulate_point(
                Eigen::Vector2d(zero_view_features[i].x, zero_view_features[i].y),
                depth, cam->intr);

            const Eigen::Vector4d depth_point_transformed = cam->T * Eigen::Vector4d(depth_point.x(), depth_point.y(), depth_point.z(), 0.0);
            const Eigen::Vector3d dpoint (depth_point_transformed.x(), depth_point_transformed.y(), depth_point_transformed.z());

            cloud_scale +=
                Eigen::Vector3d(zero_view_points[i].x, zero_view_points[i].y, zero_view_points[i].z).norm() /
                dpoint.norm();

            count++;
        }

        this->pointcloud_scale = cloud_scale / (double)count;
        #ifdef DEBUG_VERBOSE
        std::cout << "pointcloud scale: " << pointcloud_scale << "\n";
        #endif

        if (only_scale)
            return;

        // project depth images to pointclouds
        for (int i = 0; i < cameras.size(); i++)
        {
            if (cameras[i]->depth == nullptr || cameras[i]->position.hasNaN() || cameras[i]->position.norm() > 1000.0)
                continue;

            auto cloud = rgbd::create_pcloud_from_rgbd(*cameras[i]->rgb, *cameras[i]->depth, DEFAULT_DEPTH_SCALE, reg_params.depth_far_clip, false, cameras[i]->intr);

            if (cloud->points_.size() == 0)
            {
                std::cout << "skip camera " << i << " due to no points\n";
                continue;
            }

            cloud->Scale(this->pointcloud_scale, false);
            cloud->Transform(cameras[i]->T);

            depth_pointclouds.emplace_back(cloud);
        }
    }

    void PoseGraph::remove_last_camera(Camera* cam)
    {
        const uint32_t cam_id = cam->id;
        if (cameras.back()->id == cam_id)
        {
            cameras.pop_back();
        }
    }

    std::vector<std::shared_ptr<Camera>> PoseGraph::get_cameras_copy() const
    {
        std::vector<std::shared_ptr<Camera>> rcameras;

        for (const auto c : cameras)
        {
            rcameras.emplace_back(std::make_shared<Camera>(Camera(*c)));
        }

        return rcameras;
    }

    void PoseGraph::bundle_adjust()
    {
        /**
         *  MAYBE:
         *      http://docs.ros.org/en/melodic/api/gtsam/html/SFMExample__SmartFactor_8cpp_source.html
         *      https://github.com/nghiaho12/SFM_example/blob/3b95176d9758752cbb1ebb7fa0050ded0b85e950/src/main.cpp#L352
         *
         */

        #ifdef DEBUG_MINIMAL
        std::cout << "begin bundle adjustment \n";
        #endif

        gtsam::Values result;

        // add camera calibration matrix

        const double cx = cameras.front()->intr.intrinsic_matrix_.coeff(0, 2);
        const double cy = cameras.front()->intr.intrinsic_matrix_.coeff(1, 2);
        const double fx = cameras.front()->intr.intrinsic_matrix_.coeff(0, 0);
        const double fy = cameras.front()->intr.intrinsic_matrix_.coeff(1, 1);

        gtsam::Cal3_S2 K(fx ,fy, 0, cx, cy);
        const gtsam::noiseModel::Isotropic::shared_ptr measurement_noise = gtsam::noiseModel::Isotropic::Sigma(2, ADJUST_2DPOINT_NOISE);

        gtsam::NonlinearFactorGraph graph;
        gtsam::Values initial;

        // add camera indexes and transformations (R, t)
        for (int i = 0; i < cameras.size(); i++)
        {
            const Camera* cam = cameras.at(i);

            const gtsam::Rot3 R (
                cam->T.coeff(0, 0), cam->T.coeff(0, 1), cam->T.coeff(0, 2),
                cam->T.coeff(1, 0), cam->T.coeff(1, 1), cam->T.coeff(1, 2),
                cam->T.coeff(2, 0), cam->T.coeff(2, 1), cam->T.coeff(2, 2)
            );

            const gtsam::Point3 t (
                cam->T.coeff(0, 3),
                cam->T.coeff(1, 3),
                cam->T.coeff(2, 3)
            );

            const gtsam::Pose3 pose (R, t);

            if (i == 0)
            {
                gtsam::noiseModel::Diagonal::shared_ptr pose_noise = gtsam::noiseModel::Diagonal::Sigmas(
                    (gtsam::Vector(6) << gtsam::Vector3::Constant(ADJUST_3DPOINT_NOISE), gtsam::Vector3::Constant(ADJUST_3DPOINT_NOISE)).finished());

                graph.emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(gtsam::Symbol('x', 0), pose, pose_noise);
            }

            initial.insert(gtsam::Symbol('x', cam->id), pose);
        }

        // add landmarks
        for (int i = 0; i < landmarks.size(); i++)
        {
            const Landmark& lm = landmarks[i];

            for (int j = 0; j < lm.view_data.size(); j++)
            {
                const cv::Point2f& ptcv = lm.view_data[j].feature_kp.pt;
                const gtsam::Point2 pt (ptcv.x, ptcv.y);

                graph.emplace_shared<gtsam::GeneralSFMFactor2<gtsam::Cal3_S2>>(pt, measurement_noise, gtsam::Symbol('x', lm.view_data[j].camera_id),
                    gtsam::Symbol('l', i), gtsam::Symbol('K', 0));
            }
        }

        initial.insert(gtsam::Symbol('K', 0), K);

        gtsam::noiseModel::Diagonal::shared_ptr cal_noise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(5) << 0.0, 0.0, 0, 0.0, 0.0).finished());
        graph.emplace_shared<gtsam::PriorFactor<gtsam::Cal3_S2>>(gtsam::Symbol('K', 0), K, cal_noise);

        for (int i = 0; i < landmarks.size(); i++)
        {
            const Landmark& lm = landmarks[i];

            const cv::Point3f p = lm.point3d;
            initial.insert<gtsam::Point3>(gtsam::Symbol('l', i), gtsam::Point3(p.x, p.y, p.z));

            if (i == 0)
            {
                // maximum noise for pose, 0.3m -> 30cm (assumed)
                // gtsam::noiseModel::Isotropic::shared_ptr point_noise = gtsam::noiseModel::Isotropic::Sigma(3, ADJUST_3DPOINT_NOISE);
                gtsam::noiseModel::Isotropic::shared_ptr point_noise = gtsam::noiseModel::Isotropic::Sigma(3, ADJUST_3DPOINT_NOISE);
                graph.emplace_shared<gtsam::PriorFactor<gtsam::Point3>>(gtsam::Symbol('l', i), gtsam::Point3(p.x, p.y, p.z), point_noise);
            }
        }

        #ifdef DEBUG_MINIMAL
        std::cout << "data added to bundle adjustment, adjusting...\n";
        #endif

        result = gtsam::LevenbergMarquardtOptimizer(graph, initial).optimize();
        #ifdef DEBUG_VERBOSE
        std::cout << "\n" << "initial graph error: " << graph.error(initial) << "\n" << "final graph error: " << graph.error(result) << "\n";
        #endif

        // std::vector<Eigen::Vector3d> old_positions;

        // set new camera poses
        for (int i = 0; i < cameras.size(); i++)
        {
            const Eigen::Matrix3d R = result.at<gtsam::Pose3>(gtsam::Symbol('x', i)).rotation().matrix();
            const Eigen::Vector3d t = result.at<gtsam::Pose3>(gtsam::Symbol('x', i)).translation();

            // std::cout << "camera adjust movement: " << (cameras[i]->position - (-R.transpose() * t)).norm() << "\n";
            // old_positions.push_back(cameras[i]->position);

            set_camera_T_and_P(cameras[i], R, t);
        }

        // for (int i = 0; i < old_positions.size(); i++)
        // {
        //     std::cout << "old: " << old_positions[i].transpose() << ", new: " << cameras[i]->position.transpose() << "\n";
        // }

        // retriangulate_landmarks();
    }

    void PoseGraph::recompute_camera_poses_landmark_PnP()
    {
        /**
         *  for each cameras, collect applying 2d-3d feature-landmark
         *  correspondences, PnP
         */

        for (int ii = 0; ii < cameras.size(); ii++)
        {
            Camera* cam = cameras.at(ii);

            std::map<std::pair<uint32_t, uint32_t>, cv::Mat> camera_homography_pairs;

            // match features against landmarks, <landmark_id, feature_id>
            std::vector<std::pair<uint32_t, uint32_t>> landmarks_in_view =
                find_landmarks_in_view(cam, camera_homography_pairs);

            // collect 3d landmarks and 2d feature points
            std::vector<cv::Point2f> view_features;
            std::vector<cv::Point3f> landmark_points;

            std::vector<Eigen::Vector3d> points3d;

            for (const auto& lfpair : landmarks_in_view)
            {
                const auto p3d = landmarks.at(lfpair.first).point3d;
                if (p3d == cv::Point3f(0.0, 0.0, 0.0))
                    continue;

                landmark_points.push_back(p3d);
                view_features.push_back(cam->kp_features[lfpair.second].pt);
                points3d.push_back(Eigen::Vector3d(p3d.x, p3d.y, p3d.z));

                // std::cout << view_features.back() << " : " << landmark_points.back() << "\n";
            }

            // DEBUG_visualize_features(cam->rgb, view_features);
            // DEBUG_visualize_landmarks(landmark_points);

            // {
            //     std::shared_ptr<o3d::geometry::TriangleMesh> camera_mesh = std::make_shared<o3d::geometry::TriangleMesh>(o3d::geometry::TriangleMesh());
            //             o3d::io::ReadTriangleMeshFromOBJ(reg_params.assets_path + DEBUG_CAMERA_PATH, *camera_mesh, false);
            //             camera_mesh->Transform(cam->T);

            //     auto cloud = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(points3d));
            //     o3d::visualization::DrawGeometries({ cloud, camera_mesh });
            // }

            const cv::Mat cv_intr = [&]{
                cv::Mat intr;
                cv::eigen2cv(cam->intr.intrinsic_matrix_, intr);
                return intr;
            }();

            cv::Mat rcv_rodr, rcv, tcv, inliers;
            // cv::eigen2cv(cam->rotation, rcv);
            // cv::eigen2cv(cam->position, tcv);

            // these parameters have been figured out by painstakingly trial-and-erroring
            const bool retval = cv::solvePnPRansac(landmark_points, view_features, cv_intr, cam->distortion_coeffs, rcv, tcv, false, 1000, 4.0, 0.987, inliers);

            Eigen::Matrix3d R;
            Eigen::Vector3d t;

            cv::Rodrigues(rcv, rcv_rodr);
            cv::cv2eigen(rcv_rodr, R);
            cv::cv2eigen(tcv, t);

            // std::cout << "inliers: " << inliers.size << " and retval " << retval << " t " << t.transpose() <<  "\n";

            // const Eigen::Matrix3d old_rot = cam->rotation;

            set_camera_T_and_P(cam, R.transpose(), -R.transpose() * t);

            // std::cout << cam->rotation << "\n\n" << old_rot << "\n\n" << cam->position.transpose() << "\n";

            // {
            //     std::shared_ptr<o3d::geometry::TriangleMesh> camera_mesh = std::make_shared<o3d::geometry::TriangleMesh>(o3d::geometry::TriangleMesh());
            //             o3d::io::ReadTriangleMeshFromOBJ(reg_params.assets_path + DEBUG_CAMERA_PATH, *camera_mesh, false);
            //             camera_mesh->Transform(cam->T);

            //     auto cloud = std::make_shared<o3d::geometry::PointCloud>(o3d::geometry::PointCloud(points3d));
            //     o3d::visualization::DrawGeometries({ cloud, camera_mesh });
            // }
        }
    }

    void PoseGraph::visualize_tracks() const
    {
        std::vector<std::shared_ptr<o3d::geometry::TriangleMesh>> camera_meshes;
        std::vector<std::shared_ptr<const o3d::geometry::Geometry>> world_meshes;
        for (int i = 0; i < cameras.size(); i++)
        {
            const Camera* c = cameras.at(i);

            if (c->position.hasNaN() || c->position.norm() > 200)
                continue;

            std::shared_ptr<o3d::geometry::TriangleMesh> camera_mesh = std::make_shared<o3d::geometry::TriangleMesh>(o3d::geometry::TriangleMesh());
            o3d::io::ReadTriangleMeshFromOBJ(reg_params.assets_path + DEBUG_CAMERA_PATH, *camera_mesh, false);

            if (pointcloud_scale == 0.0f)
                camera_mesh->Scale(1.0f);
            else
                camera_mesh->Scale(pointcloud_scale * CAMERA_SCALE);

            camera_mesh->Transform(c->T);

            // camera_mesh->texture_.Clear();
            // camera_mesh->PaintUniformColor(index2color(i));

            camera_meshes.push_back(camera_mesh);
            world_meshes.push_back(camera_mesh);

            #ifdef DEBUG_MINIMAL
            std::cout << "camera " << c->id << " position: " << c->position.transpose() << "\n";
            #endif
        }

        #ifdef DEBUG_VERBOSE
        std::cout << "triangulated clouds size: " << ptrclouds.size() << ", depth clouds: " << depth_pointclouds.size() << "\n";
        #endif

        /*
        for (int i = 0; i < ptrclouds.size(); i++)
        {
            break;

            auto cloud = ptrclouds[i];
            if (cloud->points_.size() < 10)
                continue;

            std::cout << "cloud points: " << cloud->points_.size() << "\n";

            if (cloud->colors_.size() == 0)
            {
                std::cout << "featurecloud colors painted\n";
                cloud->PaintUniformColor(Eigen::Vector3d(1.0, 0.0, 0.0));
            }

            for (int p3d_i = 0; p3d_i < cloud->points_.size(); p3d_i++)
            {
                if (cloud->points_.at(p3d_i).norm() > 60.0)
                    cloud->points_.at(p3d_i) = Eigen::Vector3d::Zero();
            }

            world_meshes.push_back(cloud);
        }
        */

        for (int i = 0; i < depth_pointclouds.size(); i++)
        {
            // break;

            auto temp_cloud = depth_pointclouds[i];
            world_meshes.push_back(temp_cloud);
        }

        const auto lm_cloud = landmarks_to_pointcloud();
        // world_meshes.push_back(lm_cloud);

        world_meshes.push_back(o3d::geometry::TriangleMesh::CreateCoordinateFrame());

        o3d::visualization::DrawGeometries(world_meshes, "camera track", 1920, 1080, 180, 120);
    }
}


text:
/**
 * Orthanc - A Lightweight, RESTful DICOM Store
 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
 * Department, University Hospital of Liege, Belgium
 * Copyright (C) 2017-2020 Osimis S.A., Belgium
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program. If not, see
 * <http://www.gnu.org/licenses/>.
 **/

#include "PrecompiledHeaders.h"
#include "SharedLibrary.h"

#include "Logging.h"
#include "OrthancException.h"

#include <boost/filesystem.hpp>

#if defined(_WIN32)
#include <windows.h>
#elif defined(__linux__) || (defined(__APPLE__) && defined(__MACH__)) || defined(__FreeBSD_kernel__) || defined(__FreeBSD__) || defined(__OpenBSD__)
#include <dlfcn.h>
#else
#error Support your platform here
#endif

namespace Orthanc
{
  SharedLibrary::SharedLibrary(const std::string& path) :
    path_(path),
    handle_(NULL)
  {
#if defined(_WIN32)
    handle_ = ::LoadLibraryA(path_.c_str());
    if (handle_ == NULL)
    {
      LOG(ERROR) << "LoadLibrary(" << path_ << ") failed: Error " << ::GetLastError();
      throw OrthancException(ErrorCode_SharedLibrary);
    }

#elif defined(__linux__) || (defined(__APPLE__) && defined(__MACH__)) || defined(__FreeBSD_kernel__) || defined(__FreeBSD__) || defined(__OpenBSD__)

    /**
     * "RTLD_LOCAL" is the default, and is only present to be
     * explicit. "RTLD_DEEPBIND" was added in Orthanc 1.6.0, in order
     * to avoid crashes while loading plugins from the LSB binaries of
     * the Orthanc core.
     *
     * BUT this had no effect, and this results in a crash if loading
     * the Python 2.7 plugin => We disabled it again in Orthanc 1.6.1.
     **/

#if 0 // && defined(RTLD_DEEPBIND)  // This is a GNU extension
    // Disabled in Orthanc 1.6.1
    handle_ = ::dlopen(path_.c_str(), RTLD_NOW | RTLD_LOCAL | RTLD_DEEPBIND);
#else
    handle_ = ::dlopen(path_.c_str(), RTLD_NOW | RTLD_LOCAL);
#endif

    if (handle_ == NULL)
    {
      std::string explanation;
      const char *tmp = ::dlerror();
      if (tmp)
      {
        explanation = ": Error " + std::string(tmp);
      }

      LOG(ERROR) << "dlopen(" << path_ << ") failed" << explanation;
      throw OrthancException(ErrorCode_SharedLibrary);
    }

#else
#error Support your platform here
#endif
  }

  SharedLibrary::~SharedLibrary()
  {
    if (handle_)
    {
#if defined(_WIN32)
      ::FreeLibrary((HMODULE)handle_);
#elif defined(__linux__) || (defined(__APPLE__) && defined(__MACH__)) || defined(__FreeBSD_kernel__) || defined(__FreeBSD__) || defined(__OpenBSD__)
      ::dlclose(handle_);
#else
#error Support your platform here
#endif
    }
  }

  SharedLibrary::FunctionPointer SharedLibrary::GetFunctionInternal(const std::string& name)
  {
    if (!handle_)
    {
      throw OrthancException(ErrorCode_InternalError);
    }

#if defined(_WIN32)
    return ::GetProcAddress((HMODULE)handle_, name.c_str());
#elif defined(__linux__) || (defined(__APPLE__) && defined(__MACH__)) || defined(__FreeBSD_kernel__) || defined(__FreeBSD__) || defined(__OpenBSD__)
    return ::dlsym(handle_, name.c_str());
#else
#error Support your platform here
#endif
  }

  SharedLibrary::FunctionPointer SharedLibrary::GetFunction(const std::string& name)
  {
    SharedLibrary::FunctionPointer result = GetFunctionInternal(name);

    if (result == NULL)
    {
      throw OrthancException(
        ErrorCode_SharedLibrary,
        "Shared library does not expose function \"" + name + "\"");
    }
    else
    {
      return result;
    }
  }

  bool SharedLibrary::HasFunction(const std::string& name)
  {
    return GetFunctionInternal(name) != NULL;
  }
}

ext: cpp


text:
/* Copyright (c) 2015, 2020, Oracle and/or its affiliates. All rights reserved.

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License, version 2.0,
   as published by the Free Software Foundation.

   This program is also distributed with certain software (including
   but not limited to OpenSSL) that is licensed under separate terms,
   as designated in a particular file or component or in included license
   documentation.  The authors of MySQL hereby grant you an additional
   permission to link the program and your derivative works with the
   separately licensed software that they have included with MySQL.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License, version 2.0, for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301  USA */

#include <stddef.h>
#include <boost/concept/usage.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/move/utility_core.hpp>
#include <boost/range/adaptor/argument_fwd.hpp>
#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <utility>
#include <vector>

#include "sql/item_geofunc.h"
#include "sql/item_geofunc_internal.h"

struct Rtree_value_maker {
  typedef std::pair<BG_box, size_t> result_type;
  template <typename T>
  result_type operator()(T const &v) const {
    BG_box box;
    make_bg_box(v.value(), &box);
    return result_type(box, v.index());
  }
};

struct Is_rtree_box_valid {
  typedef std::pair<BG_box, size_t> Rtree_entry;
  bool operator()(Rtree_entry const &re) const {
    return is_box_valid(re.first);
  }
};

void make_rtree(const BG_geometry_collection::Geometry_list &gl,
                Rtree_index *rtree) {
  Rtree_index temp_rtree(gl | boost::adaptors::indexed() |
                         boost::adaptors::transformed(Rtree_value_maker()) |
                         boost::adaptors::filtered(Is_rtree_box_valid()));

  rtree->swap(temp_rtree);
}

/*
  A functor to make an rtree value entry from an array element of
  Boost.Geometry model type.
 */
struct Rtree_value_maker_bggeom {
  typedef std::pair<BG_box, size_t> result_type;
  template <typename T>
  result_type operator()(T const &v) const {
    BG_box box;
    boost::geometry::envelope(v.value(), box);

    return result_type(box, v.index());
  }
};

template <typename MultiGeometry>
void make_rtree_bggeom(const MultiGeometry &mg, Rtree_index *rtree) {
  Rtree_index temp_rtree(
      mg | boost::adaptors::indexed() |
      boost::adaptors::transformed(Rtree_value_maker_bggeom()) |
      boost::adaptors::filtered(Is_rtree_box_valid()));

  rtree->swap(temp_rtree);
}

// Explicit template instantiation
template void make_rtree_bggeom<Gis_multi_line_string>(
    const Gis_multi_line_string &, Rtree_index *);
template void make_rtree_bggeom<Gis_multi_point>(const Gis_multi_point &,
                                                 Rtree_index *);

template void make_rtree_bggeom<Gis_multi_polygon>(const Gis_multi_polygon &,
                                                   Rtree_index *);

ext: cc