diff --git a/README.md b/README.md index a45e160..4cfcd80 100644 --- a/README.md +++ b/README.md @@ -63,3 +63,6 @@ If you get librealsense2.so is missing for pangolin, clear build and rebuild Pan > $ cmake .. > $ make > $ sudo make install + +### Camera poses estimated for XYZ FR1 TUM data +![Demo run](./docs/full-run.gif) diff --git a/docs/final_report.pdf b/docs/final_report.pdf new file mode 100644 index 0000000..2951659 Binary files /dev/null and b/docs/final_report.pdf differ diff --git a/docs/full-run.gif b/docs/full-run.gif new file mode 100644 index 0000000..92a8900 Binary files /dev/null and b/docs/full-run.gif differ diff --git a/docs/proposal.pdf b/docs/proposal.pdf new file mode 100644 index 0000000..d3c6c83 Binary files /dev/null and b/docs/proposal.pdf differ diff --git a/docs/reference.pdf b/docs/reference.pdf new file mode 100644 index 0000000..2b6ff06 Binary files /dev/null and b/docs/reference.pdf differ diff --git a/include/frame.h b/include/frame.h index baede76..a776e9a 100644 --- a/include/frame.h +++ b/include/frame.h @@ -32,6 +32,10 @@ struct RootInvCovAll{ std::vector> cov_matrices; }; +struct system_poses{ + std::vector relative_poses; + std::vector global_poses; +}; class FramePair{ /* @@ -78,10 +82,17 @@ class FramePair{ // EigenVectors of cov matrices [For ransac] std::vector> cov_eig_vectors_im1, cov_eig_vectors_im2; + + // covariance of endpoints of optimized matrix + std::vector> line1_endPt_covs, line2_endPt_covs; // Ransac object for culling outliers Ransac *pointRefine; + // Optimized Rot and trans + Eigen::Vector3d t_optim; + Eigen::Matrix3d R_optim; + // Line Sampler in 2D void SampleIndices(const Eigen::MatrixXi& lines, std::vector& sampled_lines_2d); @@ -98,6 +109,5 @@ class FramePair{ // ctor which populates members inside FramePair(const cv::Mat& rgb_image1, cv::Mat& depth_image1, cv::Mat& rgb_image2, cv::Mat& depth_image2); - }; diff --git a/include/optim.h b/include/optim.h index fe48820..a574743 100644 --- a/include/optim.h +++ b/include/optim.h @@ -18,22 +18,64 @@ namespace optim { -struct RootInvCov{ - int idx1, idx2; - std::vector cov_matrices; -}; + class LineData{ + public: + Eigen::Vector3d A, B, u, d; + + LineData(Eigen::Vector3d startPoint, Eigen::Vector3d endPoint): A(startPoint), B(endPoint){ + Eigen::Vector3d l = B - A; + Eigen::Vector3d m = (A + B)/2; -void compute_residual(double *points, double *error, int m, int n, void *data); + u = l/l.norm(); + d = (l.cross(m))/l.norm(); + } + }; -points3d nonlinOptimize(points3d& line3D, std::vector& inv_cov_one_line, std::vector& covariance_matrices, std::vector& endPt_covs, int line_idx1, int line_idx2); + struct OptimizedLinesWithCov{ + const std::vector *l1, *l2; + const std::vector> *l1_cov, *l2_cov; + const std::vector *matches; + }; + + struct RootInvCov{ + int idx1, idx2; + std::vector cov_matrices; + }; -double m_dist(const Eigen::Vector3d& X, const Eigen::Matrix3d& sigma_x, const Eigen::Vector3d& A, const Eigen::Vector3d& B); + void compute_residual(double *points, double *error, int m, int n, void *data); -double computeRtError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, - const Eigen::Vector3d& A1, const Eigen::Vector3d& B1, - const Eigen::Matrix3d& cov_A1, const Eigen::Matrix3d& cov_B1, - const Eigen::Vector3d& A2, const Eigen::Vector3d& B2, - const Eigen::Matrix3d& cov_A2, const Eigen::Matrix3d& cov_B2 - ); + points3d nonlinOptimize(points3d& line3D, std::vector& inv_cov_one_line, std::vector& covariance_matrices, std::vector>& endPt_covs, int line_idx1, int line_idx2); + + double m_dist(const Eigen::Vector3d& X, const Eigen::Matrix3d& sigma_x, const Eigen::Vector3d& A, const Eigen::Vector3d& B); + + double computeRtError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, + const Eigen::Vector3d& A1, const Eigen::Vector3d& B1, + const Eigen::Matrix3d& cov_A1, const Eigen::Matrix3d& cov_B1, + const Eigen::Vector3d& A2, const Eigen::Vector3d& B2, + const Eigen::Matrix3d& cov_A2, const Eigen::Matrix3d& cov_B2 + ); + + + // Get anti-symmetric matrix given a vector + Eigen::Matrix3d GetCrossMatrix(const Eigen::Vector3d& vec); + + // Computing rotation matrix as per the reference paper + Eigen::Matrix3d ComputeRotationMatrix(const LineData& l1, const LineData& l1_prime, const LineData& l2, const LineData& l2_prime); + + // Computing translation as per the reference paper + Eigen::Vector3d ComputeTranslation(std::vector& im1_lines, std::vector& im2_lines, Eigen::Matrix3d& R); + + double computeRtError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, const Eigen::Vector3d& A1, + const Eigen::Vector3d& B1, const Eigen::Vector3d& A2, const Eigen::Vector3d& B2); + + double computeDistError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, const Eigen::Vector3d& X, + const Eigen::Vector3d& A, const Eigen::Vector3d& B); + + void computeRotTransResidual(double *params, double *residuals, int n_params, int n_meas, void *data); + + void optimizeRotTrans(Eigen::Matrix3d& R, Eigen::Vector3d& t, const std::vector& im1_lines, + const std::vector& im2_lines, const std::vector& matches, + const std::vector>& im1_line_cov, + const std::vector>& im2_line_cov, Eigen::Matrix3d& R_optim, Eigen::Vector3d& t_optim); } diff --git a/include/ransac.h b/include/ransac.h index 26d95fe..c5d2932 100644 --- a/include/ransac.h +++ b/include/ransac.h @@ -10,10 +10,12 @@ Date: 28 Nov 2020 */ #pragma once +// #include "utils.h" #include typedef Eigen::Vector3d point3d; typedef Eigen::Matrix3d covariance; +typedef Eigen::Matrix points3d; class Ransac{ public: @@ -35,4 +37,7 @@ class Ransac{ std::vector& updated_inv_root_covariance, const std::vector& cov_G, const std::vector& inv_cov_G); + std::vector ransac3D(const std::vector& optLines1, const std::vector& optLines2, const std::vector>& endPtCov1, + const std::vector> endPtCov2, Eigen::Matrix3d& R_best, Eigen::Vector3d& t_best); + }; diff --git a/include/utils.h b/include/utils.h index 2c6eda6..1d2d2d9 100644 --- a/include/utils.h +++ b/include/utils.h @@ -43,7 +43,7 @@ namespace utils{ void DrawLines3D(const std::vector>& lines); // Draw camera using a rectangular pyramid - void DrawSingleCamera(const cv::Mat& camera_pose, const float& w=1.0, const float& h_ratio=0.75, const float& z_ratio=0.6); + void DrawSingleCamera(const cv::Mat& camera_pose, const float& w=0.1, const float& h_ratio=0.75, const float& z_ratio=0.6); // Draw points in 3D // Add default color information if no color is passed @@ -57,4 +57,8 @@ namespace utils{ // This draws the world coordinate frame at the world origin void DrawCoordinates(); + + // Helper to make 4d transformation matrix from R and t + cv::Mat MakeCameraPose(const Eigen::Matrix3d& R, const Eigen::Vector3d& t); + } diff --git a/include/viewer.h b/include/viewer.h index b297a28..fba273e 100644 --- a/include/viewer.h +++ b/include/viewer.h @@ -18,7 +18,7 @@ Date: 19 Nov 2020 class viewer{ public: // constructor for the viewer - viewer(const std::string& window_name); + viewer(const std::string& window_name, struct system_poses* ptr_to_poses); // main render loop void run(); @@ -33,11 +33,17 @@ class viewer{ // Mutex to guard data that needs to be plotted (current_frame) std::mutex viewerVarsMtx; + + // Mutex to guard the poses data + std::mutex viewerPosesMtx; private: // Pointer to frame whose data we want to plot FramePair* current_frame; + // Pointer to the system of poses + struct system_poses* camera_poses; + // To close viewer at end of processing bool stopViewer; std::mutex stopViewerMtx; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index eef5f3c..80635e4 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -87,5 +87,8 @@ target_link_libraries(main PUBLIC levmar ${PROJECT_NAME} ) # add_executable(levmar_test ${CMAKE_CURRENT_SOURCE_DIR}/levmar_test.cpp) target_link_libraries(main PUBLIC ${PROJECT_NAME} ) -# add_executable(test_ransac ${CMAKE_CURRENT_SOURCE_DIR}/test_ransac.cpp) -# target_link_libraries(test_ransac PUBLIC ${PROJECT_NAME} ) +add_executable(test_viewer ${CMAKE_CURRENT_SOURCE_DIR}/test_viewer.cpp) +target_link_libraries(test_viewer PUBLIC ${PROJECT_NAME} ) + +add_executable(test_rotation ${CMAKE_CURRENT_SOURCE_DIR}/test_rotation.cpp) +target_link_libraries(test_rotation PUBLIC ${PROJECT_NAME} ) diff --git a/src/frame.cpp b/src/frame.cpp index 57e9394..e0fd1ce 100644 --- a/src/frame.cpp +++ b/src/frame.cpp @@ -271,26 +271,33 @@ FramePair::FramePair(const cv::Mat& rgb_image1, cv::Mat& depth_image1, cv::Mat& std::cout << "Starting Im1" << std::endl; int Inp; - std::vector line1_endPt_covs, line2_endPt_covs; - for(int i = 0; i < rsac_points_3d_im1.size(); i++){ // std::cout << rsac_points_3d_im1[i].cols() << " " << im1_data.cov_matrices[i].size() << std::endl; points3d optimized_line1 = optim::nonlinOptimize(rsac_points_3d_im1[i], im1_data.cov_matrices[i], cov_G_im1[i], line1_endPt_covs, 0, im1_data.cov_matrices[i].size()-1); optimized_lines_im1.push_back(optimized_line1); // std::cin >> Inp; } + // std::cout << line1_endPt_covs[0][0] << " \n " << line1_endPt_covs[0][1] << std::endl; std::cout << "Starting Im2" << std::endl; for(int i = 0; i < rsac_points_3d_im2.size(); i++){ points3d optimized_line2 = optim::nonlinOptimize(rsac_points_3d_im2[i], im2_data.cov_matrices[i], cov_G_im2[i], line2_endPt_covs, 0, im2_data.cov_matrices[i].size()-1); optimized_lines_im2.push_back(optimized_line2); } - std::cout << "Matrix Size" << line1_endPt_covs.size() << " " << line2_endPt_covs.size() << std::endl; + // std::cin >> inp; + Eigen::Vector3d t_best; + Eigen::Matrix3d R_best; + std::vector inlier_indices = pointRefine->ransac3D(optimized_lines_im1, optimized_lines_im2, line1_endPt_covs, line2_endPt_covs, R_best, t_best); + cout << "Ransac over: best initial estimates are: \n" << R_best << "\n" << t_best << endl; + // Optimize best_R and best_t + optim::optimizeRotTrans(R_best, t_best, optimized_lines_im1, optimized_lines_im2, inlier_indices, + line1_endPt_covs, line2_endPt_covs, R_optim, t_optim); + cout << "I have come out of optimization" << endl; + + // get Motion12 from Motion21 + R_optim.transposeInPlace(); + t_optim = -R_optim*t_optim; } - - // points3d FramePair::OptimizeFrames(){ - - // } diff --git a/src/main.cpp b/src/main.cpp index 4045a42..672ebab 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -16,6 +16,7 @@ This runs the full visual odometry system. #include #include #include +#include // Custom headers #include "frame.h" @@ -35,13 +36,23 @@ int main(int argc, char* argv[]) // Loading the data (TUMfr1) string data_dir = "../data/rgbd_dataset_freiburg1_xyz/"; string synched_file = "synched_data.txt"; + string output_file = "output_poses_full_run.txt"; - string window_name = "ROBust LINE Visual Odometry";; - viewer* robline_viewer = new viewer(window_name); + string window_name = "ROBust LINE Visual Odometry"; + struct system_poses *robline = new struct system_poses; + + // ugly, bad, very very bad - fix later [Why? Don't want vector to be empty. So populate here instead of first image] + cv::Mat world_pose = cv::Mat::eye(4, 4, CV_64F); + robline->relative_poses.push_back(world_pose); + robline->global_poses.push_back(world_pose); + + viewer* robline_viewer = new viewer(window_name, robline); + std::thread render_thread(&viewer::run, robline_viewer); string image_pair_path, rgb_path, depth_path, rgb_time, depth_time; ifstream infile(data_dir + synched_file); + ofstream outfile(output_file); bool isFirst = true; cv::Mat previous_rgb, previous_depth; @@ -50,13 +61,19 @@ int main(int argc, char* argv[]) // Each object contains lines found in both images and matches between the frames std::vector pairs; + int i=0; while(getline(infile, image_pair_path)){ + + // Sample every 4 frames + // if(i++%1) continue; + + // if(i > 100) break; + std::stringstream linestream(image_pair_path); linestream >> rgb_time >> rgb_path >> depth_time >> depth_path; cv::Mat rgb_img = utils::ReadImage(data_dir + rgb_path, true); cv::Mat depth_img = utils::ReadImage(data_dir + depth_path, false); - if (isFirst) { previous_rgb = rgb_img; @@ -66,24 +83,59 @@ int main(int argc, char* argv[]) else { + cout << "Starting pair with im2_rgb: " << data_dir + rgb_path << endl; // Detect lines, match lines between frames, sample 2d line, reproject to 3d, remove outliers FramePair* fpair = new FramePair(previous_rgb, previous_depth, rgb_img, depth_img); // pairs.push_back(fpair); + + cout << "Frame pair has been successfully created" << endl; utils::DrawSampledLines2D(fpair->rgb_image1, fpair->sampled_lines_2d_im1); - utils::DrawSampledLines2D(fpair->depth_image1, fpair->sampled_lines_2d_im1); + // utils::DrawSampledLines2D(fpair->depth_image1, fpair->sampled_lines_2d_im1); utils::DisplayImage(fpair->rgb_image1); + + cout << "Have asked the 2d sampled drawing function" << endl; + + // Update system poses + cv::Mat current_camera_pose = utils::MakeCameraPose(fpair->R_optim, fpair->t_optim); + if(fpair->t_optim.lpNorm() > 0.4) continue; + cv::Mat current_global_pose = current_camera_pose*robline->global_poses.back(); + + // Lock viewer mtx + std::unique_lock curPoseLock(robline_viewer->viewerPosesMtx); + robline->relative_poses.push_back(current_camera_pose); + robline->global_poses.push_back(current_global_pose); + curPoseLock.unlock(); + // Unlock viewer mtx + // Update the current vieweing frame robline_viewer->updateCurrentFrame(fpair); - cv::waitKey(10); + cout << "Updated the viewer content!" << endl; + + cv::waitKey(0); // Update previous frame previous_rgb = rgb_img; previous_depth = depth_img; + cout << "updated prev images" << endl; + + // generate required eigen data for saving output + Eigen::Matrix4d output_Rt; + cv::cv2eigen(current_global_pose, output_Rt); + cout << output_Rt << endl; + cout << current_global_pose << endl; + Eigen::Quaterniond q(output_Rt.block<3,3>(0,0).transpose()); + + // write to output file + outfile << rgb_time << " " << output_Rt.block<1,3>(3,0) << " " << + q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; } } + // close the output file + outfile.close(); + // close file infile.close(); diff --git a/src/optim.cpp b/src/optim.cpp index 8e7f896..15a8d6d 100644 --- a/src/optim.cpp +++ b/src/optim.cpp @@ -1,5 +1,7 @@ -#include "optim.h" +#include +#include "optim.h" +#include namespace optim{ void compute_residual(double *points, double *error, int m, int n, void *data){ @@ -52,7 +54,7 @@ namespace optim{ } - points3d nonlinOptimize(points3d& line3D, std::vector& inv_cov_one_line, std::vector& covariance_matrices, std::vector& endPt_covs, int line_idx1, int line_idx2){ + points3d nonlinOptimize(points3d& line3D, std::vector& inv_cov_one_line, std::vector& covariance_matrices, std::vector>& endPt_covs, int line_idx1, int line_idx2){ // std::cout << "Check pt 1" << std::endl; std::vector param_vector; std::vector ref_vector; @@ -116,8 +118,8 @@ namespace optim{ double opts[LM_OPTS_SZ], info[LM_INFO_SZ]; opts[0] = LM_INIT_MU; // - opts[1] = 1E-15; // gradient threshold, original 1e-15 - opts[2] = 1E-50; // relative para change threshold? original 1e-50 + opts[1] = 1E-10; // gradient threshold, original 1e-15 + opts[2] = 1E-20; // relative para change threshold? original 1e-50 opts[3] = 1E-20; // error threshold (below it, stop) opts[4] = LM_DIFF_DELTA; @@ -220,17 +222,42 @@ namespace optim{ col = col + 3; } } - + int inp; int size = covariance_matrices.size(); - Eigen::MatrixXd temp(3*size, 3*size); + Eigen::MatrixXd temp = Eigen::MatrixXd::Zero(3*size, 3*size); + // Eigen::MatrixXd temp2 = Eigen::MatrixXd::Zero(3*size, 3*size); for (int i = 0; i < size; i++) { - temp.block(3*i, 3*i, 3, 3) = covariance_matrices[i]; + temp.block(3*i, 3*i, 3, 3) = inv_cov_one_line[i].transpose()*inv_cov_one_line[i]; + // temp2.block(3*i, 3*i, 3, 3) = covariance_matrices[i]; + // if (i == 0) + // { + // std::cout << numPara << std::endl; + // std::cout << "Inverse cov \n" << inv_cov_one_line[i].transpose()*inv_cov_one_line[i] << "\n" << "Cov matrix \n" << covariance_matrices[i] << std::endl; + + // std::cout << Jac.bottomRightCorner(3,3) << std::endl; + // // for(long int k = 0; k < 100000000000; k++) + // // continue; + // } } + // std::ofstream file("test.txt"); + // if (file.is_open()) + // { + // // MatrixXf m = MatrixXf::Random(30,3); + // file << "Here is the matrix m:\n" << temp*temp2 << '\n'; + // // file << "m" << '\n' << colm(m) << '\n'; + // } + // file.close(); + // std::cin >> inp; + + Eigen::MatrixXd estimated_cov = (Jac*temp*Jac.transpose()).inverse(); + + std::vector temp_vector; + temp_vector.push_back(estimated_cov.topLeftCorner(3,3)); + temp_vector.push_back(estimated_cov.bottomRightCorner(3,3)); - Eigen::MatrixXd estimated_cov = Jac*temp.inverse()*Jac.transpose(); - endPt_covs.push_back(estimated_cov.topLeftCorner(3,3)); - endPt_covs.push_back(estimated_cov.bottomRightCorner(3,3)); + endPt_covs.push_back(temp_vector); + temp_vector.clear(); // std::cout << Jac << std::endl; delete[] para; delete[] ref; @@ -273,8 +300,195 @@ namespace optim{ r[1] = m_dist(R.transpose()*A2 - R.transpose()*t, R.transpose()*cov_A2*R, A1, B1); r[2] = m_dist(R*B1+t, R*cov_B1*(R.transpose()), A2, B2); r[3] = m_dist(R.transpose()*B2 - R.transpose()*t, R.transpose()*cov_B2*R, A1, B1); - + // std::cout << r[0]*r[0] << " " << r[1]*r[1] << " " << r[2]*r[2] << " " << r[3]*r[3] << "\n"; return r[0]*r[0] + r[1]*r[1] + r[2]*r[2] + r[3]*r[3]; } + + void computeRotTransResidual(double *params, double *residuals, int n_params, int n_meas, void *data) + { + // Unpack parameters into rotation and translation + Eigen::Vector3d rot_vec, t; + rot_vec << params[0], params[1], params[2]; + t << params[3], params[4], params[5]; + Eigen::AngleAxisd rotation_rodriguez(rot_vec.norm(), rot_vec.normalized()); + Eigen::Matrix3d R = rotation_rodriguez.toRotationMatrix(); + + // Ensure data gets accessed by the right pointer + struct OptimizedLinesWithCov *line_data = (struct OptimizedLinesWithCov*)data; + + int residual_index = 0; + // Iterate through matches to compute residual + for(int i: *(line_data->matches)) + { + Eigen::Vector3d A1, B1, A2, B2; + Eigen::Matrix3d cov_A1, cov_B1, cov_A2, cov_B2; + + A1 = (*(line_data->l1))[i].col(0); + B1 = (*(line_data->l1))[i].rightCols(1); + A2 = (*(line_data->l2))[i].col(0); + B2 = (*(line_data->l2))[i].rightCols(1); + + cov_A1 = (*(line_data->l1_cov))[i][0]; + cov_B1 = (*(line_data->l1_cov))[i][1]; + cov_A2 = (*(line_data->l2_cov))[i][0]; + cov_B2 = (*(line_data->l2_cov))[i][1]; + + residuals[residual_index] = m_dist(R*A1 + t, R*cov_A1*(R.transpose()), A2, B2); + residuals[residual_index+1] = m_dist(R.transpose()*A2 - R.transpose()*t, R.transpose()*cov_A2*R, A1, B1); + residuals[residual_index+2] = m_dist(R*B1+t, R*cov_B1*(R.transpose()), A2, B2); + residuals[residual_index+3] = m_dist(R.transpose()*B2 - R.transpose()*t, R.transpose()*cov_B2*R, A1, B1); + + residual_index += 4; + } + + } + + void optimizeRotTrans(Eigen::Matrix3d& R, Eigen::Vector3d& t, const std::vector& im1_lines, + const std::vector& im2_lines, const std::vector& matches, + const std::vector>& im1_line_cov, const std::vector>& im2_line_cov, + Eigen::Matrix3d& R_optim, Eigen::Vector3d& t_optim) + { + const int n_params = 6; // rotation + translation parameters + int n_meas = 4*matches.size(); // each match has a 4 part residual + + const int n_iters = 1000; + + double opts[LM_OPTS_SZ], info[LM_INFO_SZ]; + + opts[0] = LM_INIT_MU; + opts[1] = 1E-15; // gradient threshold, original 1e-15 + opts[2] = 1E-50; // relative para change threshold? original 1e-50 + opts[3] = 1E-20; // error threshold (below it, stop) + opts[4] = LM_DIFF_DELTA; + + // Pack required information into data - start with converting rotation into rodrigues + Eigen::AngleAxisd rodrigues_rotation(R); + Eigen::Matrix param_vector; + param_vector << rodrigues_rotation.angle()*rodrigues_rotation.axis(), t; // copies data over + double *optim_params = new double[6]; + optim_params = param_vector.data(); + + // populate target error i.e, zero + Eigen::Matrix meas_vector = Eigen::MatrixXd::Zero(n_meas, 1); + double *optim_meas = new double[n_meas]; + optim_meas = meas_vector.data(); + + // Pack in additional data + struct OptimizedLinesWithCov line_data; + line_data.l1 = &im1_lines; + line_data.l2 = &im2_lines; + line_data.l1_cov = &im1_line_cov; + line_data.l2_cov = &im2_line_cov; + line_data.matches = &matches; + + // Check pre optimization error + double *err = new double[n_meas]; + computeRotTransResidual(optim_params, err, n_params, n_meas, (void*)&line_data); + double net_error = 0; + for (int i=0; i < n_meas; ++i) net_error += err[i]; + cout << "Error before optimization: " << net_error << endl; + + + // Run Levenberg-Marquardt + int ret = dlevmar_dif(computeRotTransResidual, optim_params, optim_meas, n_params, n_meas, + n_iters, opts, info, NULL, NULL, (void*)&line_data); + + // Check post optimization error + computeRotTransResidual(optim_params, err, n_params, n_meas, (void*)&line_data); + net_error = 0; + for (int i=0; i < n_meas; ++i) net_error += err[i]; + cout << "Error after optimization: " << net_error << endl; + + // Unpack rotation and translation + Eigen::Vector3d rot_vec; + rot_vec << optim_params[0], optim_params[1], optim_params[2]; + t_optim << optim_params[3], optim_params[4], optim_params[5]; + Eigen::AngleAxisd rotation_rodriguez(rot_vec.norm(), rot_vec.normalized()); + R_optim = rotation_rodriguez.toRotationMatrix(); + cout << "R after optimization: \n" << R_optim << endl; + cout << "t after optimization: \n" << t_optim << endl; + + } + double computeDistError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, const Eigen::Vector3d& X, + const Eigen::Vector3d& A, const Eigen::Vector3d& B) + { + Eigen::Vector3d transformed_X = R*X + t; + Eigen::Vector3d direction = B-A; + double dist = ((transformed_X - A).cross(direction)).norm()/direction.norm(); + return dist; + } + + double computeRtError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, const Eigen::Vector3d& A1, + const Eigen::Vector3d& B1, const Eigen::Vector3d& A2, const Eigen::Vector3d& B2) + { + double err1 = computeDistError(R, t, A1, A2, B2); + double err2 = computeDistError(R, t, B1, A2, B2); + + return (err1 + err2)*0.5; + } + + Eigen::Matrix3d GetCrossMatrix(const Eigen::Vector3d& vec){ + Eigen::Matrix3d vec_hat = (Eigen::Matrix()<< 0, -vec(2), vec(1), + vec(2), 0, -vec(0), + -vec(1), vec(0), 0).finished(); + return vec_hat; + } + + Eigen::Matrix3d ComputeRotationMatrix(const LineData& l1, const LineData& l1_prime, const LineData& l2, const LineData& l2_prime){ + Eigen::Matrix4d A1, A2; + + // Constructing the matrix as per reference paper + A1(0,0) = 0; + A1.block(1,1,3,3) = optim::GetCrossMatrix(l1.u + l1_prime.u); + A1.bottomLeftCorner(3,1) = -(l1.u - l1_prime.u); + A1.topRightCorner(1,3) = (l1.u - l1_prime.u).transpose(); + + // std::cout << A1 << "\n" << std::endl; + + A2(0,0) = 0; + A2.block(1, 1, 3, 3) = optim::GetCrossMatrix(l2.u + l2_prime.u); + A2.bottomLeftCorner(3,1) = -(l2.u - l2_prime.u); + A2.topRightCorner(1,3) = (l2.u - l2_prime.u).transpose(); + + // std::cout << A1.transpose()*A1 + A2.transpose()*A2 << "\n" << "\n"; + // Computing the SVD of the sum of the matrices + Eigen::JacobiSVD svd(A1.transpose()*A1 + A2.transpose()*A2, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix4d U = svd.matrixU(); + Eigen::Matrix4d V = svd.matrixV(); + + // std::cout << "Matrix Product \n" << U << "\n" << V << std::endl; + // int a; + // std::cin >> a; + Eigen::Vector4d temp_Q = V.col(3); + + // Initialize a quaternion (not sure if the returned vector is (w, x, y, z), or (x, y, z, w)) + Eigen::Quaterniond q(temp_Q(0), temp_Q(1), temp_Q(2), temp_Q(3)); + + // Converting quaternion to rotation matrix + Eigen::Matrix3d R = q.normalized().toRotationMatrix(); + + // std::cout << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; + // std::cout << R << "\n" << std::endl; + + return R; + + } + + + Eigen::Vector3d ComputeTranslation(std::vector& im1_lines, std::vector& im2_lines, Eigen::Matrix3d& R){ + Eigen::Matrix3d temp1 = Eigen::Matrix3d::Zero(3,3); + Eigen::Vector3d temp2 = Eigen::Vector3d::Zero(3); + for (int i = 0; i < im2_lines.size(); i++) + { + Eigen::Matrix3d u_prime_hat = optim::GetCrossMatrix(im2_lines[i].u); + temp1 = temp1 + u_prime_hat*u_prime_hat.transpose(); + + temp2 = temp2 + u_prime_hat.transpose()*(im2_lines[i].d - R*im1_lines[i].d); + + } + Eigen::Vector3d t = temp1.inverse()*temp2; + + return t; + } } diff --git a/src/ransac.cpp b/src/ransac.cpp index 9089d10..b19949e 100644 --- a/src/ransac.cpp +++ b/src/ransac.cpp @@ -4,7 +4,7 @@ #include #include "ransac.h" - +#include "optim.h" using namespace std; Ransac::Ransac(int n_iterations, double threshold): n_iterations(n_iterations), @@ -45,9 +45,13 @@ Eigen::Matrix3Xd Ransac::removeOutlierPoints(const Eigen::Matrix3Xd& linepoints, auto ptr_to_inlier_indices = std::make_shared>(); // re-generate i and j randomly until they are not equal - int i = distribution(generator), j = distribution(generator); + int i = distribution(generator), j = distribution(generator), trial_num=0; while(i == j){ i = distribution(generator), j = distribution(generator); + if (trial_num++ > 100){ + cout << "Tried 100 times to generate two unique indices but coudln't find them" << endl; + cout << "Number of points in this line: " << linepoints.cols() << endl; + } } Eigen::Matrix3Xd support_points(3, linepoints.cols()); @@ -80,3 +84,93 @@ Eigen::Matrix3Xd Ransac::removeOutlierPoints(const Eigen::Matrix3Xd& linepoints, return best_inliers; } + +std::vector Ransac::ransac3D(const std::vector& optLines1, const std::vector& optLines2, const std::vector>& endPtCov1, + const std::vector> endPtCov2, Eigen::Matrix3d& R_best, Eigen::Vector3d& t_best) +{ + // Num of iterations of ransac + // int num_iter = 100; + float maha_dist; + int best_num_inliers = 0; + // double threshold = 5.0; + std::uniform_int_distribution distribution(0,optLines1.size() - 1); + // std::default_random_engine generator; + + std::shared_ptr> ptr_to_best_inlier_indices = NULL; + for (int k = 0; k < 200; k++) + { + int inliers = 0; + auto ptr_to_inlier_indices = std::make_shared>(); + // Random integer generation + int i = distribution(generator), j = distribution(generator); + while(i == j){ + i = distribution(generator), j = distribution(generator); + } + + // Getting random lines from the first image + optim::LineData l1(optLines1[i].leftCols(1), optLines1[i].rightCols(1)); + optim::LineData l2(optLines1[j].leftCols(1), optLines1[j].rightCols(1)); + + // checking if the lines are parallel + // don't hardcode the threshold (threshold = cos(10)) + + // Getting corresponding lines from the second image + optim::LineData l1_prime(optLines2[i].leftCols(1), optLines2[i].rightCols(1)); + optim::LineData l2_prime(optLines2[j].leftCols(1), optLines2[j].rightCols(1)); + + if (abs((l1_prime.u.dot(l2_prime.u))) > 0.984) + continue; + + std::vector im1_lines = {l1,l2}, im2_lines = {l1_prime, l2_prime}; + + Eigen::Matrix3d Rot = optim::ComputeRotationMatrix(l1, l1_prime, l2, l2_prime); + + Eigen::Vector3d T = optim::ComputeTranslation(im1_lines, im2_lines, Rot); + + // std::cout << "Translation is = \n " << T << std::endl; + + for (int size = 0; size < optLines1.size(); size++) + { + // if (!((size == i) || (size == j))) + // continue; + // std::cout << "Indices " << i << " " << j << " " << size << "\n"; + optim::LineData l(optLines1[size].leftCols(1), optLines1[size].rightCols(1)); + optim::LineData l_prime(optLines2[size].leftCols(1), optLines2[size].rightCols(1)); + + // double error = optim::computeRtError(Rot, T, l.A, l.B, endPtCov1[size][0], endPtCov1[size][1], + // l_prime.A, l_prime.B, endPtCov2[size][0], endPtCov2[size][1]); + + + double error = optim::computeRtError(Rot, T, l.A, l.B, l_prime.A, l_prime.B); + + + // std::cout << "Error is = " << error << std::endl; + if (error < 0.5) + { + // std::cout << "Rotation = \n" << Rot << "\n"; + // std::cout << "Translation = \n" << T << "\n"; + inliers++; + ptr_to_inlier_indices->push_back(size); + } + } + if(inliers > best_num_inliers){ + R_best = Rot; + t_best = T; + best_num_inliers = inliers; + ptr_to_best_inlier_indices = ptr_to_inlier_indices; + } + } + int inp; + // std::cout << "Number of possible inliers " << optLines1.size() << std::endl; + // std::cout << "Number of best Inliers " << best_num_inliers << std::endl; + // std::cout << "Best R " << R_best << std::endl; + // std::cout << "Best T " << t_best << std::endl; + // std::cin >> inp; + std::vector result; + for(auto i : *ptr_to_best_inlier_indices) + result.push_back(i); + + // std::cout << "Check 8" << std::endl; + return result; + +} diff --git a/src/test_viewer.cpp b/src/test_viewer.cpp new file mode 100644 index 0000000..255a5da --- /dev/null +++ b/src/test_viewer.cpp @@ -0,0 +1,130 @@ +#include +#include +#include +#include +#include +#include + +#include +#include "utils.h" + +using namespace std; +using namespace pangolin; +using namespace Eigen; +using namespace utils; + +int main(){ + cout << "testing 3D viewer" << endl; + + // Needs to have 16 entries which would make up the Transformation matrix + Eigen::Matrix4d origin = Eigen::MatrixXd::Identity(4,4); + Eigen::Matrix4d trans_x = Eigen::MatrixXd::Identity(4,4); + trans_x(0,3) = 1; + + Eigen::Matrix4d trans_y = Eigen::MatrixXd::Identity(4,4); + trans_y(1,3) = -2; + + Eigen::Matrix4d rot_x_trans_z; + rot_x_trans_z << 1.0000000, 0.0000000, 0.0000000, 0, + 0.0000000, 0.5253220, -0.8509035, 0, + 0.0000000, 0.8509035, 0.5253220, 1.5, + 0, 0, 0, 1; + std::vector poses; + + cv::Mat p1(4,4,CV_64F, origin.data()); + cv::Mat p2(4,4,CV_64F, trans_x.data()); + cv::Mat p3(4,4,CV_64F, trans_y.data()); + cv::Mat p4(4,4,CV_64F, rot_x_trans_z.data()); + + poses.push_back(p1); + poses.push_back(p2); + poses.push_back(p3); + poses.push_back(p4); + + std::vector gpose; + gpose.push_back(p1); + for(int i=1; i<=3; ++i){ + gpose.push_back(poses[i]*gpose.back()); + } + + pangolin::CreateWindowAndBind("Main",640,480); + glEnable(GL_DEPTH_TEST); + + // Define Projection and initial ModelView matrix + pangolin::OpenGlRenderState s_cam( + pangolin::ProjectionMatrix(640,480,420,420,320,240,0.2,100), + pangolin::ModelViewLookAt(-2,2,-2, 0,0,0, pangolin::AxisY) + ); + + // Create Interactive View in window + pangolin::Handler3D handler(s_cam); + pangolin::View& d_cam = pangolin::CreateDisplay() + .SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f) + .SetHandler(&handler); + + ifstream infile("/home/subbu/full_run_output_poses_backup.txt"); + string data, timestamp, gx, gy, gz, qx, qy, qz, qw; + std::string::size_type sz; + + double x,y,z,rx,ry,rz,rw; + std::vector cameras; + bool first = true; + + while( !pangolin::ShouldQuit() ) + { + // Clear screen and activate view to render into + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + d_cam.Activate(s_cam); + + DrawCoordinates(); + + // read data + getline(infile, data); + stringstream linestream(data); + linestream >> timestamp >> gx >> gy >> gz >> qx >> qy >> qz >> qw; + + x = stod(gx, &sz); + y = stod(gy, &sz); + z = stod(gz, &sz); + rx = stod(qx, &sz); + ry = stod(qy, &sz); + rz = stod(qz, &sz); + rw = stod(qw, &sz); + + // cout << x << " " << y << " " << z << " " << rx << " "<< ry << " "<< rz << " " << rw << endl; + // Make camera pose + double quaternion[4] = {rx, ry, rz, rw}; + Quaterniond myquat(quaternion); + Eigen::Matrix3d R = myquat.toRotationMatrix(); + Eigen::Vector3d t; + t << x, y, z; + Eigen::Matrix4d Affine; + Affine.setIdentity(); + Affine.block<3,3>(0,0) = R; + Affine.block<3,1>(0,3) = t; + cv::Mat camera_pose(4,4,CV_64F, Affine.data()); + cout << Affine << endl; + + cameras.push_back(camera_pose.clone()); + bool ml; + + // int n = 30; + // end = cameras.size() < 10? cameras.size(): 10; + for(int i=0; i> ml; + first = false; + } + } + + // Draw test camera + + usleep(100000); // sleep for 0.3 sec + + // Swap frames and Process Events + pangolin::FinishFrame(); + } + infile.close(); +} diff --git a/src/utils.cpp b/src/utils.cpp index 898555f..0f4c8cd 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -120,7 +120,7 @@ namespace utils{ // Note: gl uses column major representation for transformation matrices. // Hence we're using the mulTransposeMatrix version glPushMatrix(); - glMultTransposeMatrixf(camera_pose.ptr(0)); + glMultMatrixd(camera_pose.ptr(0)); glBegin(GL_LINES); glVertex3f(0,0,0); @@ -196,4 +196,13 @@ namespace utils{ } + cv::Mat MakeCameraPose(const Eigen::Matrix3d& R, const Eigen::Vector3d& t){ + Eigen::Matrix4d pose; pose.setIdentity(); + + pose.block<3,3>(0,0) = R; + pose.block<3,1>(0,3) = t; + + cv::Mat camera_pose(4,4,CV_64F, pose.data()); + return camera_pose.clone(); + } } diff --git a/src/viewer.cpp b/src/viewer.cpp index 9bc19e6..831cee9 100644 --- a/src/viewer.cpp +++ b/src/viewer.cpp @@ -4,8 +4,10 @@ #include #include +#include -viewer::viewer(const string& window_name) : window_name(window_name), current_frame(NULL), stopViewer(false) { +viewer::viewer(const string& window_name, struct system_poses* ptr_to_poses) : window_name(window_name), current_frame(NULL), + camera_poses(ptr_to_poses), stopViewer(false) { // create a window and bind its context to the main thread pangolin::CreateWindowAndBind(window_name, 640, 480); @@ -37,23 +39,32 @@ void viewer::run(){ d_cam.Activate(s_cam); utils::DrawCoordinates(); - + + // CRITICAL SECTION std::unique_lock curFrameLock(viewerVarsMtx); if(current_frame != NULL and current_frame->points_3d_im1.size()!= 0){ - // utils::DrawPoints3D(current_frame->rsac_points_3d_im1, {1.0, 1.0, 0.0}, 3); - // utils::DrawPoints3D(current_frame->rsac_points_3d_im2, {0.0, 1.0, 0.0}, 3); + utils::DrawPoints3D(current_frame->optimized_lines_im1, {1.0, 1.0, 0.0}, 3); + utils::DrawPoints3D(current_frame->optimized_lines_im2, {0.0, 1.0, 0.0}, 3); - utils::DrawSampledLine3D(current_frame->rsac_points_3d_im1.back(), {1.0, 1.0, 0.0}, 3); - utils::DrawSampledLine3D(current_frame->optimized_lines_im1.back(), {0.0, 1.0, 0.0}, 3); + // utils::DrawSampledLine3D(current_frame->rsac_points_3d_im1.back(), {1.0, 1.0, 0.0}, 3); + // utils::DrawSampledLine3D(current_frame->optimized_lines_im1.back(), {0.0, 1.0, 0.0}, 3); // utils::DrawSampledLine3D(current_frame->rsac_points_3d_im1[0], {1.0, 1.0, 0.0}, 3); // utils::DrawSampledLine3D(current_frame->rsac_points_3d_im1[1], {0.0, 1.0, 0.0}, 3); - } curFrameLock.unlock(); + std::unique_lock curPoseLock(viewerPosesMtx); + + if(camera_poses != NULL and camera_poses->global_poses.size()!=0){ + for(auto& current_camera_pose: camera_poses->global_poses) + utils::DrawSingleCamera(current_camera_pose); + } + + curPoseLock.unlock(); + // Swap frames and Process Events pangolin::FinishFrame(); diff --git a/tools/evaluate_rpe.py b/tools/evaluate_rpe.py new file mode 100644 index 0000000..23e50fd --- /dev/null +++ b/tools/evaluate_rpe.py @@ -0,0 +1,382 @@ +#!/usr/bin/python +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Juergen Sturm, TUM +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of TUM nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +""" +This script computes the relative pose error from the ground truth trajectory +and the estimated trajectory. +""" + +import argparse +import random +import numpy +import sys + +_EPS = numpy.finfo(float).eps * 4.0 + +def transform44(l): + """ + Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion. + + Input: + l -- tuple consisting of (stamp,tx,ty,tz,qx,qy,qz,qw) where + (tx,ty,tz) is the 3D position and (qx,qy,qz,qw) is the unit quaternion. + + Output: + matrix -- 4x4 homogeneous transformation matrix + """ + t = l[1:4] + q = numpy.array(l[4:8], dtype=numpy.float64, copy=True) + nq = numpy.dot(q, q) + if nq < _EPS: + return numpy.array(( + ( 1.0, 0.0, 0.0, t[0]) + ( 0.0, 1.0, 0.0, t[1]) + ( 0.0, 0.0, 1.0, t[2]) + ( 0.0, 0.0, 0.0, 1.0) + ), dtype=numpy.float64) + q *= numpy.sqrt(2.0 / nq) + q = numpy.outer(q, q) + return numpy.array(( + (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], t[0]), + ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], t[1]), + ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], t[2]), + ( 0.0, 0.0, 0.0, 1.0) + ), dtype=numpy.float64) + +def read_trajectory(filename, matrix=True): + """ + Read a trajectory from a text file. + + Input: + filename -- file to be read + matrix -- convert poses to 4x4 matrices + + Output: + dictionary of stamped 3D poses + """ + file = open(filename) + data = file.read() + lines = data.replace(","," ").replace("\t"," ").split("\n") + list = [[float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] + list_ok = [] + for i,l in enumerate(list): + if l[4:8]==[0,0,0,0]: + continue + isnan = False + for v in l: + if numpy.isnan(v): + isnan = True + break + if isnan: + sys.stderr.write("Warning: line %d of file '%s' has NaNs, skipping line\n"%(i,filename)) + continue + list_ok.append(l) + if matrix : + traj = dict([(l[0],transform44(l[0:])) for l in list_ok]) + else: + traj = dict([(l[0],l[1:8]) for l in list_ok]) + return traj + +def find_closest_index(L,t): + """ + Find the index of the closest value in a list. + + Input: + L -- the list + t -- value to be found + + Output: + index of the closest element + """ + beginning = 0 + difference = abs(L[0] - t) + best = 0 + end = len(L) + while beginning < end: + middle = int((end+beginning)/2) + if abs(L[middle] - t) < difference: + difference = abs(L[middle] - t) + best = middle + if t == L[middle]: + return middle + elif L[middle] > t: + end = middle + else: + beginning = middle + 1 + return best + +def ominus(a,b): + """ + Compute the relative 3D transformation between a and b. + + Input: + a -- first pose (homogeneous 4x4 matrix) + b -- second pose (homogeneous 4x4 matrix) + + Output: + Relative 3D transformation from a to b. + """ + return numpy.dot(numpy.linalg.inv(a),b) + +def scale(a,scalar): + """ + Scale the translational components of a 4x4 homogeneous matrix by a scale factor. + """ + return numpy.array( + [[a[0,0], a[0,1], a[0,2], a[0,3]*scalar], + [a[1,0], a[1,1], a[1,2], a[1,3]*scalar], + [a[2,0], a[2,1], a[2,2], a[2,3]*scalar], + [a[3,0], a[3,1], a[3,2], a[3,3]]] + ) + +def compute_distance(transform): + """ + Compute the distance of the translational component of a 4x4 homogeneous matrix. + """ + return numpy.linalg.norm(transform[0:3,3]) + +def compute_angle(transform): + """ + Compute the rotation angle from a 4x4 homogeneous matrix. + """ + # an invitation to 3-d vision, p 27 + return numpy.arccos( min(1,max(-1, (numpy.trace(transform[0:3,0:3]) - 1)/2) )) + +def distances_along_trajectory(traj): + """ + Compute the translational distances along a trajectory. + """ + keys = traj.keys() + keys.sort() + motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)] + distances = [0] + sum = 0 + for t in motion: + sum += compute_distance(t) + distances.append(sum) + return distances + +def rotations_along_trajectory(traj,scale): + """ + Compute the angular rotations along a trajectory. + """ + keys = traj.keys() + keys.sort() + motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)] + distances = [0] + sum = 0 + for t in motion: + sum += compute_angle(t)*scale + distances.append(sum) + return distances + + +def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta=False,param_delta=1.00,param_delta_unit="s",param_offset=0.00,param_scale=1.00): + """ + Compute the relative pose error between two trajectories. + + Input: + traj_gt -- the first trajectory (ground truth) + traj_est -- the second trajectory (estimated trajectory) + param_max_pairs -- number of relative poses to be evaluated + param_fixed_delta -- false: evaluate over all possible pairs + true: only evaluate over pairs with a given distance (delta) + param_delta -- distance between the evaluated pairs + param_delta_unit -- unit for comparison: + "s": seconds + "m": meters + "rad": radians + "deg": degrees + "f": frames + param_offset -- time offset between two trajectories (to model the delay) + param_scale -- scale to be applied to the second trajectory + + Output: + list of compared poses and the resulting translation and rotation error + """ + stamps_gt = list(traj_gt.keys()) + stamps_est = list(traj_est.keys()) + stamps_gt.sort() + stamps_est.sort() + + stamps_est_return = [] + for t_est in stamps_est: + t_gt = stamps_gt[find_closest_index(stamps_gt,t_est + param_offset)] + t_est_return = stamps_est[find_closest_index(stamps_est,t_gt - param_offset)] + t_gt_return = stamps_gt[find_closest_index(stamps_gt,t_est_return + param_offset)] + if not t_est_return in stamps_est_return: + stamps_est_return.append(t_est_return) + if(len(stamps_est_return)<2): + raise Exception("Number of overlap in the timestamps is too small. Did you run the evaluation on the right files?") + + if param_delta_unit=="s": + index_est = list(traj_est.keys()) + index_est.sort() + elif param_delta_unit=="m": + index_est = distances_along_trajectory(traj_est) + elif param_delta_unit=="rad": + index_est = rotations_along_trajectory(traj_est,1) + elif param_delta_unit=="deg": + index_est = rotations_along_trajectory(traj_est,180/numpy.pi) + elif param_delta_unit=="f": + index_est = range(len(traj_est)) + else: + raise Exception("Unknown unit for delta: '%s'"%param_delta_unit) + + if not param_fixed_delta: + if(param_max_pairs==0 or len(traj_est)param_max_pairs): + pairs = random.sample(pairs,param_max_pairs) + + gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])]) + gt_max_time_difference = 2*gt_interval + + result = [] + for i,j in pairs: + stamp_est_0 = stamps_est[i] + stamp_est_1 = stamps_est[j] + + stamp_gt_0 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_0 + param_offset) ] + stamp_gt_1 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_1 + param_offset) ] + + if(abs(stamp_gt_0 - (stamp_est_0 + param_offset)) > gt_max_time_difference or + abs(stamp_gt_1 - (stamp_est_1 + param_offset)) > gt_max_time_difference): + continue + + error44 = ominus( scale( + ominus( traj_est[stamp_est_1], traj_est[stamp_est_0] ),param_scale), + ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] ) ) + + trans = compute_distance(error44) + rot = compute_angle(error44) + + result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot]) + + if len(result)<2: + raise Exception("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!") + + return result + +def percentile(seq,q): + """ + Return the q-percentile of a list + """ + seq_sorted = list(seq) + seq_sorted.sort() + return seq_sorted[int((len(seq_sorted)-1)*q)] + +if __name__ == '__main__': + random.seed(0) + + parser = argparse.ArgumentParser(description=''' + This script computes the relative pose error from the ground truth trajectory and the estimated trajectory. + ''') + parser.add_argument('groundtruth_file', help='ground-truth trajectory file (format: "timestamp tx ty tz qx qy qz qw")') + parser.add_argument('estimated_file', help='estimated trajectory file (format: "timestamp tx ty tz qx qy qz qw")') + parser.add_argument('--max_pairs', help='maximum number of pose comparisons (default: 10000, set to zero to disable downsampling)', default=10000) + parser.add_argument('--fixed_delta', help='only consider pose pairs that have a distance of delta delta_unit (e.g., for evaluating the drift per second/meter/radian)', action='store_true') + parser.add_argument('--delta', help='delta for evaluation (default: 1.0)',default=1.0) + parser.add_argument('--delta_unit', help='unit of delta (options: \'s\' for seconds, \'m\' for meters, \'rad\' for radians, \'f\' for frames; default: \'s\')',default='s') + parser.add_argument('--offset', help='time offset between ground-truth and estimated trajectory (default: 0.0)',default=0.0) + parser.add_argument('--scale', help='scaling factor for the estimated trajectory (default: 1.0)',default=1.0) + parser.add_argument('--save', help='text file to which the evaluation will be saved (format: stamp_est0 stamp_est1 stamp_gt0 stamp_gt1 trans_error rot_error)') + parser.add_argument('--plot', help='plot the result to a file (requires --fixed_delta, output format: png)') + parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the mean translational error measured in meters will be printed)', action='store_true') + args = parser.parse_args() + + if args.plot and not args.fixed_delta: + sys.exit("The '--plot' option can only be used in combination with '--fixed_delta'") + + traj_gt = read_trajectory(args.groundtruth_file) + traj_est = read_trajectory(args.estimated_file) + + result = evaluate_trajectory(traj_gt, + traj_est, + int(args.max_pairs), + args.fixed_delta, + float(args.delta), + args.delta_unit, + float(args.offset), + float(args.scale)) + + stamps = numpy.array(result)[:,0] + trans_error = numpy.array(result)[:,4] + rot_error = numpy.array(result)[:,5] + + if args.save: + f = open(args.save,"w") + f.write("\n".join([" ".join(["%f"%v for v in line]) for line in result])) + f.close() + + if args.verbose: + print "compared_pose_pairs %d pairs"%(len(trans_error)) + + print "translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)) + print "translational_error.mean %f m"%numpy.mean(trans_error) + print "translational_error.median %f m"%numpy.median(trans_error) + print "translational_error.std %f m"%numpy.std(trans_error) + print "translational_error.min %f m"%numpy.min(trans_error) + print "translational_error.max %f m"%numpy.max(trans_error) + + print "rotational_error.rmse %f deg"%(numpy.sqrt(numpy.dot(rot_error,rot_error) / len(rot_error)) * 180.0 / numpy.pi) + print "rotational_error.mean %f deg"%(numpy.mean(rot_error) * 180.0 / numpy.pi) + print "rotational_error.median %f deg"%(numpy.median(rot_error) * 180.0 / numpy.pi) + print "rotational_error.std %f deg"%(numpy.std(rot_error) * 180.0 / numpy.pi) + print "rotational_error.min %f deg"%(numpy.min(rot_error) * 180.0 / numpy.pi) + print "rotational_error.max %f deg"%(numpy.max(rot_error) * 180.0 / numpy.pi) + else: + print numpy.mean(trans_error) + + if args.plot: + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + import matplotlib.pylab as pylab + fig = plt.figure() + ax = fig.add_subplot(111) + ax.plot(stamps - stamps[0],trans_error,'-',color="blue") + #ax.plot([t for t,e in err_rot],[e for t,e in err_rot],'-',color="red") + ax.set_xlabel('time [s]') + ax.set_ylabel('translational error [m]') + plt.savefig(args.plot,dpi=300) + + diff --git a/tools/output.jpg b/tools/output.jpg new file mode 100644 index 0000000..f1bc47d Binary files /dev/null and b/tools/output.jpg differ