From 36c0033f8f5560a2825d36f60e548017e3d42989 Mon Sep 17 00:00:00 2001 From: Laszlo Habon Date: Fri, 25 Jul 2014 09:34:18 +0200 Subject: [PATCH] my work included --- .gitignore | 4 + src/CMakeLists.txt | 2 + src/densemap/CMakeLists.txt | 10 + src/densemap/rectify.cc | 587 ++++++++++++++++++++++++++++++++++++ src/densemap/rectify.h | 30 ++ src/densemap/reproject.cc | 105 +++++++ src/densemap/reproject.h | 62 ++++ src/densemapper.cc | 558 ++++++++++++++++++++++++++++++++++ 8 files changed, 1358 insertions(+) create mode 100644 src/densemap/CMakeLists.txt create mode 100644 src/densemap/rectify.cc create mode 100644 src/densemap/rectify.h create mode 100644 src/densemap/reproject.cc create mode 100644 src/densemap/reproject.h create mode 100644 src/densemapper.cc diff --git a/.gitignore b/.gitignore index 483f436..e09b19e 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,7 @@ # Build directory build/ + +*~ +*.kdev4 +.kdev4 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ebaa9d0..9d2a846 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -12,6 +12,7 @@ ADD_SUBDIRECTORY(fm) ADD_SUBDIRECTORY(loop) ADD_SUBDIRECTORY(sfm) ADD_SUBDIRECTORY(util) +ADD_SUBDIRECTORY(densemap) SET(MAVMAP_LIBRARIES sequential_mapper @@ -38,6 +39,7 @@ SET(MAVMAP_LIBRARIES timer math) +ADD_EXECUTABLE(densemapper densemapper.cc) ADD_EXECUTABLE(mapper mapper.cc) TARGET_LINK_LIBRARIES(mapper ${MAVMAP_LIBRARIES} diff --git a/src/densemap/CMakeLists.txt b/src/densemap/CMakeLists.txt new file mode 100644 index 0000000..e019f03 --- /dev/null +++ b/src/densemap/CMakeLists.txt @@ -0,0 +1,10 @@ +INCLUDE_DIRECTORIES( + ../ +) + +ADD_LIBRARY(rectify rectify.cc) +ADD_LIBRARY(reproject reproject.cc) +TARGET_LINK_LIBRARIES(rectify + reproject +) + diff --git a/src/densemap/rectify.cc b/src/densemap/rectify.cc new file mode 100644 index 0000000..b694dc5 --- /dev/null +++ b/src/densemap/rectify.cc @@ -0,0 +1,587 @@ +#include "rectify.h" +#include "reproject.h" + + + + + +using namespace std; +using namespace cv; +using namespace Eigen; + +void rectify_images(const Matrix3d& calib_matrix_, + const vector& camera_poses_, + int lidx, int ridx ) +{ + + Mat cvCalibMatrix; + eigen2cv(calib_matrix_,cvCalibMatrix); + + + Vector3d rvec, tvec; + Matrix right_Rt, left_Rt, delta_Rt; + Matrix right_Rt4 = Matrix4d::Identity(), + left_Rt4 = Matrix4d::Identity(), + delta_Rt4 = Matrix4d::Identity(), + left_Rt4i = Matrix4d::Identity(), + right_Rt4i = Matrix4d::Identity(); + + + Mat dist_coeffs = Mat::zeros(1,8,CV_32F); +//Mat rimage, limage; //resizing + Mat const rimage = camera_poses_[ridx].read(); //rimageBig = camera_poses_[ridx].read(); +//resize(rimageBig, rimage, Size(), 0.4, 0.4, INTER_LINEAR); + Mat const limage = camera_poses_[lidx].read(); +//resize(limageBig, limage, Size(), 0.4, 0.4, INTER_LINEAR); + + + Size imageSize = rimage.size(); + Mat roi= Mat::zeros(imageSize.height, imageSize.width, CV_8U), lroi, rroi, roirect; + + //TODO IMPLEMENT PARAMETER FROM TERMINAL + rectangle(roi, Point(15,15),Point(imageSize.width-15,imageSize.height-15),CV_RGB(255, 255, 255) ,CV_FILLED); + + rvec[0] = camera_poses_[ridx].roll; + rvec[1] = camera_poses_[ridx].pitch; + rvec[2] = camera_poses_[ridx].yaw; + + tvec[0] = camera_poses_[ridx].tx; + tvec[1] = camera_poses_[ridx].ty; + tvec[2] = camera_poses_[ridx].tz; + + + right_Rt = compose_Rt_matrix (rvec, tvec); + + rvec[0] = camera_poses_[lidx].roll; + rvec[1] = camera_poses_[lidx].pitch; + rvec[2] = camera_poses_[lidx].yaw; + + tvec[0] = camera_poses_[lidx].tx; + tvec[1] = camera_poses_[lidx].ty; + tvec[2] = camera_poses_[lidx].tz; + + + left_Rt = compose_Rt_matrix (rvec, tvec); + + right_Rt4.block<3,4>(0,0)= right_Rt; + left_Rt4.block<3,4>(0,0)= left_Rt; + + + left_Rt4i = left_Rt4.inverse(); + right_Rt4i = right_Rt4.inverse(); + + + delta_Rt4 = right_Rt4i * left_Rt4i.inverse(); + + + + + + Matrix3d eigR = delta_Rt4.block<3,3>(0,0); + Vector3d eigT = delta_Rt4.block<3,1>(0,3); + + + + Mat R1, R2, P1, P2, Q, cvR, cvT; + eigen2cv(eigR,cvR); + eigen2cv(eigT,cvT); + +//cvR=cvR*0.4; //rescaling +//cvT=cvT*0.4; +//cvCalibMatrix=cvCalibMatrix*0.4; + + Rect validRoi[2]; + stereoRectify (cvCalibMatrix, dist_coeffs, + cvCalibMatrix, dist_coeffs, + imageSize, + cvR, cvT, + R1,R2,P1,P2, Q, + CALIB_ZERO_DISPARITY, + -1, + imageSize, &validRoi[0], &validRoi[1]); + +//cout<< validRoi[0]<< endl << validRoi[1,1]<< endl; + + + // Mat * limageptr = limage; + + + + waitKey(0); + + + Mat maplx, maply, disp(imageSize.width, imageSize.height, CV_32F), disp8, disp_eq, disp_small; + cout<tvec[0]) + { + Mat rimagerectT, limagerectT; + transpose(rimagerect, rimagerectT); + transpose(limagerect, limagerectT); + limagerect=limagerectT; + rimagerect=rimagerectT; + } + + */ + + imshow("rimgrect", rimagerect); + imshow("limgrect", limagerect); + imwrite("rightrect.bmp",rimagerect); + imwrite("leftrect.bmp",limagerect); + + + + + int depthmapMethod=1; // 1: cpu sbgm mukodik mindennel + // 2: gpu bm + // 3: cpu bm + + int reprojection=3; //1: Opencv GPU reproject function + //2: Opencv CPU reprojection function + //3: Own reprojection function + + + + switch(depthmapMethod) + { + + + case 1: //cpu sgbm + { + //********************************************PARAMETERTUNER CPU SGBM ******************************************************* + //moreorless good parameters + int SADWindowSize = 1; //1 volt 16 al kevesebb hiba a 22-23 kepnel + int numDisparities = 6; //%16=0 (!!!) + int preFilterCap = 0; + int minDisparity = 0; + int uniquenessRatio= 5; + int speckleWindowSize = 0; + int speckleRange = 0; + int disp12MaxDiff = 0; + int fullDP = true; + int P11 = 8; + int P22 = 32; + + + namedWindow("CPU SGBM", 1); + + + + createTrackbar( "SADWindowSize ", "CPU SGBM", &SADWindowSize, 96, 0 ); + createTrackbar( "numDisparities *16", "CPU SGBM", &numDisparities, 16, 0 ); + createTrackbar( "preFilterCap", "CPU SGBM", &preFilterCap, 1000, 0 ); + createTrackbar( "minDisparity", "CPU SGBM", &minDisparity, 100, 0 ); + createTrackbar( "uniquenessRatio", "CPU SGBM", &uniquenessRatio, 15, 0 ); + createTrackbar( "speckleWindowSize", "CPU SGBM", &speckleWindowSize, 200, 0 ); + createTrackbar( "speckleRange", "CPU SGBM", &speckleRange, 10, 0 ); + createTrackbar( "disp12MaxDiff", "CPU SGBM", &disp12MaxDiff, 100, 0 ); + createTrackbar( "P1", "CPU SGBM", &P11, 128, 0 ); + createTrackbar( "P2", "CPU SGBM", &P22, 128, 0 ); + + while(1) + { + Ptr sgbm; + + sgbm = createStereoSGBM(minDisparity, numDisparities*16, SADWindowSize, + P11, P22, disp12MaxDiff, preFilterCap, uniquenessRatio, + speckleWindowSize, speckleRange, fullDP); + + sgbm->compute(limagerect, rimagerect, disp); + + + + char key=waitKey(0); + if(key == 27) + break; + + + double minVal, maxVal; + + //disp.convertTo( disp8, CV_8UC1, 255/(maxVal- minVal)); + //disp.convertTo(disp8, CV_8U); + cv::normalize(disp, disp8, 0, 255, NORM_MINMAX, CV_8U); + imwrite("disp_sgbm.bmp",disp8); + minMaxLoc( disp, &minVal, &maxVal ); + cout<<"min: "< gpubm; + gpubm = gpu::createStereoBM(48,31); + + if(BlockSize_GPU%2==0) //block size has to be odd + BlockSize_GPU++; + + gpubm->setNumDisparities(NumDisparities_GPU*16); + gpubm->setMinDisparity(MinDisparity_GPU); + gpubm->setBlockSize(BlockSize_GPU); + //gpubm->setSpeckleWindowSize(SpeckleWindowSize_GPU); + //gpubm->setSpeckleRange(SpeckleRange_GPU); + gpubm->setDisp12MaxDiff(Disp12MaxDiff_GPU); + //gpubm->setPreFilterType(); + gpubm->setPreFilterCap(PreFilterCap_GPU); + //gpubm->setTextureThreshold(); + gpubm->setUniquenessRatio(UniquenessRatio_GPU); + + + + gpubm->compute(d_left, d_right, d_disp); + + char key=waitKey(0); + if(key == 27) + break; + + if(imageSize.width>2000) + { + gpu::resize(d_disp, d_disp_small, Size(), 0.2, 0.2,INTER_LINEAR); + d_disp_small.download(disp); + + cv::normalize(disp, disp8, 0, 255, NORM_MINMAX, CV_8U); + double minVal, maxVal; + minMaxLoc( disp, &minVal, &maxVal ); + cout<<"min: "< cpubm; + cpubm = createStereoBM(0,21); + + if(BlockSize%2==0) //block size has to be odd + BlockSize++; + + cpubm->setNumDisparities(NumDisparities*16); + cpubm->setMinDisparity(MinDisparity); + cpubm->setBlockSize(BlockSize); + cpubm->setSpeckleWindowSize(SpeckleWindowSize); + cpubm->setSpeckleRange(SpeckleRange); + cpubm->setDisp12MaxDiff(Disp12MaxDiff); + //cpubm->setPreFilterType(); + cpubm->setPreFilterCap(PreFilterCap); + //cpubm->setTextureThreshold(); + cpubm->setUniquenessRatio(UniquenessRatio); + + +cout<compute(limagerect, rimagerect, disp); + + char key=waitKey(0); + if(key == 27) + break; + + //resize(disp, disp_small, Size(), 0.2, 0.2,INTER_LINEAR); + double minVal, maxVal; + + //disp.convertTo( disp8, CV_8UC1, 255/(maxVal- minVal)); + //disp.convertTo(disp8, CV_8U); + cv::normalize(disp, disp8, 0, 255, NORM_MINMAX, CV_8U); + //disp_small.convertTo(disp8, CV_8U, 1/8.); + minMaxLoc( disp, &minVal, &maxVal ); + cout<<"min: "<(i,j)==-16) +// { +// +// disp.at(i,j) = 0; //-16 means invalid pixel in disparity (not sure though) +// // disp8.at(i,j)=0; +// } +// +// } +// } + + + Mat _3dimage(disp.size(), CV_32FC3); //float type + + + //write disp to file to check type +FileStorage fs("disp.yaml", FileStorage::WRITE); +fs<<"disp"<(i,j)!=-16 && + !( + isinf(_3dimage.at(i,j)[0]) || //TODO only needed if using opencv reprojection function should move to the reprojection and set to 0 + isinf(_3dimage.at(i,j)[1]) || + isinf(_3dimage.at(i,j)[2]) + ) + ) + { + */ + break; + }//case 1 + + case 2: + { + reprojectImageTo3D(disp, _3dimage, Q); + break; + }//case 2 + + case 3: + { + + +cout<<"reprojecting..."< +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include + +#include "base2d/image.h" +#include +#include + +void rectify_images (const Eigen::Matrix3d& calib_matrix_, + const std::vector& camera_poses_, + int lidx, int ridx ); + + +#endif \ No newline at end of file diff --git a/src/densemap/reproject.cc b/src/densemap/reproject.cc new file mode 100644 index 0000000..7789759 --- /dev/null +++ b/src/densemap/reproject.cc @@ -0,0 +1,105 @@ +#include "reproject.h" + + +void filterDisparity(cv::Mat& disparity) +{ + + for (int i = 0; i < disparity.rows; i++) + { + for (int j = 0; j < disparity.cols; j++) + { + if(isnan(disparity.at(i,j))||disparity.at(i,j)<0) + disparity.at(i,j)=0; + + } + + } + + +}//filterDisparity + + + + + +void reprojectTo3d(const cv::Mat& Q, const cv::Mat& disparity, cv::Mat& _3dimage) +{ + + double Q03, Q13, Q23, Q32, Q33; + + Q03 = Q.at(0,3); + Q13 = Q.at(1,3); + Q23 = Q.at(2,3); + Q32 = Q.at(3,2); + Q33 = Q.at(3,3); + + + + double px, py, pz; + +//TODO check disparity type + + + for (int i = 0; i < disparity.rows; i++) + { + for (int j = 0; j < disparity.cols; j++) + { + short d= disparity.at(i,j); + if ( d == 0 ) continue; + double pw = 1.0 * static_cast(d) * Q32 + Q33; + px = static_cast(j) + Q03; + py = static_cast(i) + Q13; + pz = Q23; + + px = px/pw; + py = py/pw; + pz = pz/pw; + + //std::cout<(i,j)[0] = static_cast(px); + + + //cv::waitKey(0); + + + _3dimage.at(i,j)[1] = static_cast(py); + _3dimage.at(i,j)[2] = static_cast(pz); + + } + } + + + //std::cout<<_3dimage<(i,j)[0]<<", " + <<_3dimage.at(i,j)[1]<<", " + <<_3dimage.at(i,j)[2]<<"\n"; + } + } + pointcloud<<"\n] }}}\n"; + //pointcloud<<" color Color { color [\n"; + pointcloud.close(); +}//writeVRML \ No newline at end of file diff --git a/src/densemap/reproject.h b/src/densemap/reproject.h new file mode 100644 index 0000000..8ea4e32 --- /dev/null +++ b/src/densemap/reproject.h @@ -0,0 +1,62 @@ +#ifndef _REPROJECT_H +#define _REPROJECT_H + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "util/defs.h" + + + + + + +using namespace cv; + +/** + * @brief Removes non number values from disparity map like inf or -inf + * + * @param disparity the disparity map to be filterd + * @return void + **/ +void filterDisparity(cv::Mat& disparity); + +/** + * @brief Reprojects a disparitymap to 3d coordinates + * + * @param Q reprojection matrix computed by cv::stereorectify(); + * @param disparity disparitymap + * @param _3dimage 3d points + * @return void + **/ +void reprojectTo3d(const cv::Mat& Q, const cv::Mat& disparity, cv::Mat& _3dimage); + +/** + * @brief writes pointcloud to VRML file + * + * @param _3dimage points in X, Y, Z format + * @param imageSize size of the disparity image + * @return void + **/ +void writeVRML(const cv::Mat& _3dimage); + + +#endif + + + + diff --git a/src/densemapper.cc b/src/densemapper.cc new file mode 100644 index 0000000..4e22c06 --- /dev/null +++ b/src/densemapper.cc @@ -0,0 +1,558 @@ +#include +#include +#include +#include + +#include +#include + +#include + +#include "base2d/feature.h" +#include "base3d/bundle_adjustment.h" +#include "base3d/projection.h" +#include "fm/feature_management.h" +#include "sfm/sequential_mapper.h" +#include "util/io.h" +#include "util/timer.h" +#include "densemap/rectify.h" +#include "base2d/image.h" + + +#define LOCAL_BA_POSE_FREE 0 +#define LOCAL_BA_POSE_FIXED 1 +#define LOCAL_BA_POSE_FIXED_X 2 + +using namespace std; + + + +namespace config = boost::program_options; + +const double FINAL_COST_THRESHOLD = 1; +const double LOSS_SCALE_FACTOR = 1; +const double MIN_INLIER_STOP = 0.5; +const double MIN_INLIER_THRESHOLD = 20; +const double MAX_REPROJ_ERROR = 2; +const double MIN_INIT_DISPARITY = 100; +const double MIN_DISPARITY = 50; +const std::vector LOCAL_BA_POSE_STATE += boost::assign::list_of ( LOCAL_BA_POSE_FIXED ) + ( LOCAL_BA_POSE_FIXED ) + ( LOCAL_BA_POSE_FREE ) + ( LOCAL_BA_POSE_FREE ) + ( LOCAL_BA_POSE_FREE ) + ( LOCAL_BA_POSE_FREE ) + ( LOCAL_BA_POSE_FREE ) + ( LOCAL_BA_POSE_FREE ) + ( LOCAL_BA_POSE_FREE ) + ( LOCAL_BA_POSE_FREE ); + + +void print_report_heading ( const size_t image_idx, + const std::vector& image_data ) +{ + std::cout << std::endl; + std::cout << std::string ( 80, '=' ) << std::endl; + std::cout << "Processing image #" << image_idx + 1 + << " (" << image_data[image_idx].timestamp << ")" << std::endl; + std::cout << std::string ( 80, '=' ) << std::endl << std::endl; +} + + +void print_report_summary ( const size_t image_idx, + SequentialMapper& mapper ) +{ + + const size_t image_id = mapper.get_image_id ( image_idx ); + + Eigen::Matrix4d matrix = Eigen::MatrixXd::Identity ( 4, 4 ); + matrix.block<3, 4> ( 0, 0 ) + = compose_Rt_matrix ( mapper.feature_manager.rvecs[image_id], + mapper.feature_manager.tvecs[image_id] ); + matrix = matrix.inverse().eval(); + + const Eigen::Vector3d tvec = matrix.block<3, 1> ( 0, 3 ); + + std::cout << "Global position" << std::endl; + std::cout << "---------------" << std::endl; + std::cout << std::setw ( 15 ) + << tvec ( 0 ) << std::endl + << std::setw ( 15 ) + << tvec ( 1 ) << std::endl + << std::setw ( 15 ) + << tvec ( 2 ) << std::endl + << std::endl; + +} + + +void adjust_local_bundle ( SequentialMapper& mapper, + const std::list& image_idxs ) +{ + + if ( image_idxs.size() > LOCAL_BA_POSE_STATE.size() ) + { + throw std::range_error ( "Number of local images in `image_idxs` must not " + "be greater than number of poses defined in " + "`LOCAL_BA_POSE_STATE`." ); + } + + std::vector free_image_idxs; + std::vector fixed_image_idxs; + std::vector fixed_x_image_idxs; + + // Set parameters of image poses as constant or variable + size_t i=0; + for ( std::list::const_iterator it=image_idxs.begin(); + it!=image_idxs.end(); it++, i++ ) + { + switch ( LOCAL_BA_POSE_STATE[i] ) + { + case LOCAL_BA_POSE_FREE: + free_image_idxs.push_back ( *it ); + break; + case LOCAL_BA_POSE_FIXED: + fixed_image_idxs.push_back ( *it ); + break; + case LOCAL_BA_POSE_FIXED_X: + fixed_x_image_idxs.push_back ( *it ); + break; + } + } + + mapper.adjust_bundle ( free_image_idxs, fixed_image_idxs, fixed_x_image_idxs, + LOSS_SCALE_FACTOR, + false, // update_cov + true, // print_summary + false // print_progress + ); + +} + + +void adjust_global_bundle ( SequentialMapper& mapper, + const size_t start_image_idx, + const size_t end_image_idx ) +{ + + Timer timer; + timer.start(); + + // Globally adjust all poses with fixed initial two poses + + std::vector free_image_idxs; + std::vector fixed_image_idxs; + std::vector fixed_x_image_idxs; + + // adjust first two poses as fixed and with fixed x-coordinate + fixed_image_idxs.push_back ( mapper.get_first_image_idx() ); + fixed_x_image_idxs.push_back ( mapper.get_second_image_idx() ); + + // All other poses as free + for ( size_t image_idx=start_image_idx; + image_idx<=end_image_idx; image_idx++ ) + if ( mapper.is_image_processed ( image_idx ) + && image_idx != mapper.get_first_image_idx() + && image_idx != mapper.get_second_image_idx() ) + free_image_idxs.push_back ( image_idx ); + + std::cout << std::endl; + std::cout << std::string ( 80, '=' ) << std::endl; + std::cout << "Global bundle adjustment" << std::endl; + std::cout << std::string ( 80, '=' ) << std::endl << std::endl; + + mapper.adjust_bundle ( free_image_idxs, fixed_image_idxs, fixed_x_image_idxs, + LOSS_SCALE_FACTOR, + true, // update_cov + true, // print_summary + true // print_progress + ); + + timer.stop(); + timer.print(); + +} + + +void process_remaining_images ( SequentialMapper& mapper, + const size_t start_image_idx, + const size_t end_image_idx ) +{ + + Timer timer; + + for ( size_t image_idx=start_image_idx+1; + image_idx ( &input_path )->required(), + "path to imagedata.txt, calibrationdata.txt and image files" ) + ( "output-path,o", + config::value ( &output_path )->required(), + "path to output files" ) + ( "cache-path,o", + config::value ( &cache_path )->required(), + "path to cache files" ) + ( "image-prefix", + config::value ( &image_prefix )->default_value ( "" ), + "prefix of image file names before timestamp" ) + ( "image-suffix", + config::value ( &image_suffix )->default_value ( "" ), + "suffix of image file names after timestamp" ) + ( "image-ext", + config::value ( &image_ext )->default_value ( ".bmp" ), + "image file name extension" ) + ( "start-image-idx", + config::value ( &start_image_idx )->default_value ( 0 ), + "index of first image to be processed (position in image data file)" ) + ( "end-image-idx", + config::value ( &end_image_idx )->default_value ( UINT_MAX ), + "index of last image to be processed (position in image data file)" ) + ( "debug", + config::bool_switch ( &debug )->default_value ( false ), + "change to debug mode" ) + ( "debug-path", + config::value ( &debug_path )->default_value ( "" ), + "path to debug output files" ) + ( "debug-delay", + config::value ( &debug_delay )->default_value ( 0 ), + "delay in debug mode for visualizing feature matches (0 for wait key," + "otherwise in [ms])" ); + config::variables_map variables_map; + config::store ( config::parse_command_line ( argc, argv, desc ), + variables_map ); + variables_map.notify(); + + if ( variables_map.count ( "help" ) ) + { + std::cout << desc << std::endl; + return 1; + } + + // Add trailing slash from input_path + if ( output_path.at ( output_path.length()-1 ) != '/' ) + { + output_path += "/"; + } + + } + catch ( std::exception& e ) + { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + catch ( ... ) + { + std::cerr << "Unknown error!" << "\n"; + return 1; + } + + // Read input data + + std::vector image_data + = read_image_data ( input_path + "imagedata.txt", input_path, + image_prefix, image_suffix, image_ext ); + Eigen::Matrix3d calib_matrix + = read_calib_matrix ( input_path + "calibrationdata.txt" ); + + //Read Poses !!!!!!!!nezd meg meg nem az outputot olvassa be + std::vector camera_poses + = read_image_data ( output_path + "imagedataout.txt", input_path, + image_prefix, image_suffix, image_ext ); + + + rectify_images(calib_matrix ,camera_poses, 33, 34); //22,23 //29,30 + + + + + + /* + SequentialMapper mapper ( image_data, calib_matrix, + cache_path, debug, debug_path, debug_delay ); + + + + // Last image to be processed + end_image_idx = std::min ( end_image_idx, image_data.size() - 1 ); + + // Dynamically adjusted minimum track length for 2D-3D pose estimation + size_t min_track_len = 2; + + Timer timer; + + size_t image_idx = -1; + size_t prev_image_idx = -1; + size_t prev_prev_image_idx = -1; + size_t prev_prev_prev_image_idx = -1; + + // Image list for local bundle adjustment + std::list ba_image_idxs; + + for ( image_idx=start_image_idx; image_idx MIN_INIT_DISPARITY ) + { + break; + } + } + + mapper.process_initial ( start_image_idx, image_idx ); + + print_report_summary ( start_image_idx, mapper ); + + print_report_heading ( image_idx, image_data ); + print_report_summary ( image_idx, mapper ); + + // Adjust first two poses + std::vector free_image_idxs; + std::vector fixed_image_idxs; + std::vector fixed_x_image_idxs; + fixed_image_idxs.push_back ( start_image_idx ); + fixed_x_image_idxs.push_back ( image_idx ); + mapper.adjust_bundle ( free_image_idxs, + fixed_image_idxs, + fixed_x_image_idxs ); + + // Add to local bundle adjustment + ba_image_idxs.push_back ( mapper.get_first_image_idx() ); + ba_image_idxs.push_back ( mapper.get_second_image_idx() ); + + } + else if ( mapper.get_num_proc_images() >= 2 ) + { + + // Increase minimum track length for 2D-3D pose estimation + if ( mapper.get_num_proc_images() > 4 ) + { + min_track_len = 3; + } + else if ( mapper.get_num_proc_images() > 6 ) + { + min_track_len = 4; + } + + // try to process image + bool success; + while ( ! ( success = mapper.process ( image_idx, prev_image_idx, + FINAL_COST_THRESHOLD, + LOSS_SCALE_FACTOR, + MIN_INLIER_STOP, + MIN_INLIER_THRESHOLD, + MAX_REPROJ_ERROR, + MIN_DISPARITY, + min_track_len ) ) ) + { + timer.stop(); + timer.print(); + timer.restart(); + image_idx++; + if ( image_idx >= end_image_idx ) + { + break; + } + print_report_heading ( image_idx, image_data ); + } + if ( !success ) + { + break; + } + + // if (prev_prev_image_idx != -1) { + // mapper.process(image_idx, prev_prev_image_idx, FINAL_COST_THRESHOLD, + // LOSS_SCALE_FACTOR, MIN_INLIER_STOP, + // MIN_INLIER_THRESHOLD, MAX_REPROJ_ERROR, + // 0, min_track_len); + // } + // if (prev_prev_prev_image_idx != -1) { + // mapper.process(image_idx, prev_prev_prev_image_idx, + // FINAL_COST_THRESHOLD, LOSS_SCALE_FACTOR, + // MIN_INLIER_STOP, MIN_INLIER_THRESHOLD, + // MAX_REPROJ_ERROR, 0, min_track_len); + // } + + // Adjust local bundle + + if ( ba_image_idxs.size() == LOCAL_BA_POSE_STATE.size() ) + { + ba_image_idxs.pop_front(); + } + ba_image_idxs.push_back ( image_idx ); + + adjust_local_bundle ( mapper, ba_image_idxs ); + + } + + print_report_summary ( image_idx, mapper ); + + timer.stop(); + timer.print(); + + prev_prev_prev_image_idx = prev_prev_image_idx; + prev_prev_image_idx = prev_image_idx; + prev_image_idx = image_idx; + + } + + // Process all skipped frames + process_remaining_images ( mapper, start_image_idx, end_image_idx ); + + // for (size_t i=start_image_idx; i