diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp index 8e43ff8fc25922d78db414b5f24107ddc6811df6..31c29445830dbaf9d26d6a36add955e7312a59b1 100644 --- a/demos/demo_analytic_autodiff_factor.cpp +++ b/demos/demo_analytic_autodiff_factor.cpp @@ -20,17 +20,17 @@ // Testing creating wolf tree from imported .graph file -//C includes for sleep, time and main args +// C includes for sleep, time and main args #include "unistd.h" -//std includes +// std includes #include <iostream> #include <memory> #include <random> #include <cmath> #include <queue> -//Wolf includes +// Wolf includes #include "core/ceres_wrapper/solver_ceres.h" #include "wolf_manager.h" #include "core/capture/capture_void.h" @@ -38,25 +38,29 @@ // EIGEN //#include <Eigen/CholmodSupport> -namespace wolf { +namespace wolf +{ // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers -void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col) +void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, + Eigen::SparseMatrix<double>& original, + const unsigned int& row, + const unsigned int& col) { - for (int k=0; k<ins.outerSize(); ++k) - for (Eigen::SparseMatrix<double>::InnerIterator iti(ins,k); iti; ++iti) - original.coeffRef(iti.row() + row, iti.col() + col) = iti.value(); + for (int k = 0; k < ins.outerSize(); ++k) + for (Eigen::SparseMatrix<double>::InnerIterator iti(ins, k); iti; ++iti) + original.coeffRef(iti.row() + row, iti.col() + col) = iti.value(); - original.makeCompressed(); + original.makeCompressed(); } -int main(int argc, char** argv) +int main(int argc, char** argv) { using namespace wolf; - //Welcome message + // Welcome message std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl; - if (argc != 3 || atoi(argv[2]) < 0 ) + if (argc != 3 || atoi(argv[2]) < 0) { std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl; std::cout << " FILE_PATH is the .graph file path" << std::endl; @@ -66,10 +70,10 @@ int main(int argc, char** argv) } // auxiliar variables - std::string file_path_ = argv[1]; + std::string file_path_ = argv[1]; unsigned int MAX_VERTEX = atoi(argv[2]); if (MAX_VERTEX == 0) MAX_VERTEX = 1e6; - std::ifstream offLineFile_; + std::ifstream offLineFile_; ceres::Solver::Summary summary_autodiff, summary_analytic; // loading variables @@ -77,212 +81,219 @@ int main(int argc, char** argv) std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_analytic; // Wolf problem - ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d); - ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d); - SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2); + ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d); + ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d); + SensorBasePtr sensor = new SensorBase("ODOM 2d", + std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), + std::make_shared<StateAngle>(0), + std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), + 2); // Ceres wrapper - SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options); - SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options); - ceres_manager_autodiff.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options); + SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options); + ceres_manager_autodiff.getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;LINE_SEARCH ceres_manager_autodiff.getSolverOptions().max_line_search_step_contraction = 1e-3; - ceres_manager_autodiff.getSolverOptions().max_num_iterations = 1e4; - ceres_manager_analytic.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + ceres_manager_autodiff.getSolverOptions().max_num_iterations = 1e4; + ceres_manager_analytic.getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;LINE_SEARCH ceres_manager_analytic.getSolverOptions().max_line_search_step_contraction = 1e-3; - ceres_manager_analytic.getSolverOptions().max_num_iterations = 1e4; + ceres_manager_analytic.getSolverOptions().max_num_iterations = 1e4; // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); if (offLineFile_.is_open()) { - std::string buffer; + std::string buffer; unsigned int j = 0; // Line by line while (std::getline(offLineFile_, buffer)) { - //std::cout << "new line:" << buffer << std::endl; - std::string bNum; + // std::cout << "new line:" << buffer << std::endl; + std::string bNum; unsigned int i = 0; // VERTEX if (buffer.at(0) == 'V') { - //skip rest of VERTEX word + // skip rest of VERTEX word while (buffer.at(i) != ' ') i++; - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; - //vertex index - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + // vertex index + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); unsigned int vertex_index = atoi(bNum.c_str()); bNum.clear(); - if (vertex_index <= MAX_VERTEX+1) + if (vertex_index <= MAX_VERTEX + 1) { - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // vertex pose Eigen::Vector3d vertex_pose; // x - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); vertex_pose(0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // y - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); vertex_pose(1) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // theta - while (i < buffer.size() && buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); vertex_pose(2) = atof(bNum.c_str()); bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_autodiff = + new FrameBase(TimeStamp(0), + std::make_shared<StatePoint2d>(vertex_pose.head(2)), + std::make_shared<StateAngle>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_analytic = + new FrameBase(TimeStamp(0), + std::make_shared<StatePoint2d>(vertex_pose.head(2)), + std::make_shared<StateAngle>(vertex_pose.tail(1))); wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff); wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic); // store index_2_frame_ptr_autodiff[vertex_index] = vertex_frame_ptr_autodiff; index_2_frame_ptr_analytic[vertex_index] = vertex_frame_ptr_analytic; - //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_analytic->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl; + // std::cout << "Added vertex! index: " << vertex_index << " id: " << + // vertex_frame_ptr_analytic->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl; } } // EDGE else if (buffer.at(0) == 'E') { j++; - //skip rest of EDGE word + // skip rest of EDGE word while (buffer.at(i) != ' ') i++; - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; - //from - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + // from + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); unsigned int edge_old = atoi(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; - //to index - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + // to index + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); unsigned int edge_new = atoi(bNum.c_str()); bNum.clear(); - if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 ) + if (edge_new <= MAX_VERTEX + 1 && edge_old <= MAX_VERTEX + 1) { - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // edge vector Eigen::Vector3d edge_vector; // x - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); edge_vector(0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // y - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); edge_vector(1) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // theta - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); edge_vector(2) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // edge covariance Eigen::Matrix3d edge_information; // xx - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(0,0) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(0, 0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // xy - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(0,1) = atof(bNum.c_str()); - edge_information(1,0) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(0, 1) = atof(bNum.c_str()); + edge_information(1, 0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // yy - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(1,1) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(1, 1) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // thetatheta - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(2,2) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(2, 2) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // xtheta - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(0,2) = atof(bNum.c_str()); - edge_information(2,0) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(0, 2) = atof(bNum.c_str()); + edge_information(2, 0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // ytheta - while (i < buffer.size() && buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(1,2) = atof(bNum.c_str()); - edge_information(2,1) = atof(bNum.c_str()); + while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(1, 2) = atof(bNum.c_str()); + edge_information(2, 1) = atof(bNum.c_str()); bNum.clear(); // add capture, feature and factor to problem - FeatureBasePtr feature_ptr_autodiff = new FeatureBase("POSE", edge_vector, edge_information.inverse()); + FeatureBasePtr feature_ptr_autodiff = + new FeatureBase("POSE", edge_vector, edge_information.inverse()); CaptureVoid* capture_ptr_autodiff = new CaptureVoid(TimeStamp(0), sensor); - assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && "edge from vertex not added!"); + assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && + "edge from vertex not added!"); FrameBasePtr frame_old_ptr_autodiff = index_2_frame_ptr_autodiff[edge_old]; - assert(index_2_frame_ptr_autodiff.find(edge_new) != index_2_frame_ptr_autodiff.end() && "edge to vertex not added!"); + assert(index_2_frame_ptr_autodiff.find(edge_new) != index_2_frame_ptr_autodiff.end() && + "edge to vertex not added!"); FrameBasePtr frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new]; frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff); capture_ptr_autodiff->addFeature(feature_ptr_autodiff); FactorOdom2d* factor_ptr_autodiff = new FactorOdom2d(feature_ptr_autodiff, frame_old_ptr_autodiff); feature_ptr_autodiff->addFactor(factor_ptr_autodiff); - //std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << factor_ptr_autodiff->getFrameOther()->id() << std::endl; + // std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << + // factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << + // factor_ptr_autodiff->getFrameOther()->id() << std::endl; // add capture, feature and factor to problem - FeatureBasePtr feature_ptr_analytic = new FeatureBase("POSE", edge_vector, edge_information.inverse()); + FeatureBasePtr feature_ptr_analytic = + new FeatureBase("POSE", edge_vector, edge_information.inverse()); CaptureVoid* capture_ptr_analytic = new CaptureVoid(TimeStamp(0), sensor); - assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && "edge from vertex not added!"); + assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && + "edge from vertex not added!"); FrameBasePtr frame_old_ptr_analytic = index_2_frame_ptr_analytic[edge_old]; - assert(index_2_frame_ptr_analytic.find(edge_new) != index_2_frame_ptr_analytic.end() && "edge to vertex not added!"); + assert(index_2_frame_ptr_analytic.find(edge_new) != index_2_frame_ptr_analytic.end() && + "edge to vertex not added!"); FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new]; frame_new_ptr_analytic->addCapture(capture_ptr_analytic); capture_ptr_analytic->addFeature(feature_ptr_analytic); - FactorOdom2dAnalytic* factor_ptr_analytic = new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic); + FactorOdom2dAnalytic* factor_ptr_analytic = + new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic); feature_ptr_analytic->addFactor(factor_ptr_analytic); - //std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl; - //std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl; - //std::cout << "information " << std::endl << edge_information << std::endl; - //std::cout << "covariance " << std::endl << factor_ptr_analytic->getMeasurementCovariance() << std::endl; + // std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << + // factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << + // factor_ptr_analytic->getFrameOther()->id() << std::endl; std::cout << "vector " << + // factor_ptr_analytic->getMeasurement().transpose() << std::endl; std::cout << "information " << + // std::endl << edge_information << std::endl; std::cout << "covariance " << std::endl << + // factor_ptr_analytic->getMeasurementCovariance() << std::endl; } } else @@ -296,8 +307,16 @@ int main(int argc, char** argv) // PRIOR FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFirstFrame(); FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFirstFrame(); - CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01); - CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01); + CaptureFix* initial_covariance_autodiff = + new CaptureFix(TimeStamp(0), + new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), + first_frame_autodiff->getState(), + Eigen::Matrix3d::Identity() * 0.01); + CaptureFix* initial_covariance_analytic = + new CaptureFix(TimeStamp(0), + new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), + first_frame_analytic->getState(), + Eigen::Matrix3d::Identity() * 0.01); first_frame_autodiff->addCapture(initial_covariance_autodiff); first_frame_analytic->addCapture(initial_covariance_analytic); initial_covariance_autodiff->emplaceFeatureAndFactor(); @@ -316,21 +335,21 @@ int main(int argc, char** argv) std::cout << "computing covariances..." << std::endl; std::cout << "ANALYTIC -----------------------------------" << std::endl; clock_t t1 = clock(); - ceres_manager_analytic->computeCovariances(ALL);//ALL_MARGINALS - std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl; + ceres_manager_analytic->computeCovariances(ALL); // ALL_MARGINALS + std::cout << "Time: " << ((double)clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl; std::cout << "AUTODIFF -----------------------------------" << std::endl; t1 = clock(); - ceres_manager_autodiff->computeCovariances(ALL);//ALL_MARGINALS - std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl; + ceres_manager_autodiff->computeCovariances(ALL); // ALL_MARGINALS + std::cout << "Time: " << ((double)clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl; - delete wolf_problem_autodiff; //not necessary to delete anything more, wolf will do it! + delete wolf_problem_autodiff; // not necessary to delete anything more, wolf will do it! std::cout << "wolf_problem_ deleted!" << std::endl; delete ceres_manager_autodiff; delete ceres_manager_analytic; std::cout << "ceres_manager deleted!" << std::endl; - //End message + // End message std::cout << " =========================== END ===============================" << std::endl << std::endl; - - //exit + + // exit return 0; } diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp index 82e3dc3e34d7f2d43858d97e16991ce7b4a0c53a..ae87c1af8f39089fa055086ae031ebfb8facfadd 100644 --- a/demos/demo_wolf_imported_graph.cpp +++ b/demos/demo_wolf_imported_graph.cpp @@ -20,17 +20,17 @@ // Testing creating wolf tree from imported .graph file -//C includes for sleep, time and main args +// C includes for sleep, time and main args #include "unistd.h" -//std includes +// std includes #include <iostream> #include <memory> #include <random> #include <cmath> #include <queue> -//Wolf includes +// Wolf includes #include "ceres_wrapper/solver_ceres.h" #include "wolf_manager.h" #include "core/capture/capture_void.h" @@ -38,25 +38,29 @@ // EIGEN //#include <Eigen/CholmodSupport> -namespace wolf{ +namespace wolf +{ // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers -void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col) +void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, + Eigen::SparseMatrix<double>& original, + const unsigned int& row, + const unsigned int& col) { - for (int k=0; k<ins.outerSize(); ++k) - for (Eigen::SparseMatrix<double>::InnerIterator iti(ins,k); iti; ++iti) - original.coeffRef(iti.row() + row, iti.col() + col) = iti.value(); + for (int k = 0; k < ins.outerSize(); ++k) + for (Eigen::SparseMatrix<double>::InnerIterator iti(ins, k); iti; ++iti) + original.coeffRef(iti.row() + row, iti.col() + col) = iti.value(); - original.makeCompressed(); + original.makeCompressed(); } -int main(int argc, char** argv) +int main(int argc, char** argv) { using namespace wolf; - //Welcome message + // Welcome message std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl; - if (argc != 3 || atoi(argv[2]) < 0 ) + if (argc != 3 || atoi(argv[2]) < 0) { std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl; std::cout << " FILE_PATH is the .graph file path" << std::endl; @@ -66,14 +70,14 @@ int main(int argc, char** argv) } // auxiliar variables - std::string file_path_ = argv[1]; + std::string file_path_ = argv[1]; unsigned int MAX_VERTEX = atoi(argv[2]); if (MAX_VERTEX == 0) MAX_VERTEX = 1e6; - std::ifstream offLineFile_; - clock_t t1; + std::ifstream offLineFile_; + clock_t t1; ceres::Solver::Summary summary_full, summary_prun; - Eigen::MatrixXd Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3); - double xi, yi, thi, si, ci, xj, yj; + Eigen::MatrixXd Sigma_ii(3, 3), Sigma_ij(3, 3), Sigma_jj(3, 3), Sigma_z(3, 3), Ji(3, 3), Jj(3, 3); + double xi, yi, thi, si, ci, xj, yj; // loading variables std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_full; @@ -81,198 +85,198 @@ int main(int argc, char** argv) std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun; // Wolf problem - ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d); - ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d); - SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(Eigen::VectorXd::Zero(1)), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2); + ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d); + ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d); + SensorBasePtr sensor = new SensorBase("ODOM 2d", + std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), + std::make_shared<StateAngle>(Eigen::VectorXd::Zero(1)), + std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), + 2); - Eigen::SparseMatrix<double> Lambda(0,0); + Eigen::SparseMatrix<double> Lambda(0, 0); // Ceres wrapper ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + ceres_options.minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;LINE_SEARCH ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - SolverCeres* ceres_manager_full = new SolverCeres(wolf_problem_full, ceres_options); - SolverCeres* ceres_manager_prun = new SolverCeres(wolf_problem_prun, ceres_options); + ceres_options.max_num_iterations = 1e4; + SolverCeres* ceres_manager_full = new SolverCeres(wolf_problem_full, ceres_options); + SolverCeres* ceres_manager_prun = new SolverCeres(wolf_problem_prun, ceres_options); // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); if (offLineFile_.is_open()) { - std::string buffer; + std::string buffer; unsigned int j = 0; // Line by line while (std::getline(offLineFile_, buffer)) { - //std::cout << "new line:" << buffer << std::endl; - std::string bNum; + // std::cout << "new line:" << buffer << std::endl; + std::string bNum; unsigned int i = 0; // VERTEX if (buffer.at(0) == 'V') { - //skip rest of VERTEX word + // skip rest of VERTEX word while (buffer.at(i) != ' ') i++; - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; - //vertex index - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + // vertex index + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); unsigned int vertex_index = atoi(bNum.c_str()); bNum.clear(); - if (vertex_index <= MAX_VERTEX+1) + if (vertex_index <= MAX_VERTEX + 1) { - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // vertex pose Eigen::Vector3d vertex_pose; // x - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); vertex_pose(0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // y - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); vertex_pose(1) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // theta - while (i < buffer.size() && buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); vertex_pose(2) = atof(bNum.c_str()); bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_full = + new FrameBase(TimeStamp(0), + std::make_shared<StatePoint2d>(vertex_pose.head(2)), + std::make_shared<StateAngle>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_prun = + new FrameBase(TimeStamp(0), + std::make_shared<StatePoint2d>(vertex_pose.head(2)), + std::make_shared<StateAngle>(vertex_pose.tail(1))); wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full); wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun); // store - index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full; - index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun; + index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full; + index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun; frame_ptr_2_index_prun[vertex_frame_ptr_prun] = vertex_index; // Information matrix - Lambda.conservativeResize(Lambda.rows()+3,Lambda.cols()+3); + Lambda.conservativeResize(Lambda.rows() + 3, Lambda.cols() + 3); - //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_prun->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl; + // std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_prun->id() + // << std::endl << "pose: " << vertex_pose.transpose() << std::endl; } } // EDGE else if (buffer.at(0) == 'E') { j++; - //skip rest of EDGE word + // skip rest of EDGE word while (buffer.at(i) != ' ') i++; - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; - //from - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + // from + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); unsigned int edge_old = atoi(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; - //to index - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + // to index + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); unsigned int edge_new = atoi(bNum.c_str()); bNum.clear(); - if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 ) + if (edge_new <= MAX_VERTEX + 1 && edge_old <= MAX_VERTEX + 1) { - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // edge vector Eigen::Vector3d edge_vector; // x - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); edge_vector(0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // y - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); edge_vector(1) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // theta - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); edge_vector(2) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // edge covariance Eigen::Matrix3d edge_information; // xx - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(0,0) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(0, 0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // xy - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(0,1) = atof(bNum.c_str()); - edge_information(1,0) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(0, 1) = atof(bNum.c_str()); + edge_information(1, 0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // yy - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(1,1) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(1, 1) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // thetatheta - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(2,2) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(2, 2) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // xtheta - while (buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(0,2) = atof(bNum.c_str()); - edge_information(2,0) = atof(bNum.c_str()); + while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(0, 2) = atof(bNum.c_str()); + edge_information(2, 0) = atof(bNum.c_str()); bNum.clear(); - //skip white spaces + // skip white spaces while (buffer.at(i) == ' ') i++; // ytheta - while (i < buffer.size() && buffer.at(i) != ' ') - bNum.push_back(buffer.at(i++)); - edge_information(1,2) = atof(bNum.c_str()); - edge_information(2,1) = atof(bNum.c_str()); + while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++)); + edge_information(1, 2) = atof(bNum.c_str()); + edge_information(2, 1) = atof(bNum.c_str()); bNum.clear(); // add capture, feature and factor to problem FeatureBasePtr feature_ptr_full = new FeatureBase("POSE", edge_vector, edge_information.inverse()); FeatureBasePtr feature_ptr_prun = new FeatureBase("POSE", edge_vector, edge_information.inverse()); - CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor); - CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor); - assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && "edge from vertex not added!"); - assert(index_2_frame_ptr_prun.find(edge_old) != index_2_frame_ptr_prun.end() && "edge from vertex not added!"); + CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor); + CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor); + assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && + "edge from vertex not added!"); + assert(index_2_frame_ptr_prun.find(edge_old) != index_2_frame_ptr_prun.end() && + "edge from vertex not added!"); FrameBasePtr frame_old_ptr_full = index_2_frame_ptr_full[edge_old]; FrameBasePtr frame_old_ptr_prun = index_2_frame_ptr_prun[edge_old]; - assert(index_2_frame_ptr_full.find(edge_new) != index_2_frame_ptr_full.end() && "edge to vertex not added!"); - assert(index_2_frame_ptr_prun.find(edge_new) != index_2_frame_ptr_prun.end() && "edge to vertex not added!"); + assert(index_2_frame_ptr_full.find(edge_new) != index_2_frame_ptr_full.end() && + "edge to vertex not added!"); + assert(index_2_frame_ptr_prun.find(edge_new) != index_2_frame_ptr_prun.end() && + "edge to vertex not added!"); FrameBasePtr frame_new_ptr_full = index_2_frame_ptr_full[edge_new]; FrameBasePtr frame_new_ptr_prun = index_2_frame_ptr_prun[edge_new]; frame_new_ptr_full->addCapture(capture_ptr_full); @@ -283,32 +287,42 @@ int main(int argc, char** argv) FactorOdom2d* factor_ptr_prun = new FactorOdom2d(feature_ptr_prun, frame_old_ptr_prun); feature_ptr_full->addFactor(factor_ptr_full); feature_ptr_prun->addFactor(factor_ptr_prun); - //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id() << std::endl; - //std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << std::endl; - //std::cout << "information " << std::endl << edge_information << std::endl; - //std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl; - - double xi = *(frame_old_ptr_prun->getP()->get()); - double yi = *(frame_old_ptr_prun->getP()->get()+1); - double thi = *(frame_old_ptr_prun->getO()->get()); - double si = sin(thi); - double ci = cos(thi); - double xj = *(frame_new_ptr_prun->getP()->get()); - double yj = *(frame_new_ptr_prun->getP()->get()+1); - Eigen::MatrixXd Ji(3,3), Jj(3,3); - Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci, - si,-ci,-(xj-xi)*ci-(yj-yi)*si, - 0, 0, -1; - Jj << ci, si, 0, - -si, ci, 0, - 0, 0, 1; - //std::cout << "Ji" << std::endl << Ji << std::endl; - //std::cout << "Jj" << std::endl << Jj << std::endl; + // std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << + // factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id() + // << std::endl; std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << + // std::endl; std::cout << "information " << std::endl << edge_information << std::endl; std::cout + // << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl; + + double xi = *(frame_old_ptr_prun->getP()->get()); + double yi = *(frame_old_ptr_prun->getP()->get() + 1); + double thi = *(frame_old_ptr_prun->getO()->get()); + double si = sin(thi); + double ci = cos(thi); + double xj = *(frame_new_ptr_prun->getP()->get()); + double yj = *(frame_new_ptr_prun->getP()->get() + 1); + Eigen::MatrixXd Ji(3, 3), Jj(3, 3); + Ji << -ci, -si, -(xj - xi) * si + (yj - yi) * ci, si, -ci, -(xj - xi) * ci - (yj - yi) * si, 0, 0, + -1; + Jj << ci, si, 0, -si, ci, 0, 0, 0, 1; + // std::cout << "Ji" << std::endl << Ji << std::endl; + // std::cout << "Jj" << std::endl << Jj << std::endl; Eigen::SparseMatrix<double> DeltaLambda(Lambda.rows(), Lambda.cols()); - insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3); - insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3); - insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3); - insertSparseBlock((Jj.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_new*3, edge_old*3); + insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), + DeltaLambda, + edge_old * 3, + edge_old * 3); + insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), + DeltaLambda, + edge_new * 3, + edge_new * 3); + insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), + DeltaLambda, + edge_old * 3, + edge_new * 3); + insertSparseBlock((Jj.transpose() * edge_information * Ji).sparseView(), + DeltaLambda, + edge_new * 3, + edge_old * 3); Lambda = Lambda + DeltaLambda; } } @@ -321,15 +335,23 @@ int main(int argc, char** argv) printf("\nError opening file\n"); // PRIOR - FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFirstFrame(); - FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFirstFrame(); - CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01); - CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01); + FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFirstFrame(); + FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFirstFrame(); + CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), + new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), + first_frame_full->getState(), + Eigen::Matrix3d::Identity() * 0.01); + CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), + new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), + first_frame_prun->getState(), + Eigen::Matrix3d::Identity() * 0.01); first_frame_full->addCapture(initial_covariance_full); first_frame_prun->addCapture(initial_covariance_prun); initial_covariance_full->process(); initial_covariance_prun->process(); - //std::cout << "initial covariance: factor " << initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl; + // std::cout << "initial covariance: factor " << + // initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << + // initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl; Eigen::SparseMatrix<double> DeltaLambda(Lambda.rows(), Lambda.cols()); insertSparseBlock((Eigen::Matrix3d::Identity() * 100).sparseView(), DeltaLambda, 0, 0); Lambda = Lambda + DeltaLambda; @@ -340,77 +362,94 @@ int main(int argc, char** argv) // Manual covariance computation t1 = clock(); Eigen::SimplicialLLT<Eigen::SparseMatrix<double>> chol(Lambda); // performs a Cholesky factorization of A - Eigen::MatrixXd Sigma = chol.solve(Eigen::MatrixXd::Identity(Lambda.rows(), Lambda.cols())); - double t_sigma_manual = ((double) clock() - t1) / CLOCKS_PER_SEC; - //std::cout << "Lambda" << std::endl << Lambda << std::endl; - //std::cout << "Sigma" << std::endl << Sigma << std::endl; + Eigen::MatrixXd Sigma = chol.solve(Eigen::MatrixXd::Identity(Lambda.rows(), Lambda.cols())); + double t_sigma_manual = ((double)clock() - t1) / CLOCKS_PER_SEC; + // std::cout << "Lambda" << std::endl << Lambda << std::endl; + // std::cout << "Sigma" << std::endl << Sigma << std::endl; std::cout << " ceres is computing covariances..." << std::endl; t1 = clock(); - ceres_manager_prun->computeCovariances(ALL);//ALL_MARGINALS - double t_sigma_ceres = ((double) clock() - t1) / CLOCKS_PER_SEC; + ceres_manager_prun->computeCovariances(ALL); // ALL_MARGINALS + double t_sigma_ceres = ((double)clock() - t1) / CLOCKS_PER_SEC; std::cout << "computed!" << std::endl; t1 = clock(); - for (auto c_it=factors.begin(); c_it!=factors.end(); c_it++) + for (auto c_it = factors.begin(); c_it != factors.end(); c_it++) { if ((*c_it)->getCategory() != FAC_FRAME) continue; // ii (old) - wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0); - wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2); - wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0); - wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2); -// std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl; -// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl; + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2); + // std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl; + // std::cout << "Sigma(i,i)" << std::endl << + // Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, + // frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl; // jj (new) - wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0); - wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2); - wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0); - wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2); -// std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl; -// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl; + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2); + // std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl; + // std::cout << "Sigma(j,j)" << std::endl << + // Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, + // frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl; // ij (old-new) - wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0); - wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2); - wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0); - wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2); -// std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl; -// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl; - - //jacobian - xi = *(*c_it)->getFrameOther()->getP()->get(); - yi = *((*c_it)->getFrameOther()->getP()->get()+1); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0); + wolf_problem_prun->getCovarianceBlock( + (*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2); + // std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl; + // std::cout << "Sigma(i,j)" << std::endl << + // Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, + // frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl; + + // jacobian + xi = *(*c_it)->getFrameOther()->getP()->get(); + yi = *((*c_it)->getFrameOther()->getP()->get() + 1); thi = *(*c_it)->getFrameOther()->getO()->get(); - si = sin(thi); - ci = cos(thi); - xj = *(*c_it)->getCapture()->getFrame()->getP()->get(); - yj = *((*c_it)->getCapture()->getFrame()->getP()->get()+1); - - Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci, - si,-ci,-(xj-xi)*ci-(yj-yi)*si, - 0, 0, -1; - Jj << ci, si, 0, - -si, ci, 0, - 0, 0, 1; - //std::cout << "Ji" << std::endl << Ji << std::endl; - //std::cout << "Jj" << std::endl << Jj << std::endl; + si = sin(thi); + ci = cos(thi); + xj = *(*c_it)->getCapture()->getFrame()->getP()->get(); + yj = *((*c_it)->getCapture()->getFrame()->getP()->get() + 1); + + Ji << -ci, -si, -(xj - xi) * si + (yj - yi) * ci, si, -ci, -(xj - xi) * ci - (yj - yi) * si, 0, 0, -1; + Jj << ci, si, 0, -si, ci, 0, 0, 0, 1; + // std::cout << "Ji" << std::endl << Ji << std::endl; + // std::cout << "Jj" << std::endl << Jj << std::endl; // Measurement covariance Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance(); - //std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl; - //std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl; + // std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl; + // std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl; - //std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl; + // std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * + // Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl; // Information gain - double IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() ); - //std::cout << "IG = " << IG << std::endl; + double IG = 0.5 * log(Sigma_z.determinant() / + (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())) + .determinant()); + // std::cout << "IG = " << IG << std::endl; - if (IG < 2) - (*c_it)->setStatus(FAC_INACTIVE); + if (IG < 2) (*c_it)->setStatus(FAC_INACTIVE); } - double t_ig = ((double) clock() - t1) / CLOCKS_PER_SEC; + double t_ig = ((double)clock() - t1) / CLOCKS_PER_SEC; std::cout << "manual sigma computation " << t_sigma_manual << "s" << std::endl; std::cout << "ceres sigma computation " << t_sigma_ceres << "s" << std::endl; std::cout << "information gain computation " << t_ig << "s" << std::endl; @@ -422,14 +461,14 @@ int main(int argc, char** argv) summary_prun = ceres_manager_prun->solve(); std::cout << summary_prun.FullReport() << std::endl; - delete wolf_problem_full; //not necessary to delete anything more, wolf will do it! + delete wolf_problem_full; // not necessary to delete anything more, wolf will do it! std::cout << "wolf_problem_ deleted!" << std::endl; delete ceres_manager_full; delete ceres_manager_prun; std::cout << "ceres_manager deleted!" << std::endl; - //End message + // End message std::cout << " =========================== END ===============================" << std::endl << std::endl; - - //exit + + // exit return 0; } diff --git a/demos/hello_wolf/capture_range_bearing.cpp b/demos/hello_wolf/capture_range_bearing.cpp index ca2f7c10ab8f395c4f9fdfbafa668955be42cb75..fb2aaa1e402ec254757151575e680b4130184b4e 100644 --- a/demos/hello_wolf/capture_range_bearing.cpp +++ b/demos/hello_wolf/capture_range_bearing.cpp @@ -22,12 +22,12 @@ namespace wolf { - -CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) : - CaptureBase("CaptureRangeBearing", _ts, _scanner), - ids_(_ids), - ranges_(_ranges), - bearings_(_bearings) +CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, + const SensorBasePtr& _scanner, + const Eigen::VectorXi& _ids, + const Eigen::VectorXd& _ranges, + const Eigen::VectorXd& _bearings) + : CaptureBase("CaptureRangeBearing", _ts, _scanner), ids_(_ids), ranges_(_ranges), bearings_(_bearings) { // } diff --git a/demos/hello_wolf/capture_range_bearing.h b/demos/hello_wolf/capture_range_bearing.h index 5600dd0fe928ec87be31ca9b28927b8572581b56..e24ec94c4aec176e5e6b5010977c0ada391be6e1 100644 --- a/demos/hello_wolf/capture_range_bearing.h +++ b/demos/hello_wolf/capture_range_bearing.h @@ -24,30 +24,33 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(CaptureRangeBearing) using namespace Eigen; class CaptureRangeBearing : public CaptureBase { - public: - CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings); - ~CaptureRangeBearing() override; - - const VectorXi& getIds () const; - const int& getId (int _i) const; - const Eigen::VectorXd& getRanges () const; - const Eigen::VectorXd& getBearings () const; - const double& getRange (int _i) const; - const double& getBearing (int _i) const; - Eigen::Vector2d getRangeBearing (int _i) const; - Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const; - - private: - VectorXi ids_; // identifiers - VectorXd ranges_; // ranges - VectorXd bearings_; // bearings + public: + CaptureRangeBearing(const TimeStamp& _ts, + const SensorBasePtr& _scanner, + const Eigen::VectorXi& _ids, + const Eigen::VectorXd& _ranges, + const Eigen::VectorXd& _bearings); + ~CaptureRangeBearing() override; + + const VectorXi& getIds() const; + const int& getId(int _i) const; + const Eigen::VectorXd& getRanges() const; + const Eigen::VectorXd& getBearings() const; + const double& getRange(int _i) const; + const double& getBearing(int _i) const; + Eigen::Vector2d getRangeBearing(int _i) const; + Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const; + + private: + VectorXi ids_; // identifiers + VectorXd ranges_; // ranges + VectorXd bearings_; // bearings }; inline const Eigen::VectorXi& CaptureRangeBearing::getIds() const @@ -80,9 +83,9 @@ inline const double& CaptureRangeBearing::getBearing(int _i) const return bearings_(_i); } -inline Eigen::Matrix<double,Dynamic,2> CaptureRangeBearing::getRangeBearing() const +inline Eigen::Matrix<double, Dynamic, 2> CaptureRangeBearing::getRangeBearing() const { - return (Matrix<double,Dynamic,2>() << ranges_, bearings_).finished(); + return (Matrix<double, Dynamic, 2>() << ranges_, bearings_).finished(); } inline Eigen::Vector2d CaptureRangeBearing::getRangeBearing(int _i) const diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h index 2e4ed1d62e182e2d76b3f6add91b9c32c974133c..53caf18af0ad54cfeabbec313e08ddc1455edc69 100644 --- a/demos/hello_wolf/factor_bearing.h +++ b/demos/hello_wolf/factor_bearing.h @@ -24,48 +24,47 @@ namespace wolf { - using namespace Eigen; class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2> { - public: - FactorBearing(const FeatureBasePtr& _feature_ptr, - const LandmarkBasePtr& _landmark_other_ptr, - const ProcessorBasePtr& _processor_ptr, - bool _apply_loss_function, FactorStatus _status) : - FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING", - TOP_LMK, - _feature_ptr, - nullptr, - nullptr, - nullptr, - _landmark_other_ptr, - _processor_ptr, - _apply_loss_function, - _status, - _feature_ptr->getCapture()->getFrame()->getP(), - _feature_ptr->getCapture()->getFrame()->getO(), - _feature_ptr->getCapture()->getSensor()->getP(), - _feature_ptr->getCapture()->getSensor()->getO(), - _landmark_other_ptr->getP()) - { - // - } - - virtual ~FactorBearing() - { - // - } - - template<typename T> - bool operator ()(const T* const _p_w_r, - const T* const _o_w_r, - const T* const _p_r_s, // sensor position - const T* const _o_r_s, // sensor orientation - const T* const _lmk, - T* _res) const; - + public: + FactorBearing(const FeatureBasePtr& _feature_ptr, + const LandmarkBasePtr& _landmark_other_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status) + : FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING", + TOP_LMK, + _feature_ptr, + nullptr, + nullptr, + nullptr, + _landmark_other_ptr, + _processor_ptr, + _apply_loss_function, + _status, + _feature_ptr->getCapture()->getFrame()->getP(), + _feature_ptr->getCapture()->getFrame()->getO(), + _feature_ptr->getCapture()->getSensor()->getP(), + _feature_ptr->getCapture()->getSensor()->getO(), + _landmark_other_ptr->getP()) + { + // + } + + virtual ~FactorBearing() + { + // + } + + template <typename T> + bool operator()(const T* const _p_w_r, + const T* const _o_w_r, + const T* const _p_r_s, // sensor position + const T* const _o_r_s, // sensor orientation + const T* const _lmk, + T* _res) const; }; } /* namespace wolf */ @@ -74,44 +73,44 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2> namespace wolf { - -template<typename T> -inline bool FactorBearing::operator ()( const T* const _p_w_r, - const T* const _o_w_r, - const T* const _p_r_s, // sensor position - const T* const _o_r_s, // sensor orientation - const T* const _lmk, - T* _res) const +template <typename T> +inline bool FactorBearing::operator()(const T* const _p_w_r, + const T* const _o_w_r, + const T* const _p_r_s, // sensor position + const T* const _o_r_s, // sensor orientation + const T* const _lmk, + T* _res) const { - // 1. produce a transformation matrix to transform from robot frame to world frame - Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform - Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Robot frame = robot-to-world transform - + Transform<T, 2, Isometry> H_w_r = + Translation<T, 2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r); // Robot frame = robot-to-world transform + Transform<T, 2, Isometry> H_r_s = + Translation<T, 2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s); // Robot frame = robot-to-world transform + // Map input pointers into meaningful Eigen elements - Map<const Matrix<T, 2, 1>> point_w(_lmk); - Map<const Matrix<T, 1, 1>> res(_res); + Map<const Matrix<T, 2, 1>> point_w(_lmk); + Map<const Matrix<T, 1, 1>> res(_res); // 2. Transform world point to sensor-referenced point Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w; // 3. Get the expected bearing of the point - T bearing = atan2(point_s(1), point_s(0)); + T bearing = atan2(point_s(1), point_s(0)); // 4. Get the measured range-and-bearing to the point Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>(); // 5. Get the bearing error by comparing against the bearing measurement - T er = range_bearing(1) - bearing; + T er = range_bearing(1) - bearing; if (er < T(-M_PI)) - er += T(2*M_PI); - else if (er > T(-M_PI)) - er -= T(2*M_PI); + er += T(2 * M_PI); + else if (er > T(-M_PI)) + er -= T(2 * M_PI); // 6. Compute the residual by scaling according to the standard deviation of the bearing part - *_res = er * getMeasurementSquareRootInformationUpper()(1,1); + *_res = er * getMeasurementSquareRootInformationUpper()(1, 1); return true; } -} // namespace wolf +} // namespace wolf diff --git a/demos/hello_wolf/feature_range_bearing.cpp b/demos/hello_wolf/feature_range_bearing.cpp index a99fa854e8244500d359acbe28cdf90758af2dcf..e5a806203bb65b01f3160546605c10031ff612f9 100644 --- a/demos/hello_wolf/feature_range_bearing.cpp +++ b/demos/hello_wolf/feature_range_bearing.cpp @@ -22,9 +22,8 @@ namespace wolf { - -FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) : - FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance) +FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) + : FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance) { // } @@ -34,5 +33,4 @@ FeatureRangeBearing::~FeatureRangeBearing() // } -} // namespace wolf - +} // namespace wolf diff --git a/demos/hello_wolf/feature_range_bearing.h b/demos/hello_wolf/feature_range_bearing.h index db4e11fabc503aa966c55b31025b9ce665560030..e729b350faea2f0acbc0e44047f81d0751c65ddb 100644 --- a/demos/hello_wolf/feature_range_bearing.h +++ b/demos/hello_wolf/feature_range_bearing.h @@ -24,14 +24,13 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FeatureRangeBearing); class FeatureRangeBearing : public FeatureBase { - public: - FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance); - ~FeatureRangeBearing() override; + public: + FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance); + ~FeatureRangeBearing() override; }; -} // namespace wolf +} // namespace wolf diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index 4416017843e8a97b43d36701008bbf2ca907c92d..c5ee1d7d2252d824148dd8a50b8901388b21a809 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -26,9 +26,8 @@ #include "core/processor/processor_motion.h" #include "capture_range_bearing.h" - int main() - { +{ /* * ============= PROBLEM DEFINITION ================== * @@ -96,7 +95,8 @@ int main() * - Second, using random values * Both solutions must produce the same exact values as in the sketches above. * - * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 136) + * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line + * 136) * * (c) 2017 Joan Sola @ IRI-CSIC */ @@ -105,7 +105,6 @@ int main() using namespace wolf; - WOLF_INFO("======== CONFIGURE PROBLEM ======="); // Config file to parse. Here is where all the problem is defined: @@ -120,38 +119,38 @@ int main() throw std::runtime_error("Couldn't load and validate the yaml file " + config_file); } // Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server: - ProblemPtr problem = Problem::autoSetup(server.getNode()); - + ProblemPtr problem = Problem::autoSetup(server.getNode()); + // Print problem to see its status before processing any sensor data - problem->print(4,0,1,0); + problem->print(4, 0, 1, 0); // Solver. Configure a Ceres solver SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server.getNode()["solver"]); // recover sensor pointers and other stuff for later use (access by sensor name) - SensorBasePtr sensor_odo = problem->findSensor("sen odom"); - SensorBasePtr sensor_rb = problem->findSensor("sen rb"); + SensorBasePtr sensor_odo = problem->findSensor("sen odom"); + SensorBasePtr sensor_rb = problem->findSensor("sen rb"); // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN =================================================== - TimeStamp t(0.0); + TimeStamp t(0.0); FrameBasePtr KF1 = problem->applyPriorOptions(t); -// std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1); + // std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1); // SELF CALIBRATION =================================================== // These few lines control whether we calibrate some sensor parameters or not. // SELF-CALIBRATION OF SENSOR ORIENTATION - // Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable) - // sensor_rb->getO()->unfix(); + // Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not + // observable) sensor_rb->getO()->unfix(); // SELF-CALIBRATION OF SENSOR POSITION - // The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below. - // sensor_rb->getP()->unfix(); + // The position is however not observable, and thus self-calibration would not work. You can try uncommenting the + // line below. sensor_rb->getP()->unfix(); // CONFIGURE input data (motion and measurements) ============================================== // Motion data - Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn. + Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn. Matrix2d motion_cov = 0.1 * Matrix2d::Identity(); // landmark observations data @@ -167,69 +166,75 @@ int main() // STEP 1 -------------------------------------------------------------- // move zero motion to accept the first keyframe and initialize the processor - CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0*motion_data, 0*motion_cov); - cap_motion ->process(); // KF1 : (0,0,0) + CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0 * motion_data, 0 * motion_cov); + cap_motion->process(); // KF1 : (0,0,0) // observe lmks - ids.resize(1); ranges.resize(1); bearings.resize(1); - ids << 1; // will observe Lmk 1 - ranges << 1.0; // see drawing - bearings << M_PI/2; + ids.resize(1); + ranges.resize(1); + bearings.resize(1); + ids << 1; // will observe Lmk 1 + ranges << 1.0; // see drawing + bearings << M_PI / 2; CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); - cap_rb->process(); // L1 : (1,2) + cap_rb->process(); // L1 : (1,2) // STEP 2 -------------------------------------------------------------- - t += 1.0; // t : 1.0 + t += 1.0; // t : 1.0 // motion cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov); - cap_motion ->process(); // KF2 : (1,0,0) + cap_motion->process(); // KF2 : (1,0,0) // observe lmks - ids.resize(2); ranges.resize(2); bearings.resize(2); - ids << 1, 2; // will observe Lmks 1 and 2 - ranges << sqrt(2.0), 1.0; // see drawing - bearings << 3*M_PI/4, M_PI/2; + ids.resize(2); + ranges.resize(2); + bearings.resize(2); + ids << 1, 2; // will observe Lmks 1 and 2 + ranges << sqrt(2.0), 1.0; // see drawing + bearings << 3 * M_PI / 4, M_PI / 2; - cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); - cap_rb ->process(); // L1 : (1,2), L2 : (2,2) + cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); + cap_rb->process(); // L1 : (1,2), L2 : (2,2) // STEP 3 -------------------------------------------------------------- - t += 1.0; // t : 2.0 + t += 1.0; // t : 2.0 // motion - cap_motion ->setTimeStamp(t); - cap_motion ->process(); // KF3 : (2,0,0) + cap_motion->setTimeStamp(t); + cap_motion->process(); // KF3 : (2,0,0) // observe lmks - ids.resize(2); ranges.resize(2); bearings.resize(2); - ids << 2, 3; // will observe Lmks 2 and 3 - ranges << sqrt(2.0), 1.0; // see drawing - bearings << 3*M_PI/4, M_PI/2; + ids.resize(2); + ranges.resize(2); + bearings.resize(2); + ids << 2, 3; // will observe Lmks 2 and 3 + ranges << sqrt(2.0), 1.0; // see drawing + bearings << 3 * M_PI / 4, M_PI / 2; - cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); - cap_rb ->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2) + cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); + cap_rb->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2) - problem->print(1,0,1,0); + problem->print(1, 0, 1, 0); // SOLVE ================================================================ // SOLVE with exact initial guess WOLF_INFO("======== SOLVE PROBLEM WITH EXACT PRIORS =======") std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); // should show a very low iteration number (possibly 1) - problem->print(1,0,1,0); + WOLF_INFO(report); // should show a very low iteration number (possibly 1) + problem->print(1, 0, 1, 0); // PERTURB initial guess WOLF_INFO("======== PERTURB PROBLEM PRIORS =======") - problem->perturb(0.5); // Perturb all state blocks that are not fixed - problem->print(1,0,1,0); + problem->perturb(0.5); // Perturb all state blocks that are not fixed + problem->print(1, 0, 1, 0); // SOLVE again WOLF_INFO("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======") report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!) - problem->print(1,0,1,0); + WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!) + problem->print(1, 0, 1, 0); // GET COVARIANCES of all states WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======") @@ -249,7 +254,7 @@ int main() std::cout << std::endl; WOLF_INFO("======== FINAL PRINT FOR INTERPRETATION =======") - problem->print(4,1,1,1); + problem->print(4, 1, 1, 1); /* * ============= FIRST COMMENT ON THE RESULTS ================== @@ -278,34 +283,21 @@ int main() * P: wolf tree status --------------------- Hardware Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D - sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below) - PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D - o: Cap6 - KFrm3 // origin at Capture 6, Frame 3 - l: Cap8 - Frm4 // last at Capture 8, Frame 4 - Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing - sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below) - Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing - Trajectory - KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4 - P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated - O[Est] = ( 2.6e-09 ) // Orientation is estimated - Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior - Ftr1 trk0 prior <-- // Position prior - m = ( 0 0 ) - Fac1 FactorBlockAbsolute --> Abs - Ftr2 trk0 prior <-- // Orientation prior - m = ( 0 ) - Fac2 FactorBlockAbsolute --> Abs - CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1. - buffer size : 0 - Cap4 CaptureRangeBearing -> Sen2 <-- - Ftr3 trk0 FeatureRangeBearing <-- - m = ( 1 1.6 ) - Fac3 RANGE BEARING --> Lmk1 - KFrm2 <-- Fac7 - P[Est] = ( 1 5.7e-09 ) - O[Est] = ( 5.7e-09 ) - CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1. + sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation + (See notes 1 and 2 below) PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D o: + Cap6 - KFrm3 // origin at Capture 6, Frame 3 l: Cap8 - Frm4 // last at Capture 8, + Frame 4 Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing sb: P[Sta,Fix] = ( + 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 + below) Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing Trajectory KFrm1 + <-- Fac4 // KeyFrame 1, constrained by Factor 4 P[Est] = ( -4.4e-12 1.5e-09 ) + // Position is estimated O[Est] = ( 2.6e-09 ) // Orientation is estimated Cap1 + CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative + to the prior Ftr1 trk0 prior <-- // Position prior m = ( 0 0 ) Fac1 FactorBlockAbsolute + --> Abs Ftr2 trk0 prior <-- // Orientation prior m = ( 0 ) Fac2 FactorBlockAbsolute --> + Abs CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1. buffer + size : 0 Cap4 CaptureRangeBearing -> Sen2 <-- Ftr3 trk0 FeatureRangeBearing <-- m = ( 1 1.6 ) Fac3 RANGE + BEARING --> Lmk1 KFrm2 <-- Fac7 P[Est] = ( 1 5.7e-09 ) O[Est] = ( 5.7e-09 ) CapM3 CaptureOdom2d -> Sen1 -> + OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1. // Its origin is at Capture 2; Frame 1 buffer size : 2 delta preint : (1 0 0) @@ -363,7 +355,8 @@ int main() * Fixed: they are used as constant values, never estimated * Estimated: they are estimated by the solver iteratively * - * Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This produces 4 combinations: + * Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This + produces 4 combinations: * * 1 Fixed + Static : general case of calibrated sensor. * Example: rigidly fixed sensor with calibrated parameters diff --git a/demos/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp index e5dc4bafa4fd87ee6a937f6960ba75aedf40f1d9..481ca4eeaf8829c471b071fedb8610a17b78b84e 100644 --- a/demos/hello_wolf/landmark_point_2d.cpp +++ b/demos/hello_wolf/landmark_point_2d.cpp @@ -23,9 +23,8 @@ namespace wolf { - -LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) : - LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy)) +LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) + : LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy)) { setId(_id); } diff --git a/demos/hello_wolf/landmark_point_2d.h b/demos/hello_wolf/landmark_point_2d.h index d86c3e151a3b7bfa46836216d9d83fdd1f040113..753370bf79aaf3f23ab26dfac42fc81dba0b6476 100644 --- a/demos/hello_wolf/landmark_point_2d.h +++ b/demos/hello_wolf/landmark_point_2d.h @@ -24,14 +24,13 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(LandmarkPoint2d); class LandmarkPoint2d : public LandmarkBase { - public: - LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy); - ~LandmarkPoint2d() override; + public: + LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy); + ~LandmarkPoint2d() override; }; } /* namespace wolf */ diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp index be6851b6f986171e18236dad999c37f86436d1b8..5475edd8941efaa89b364b513481c1de6fa8c69f 100644 --- a/demos/hello_wolf/processor_range_bearing.cpp +++ b/demos/hello_wolf/processor_range_bearing.cpp @@ -26,9 +26,8 @@ namespace wolf { - -ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params) : - ProcessorBase("ProcessorRangeBearing", 2, _params) +ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params) + : ProcessorBase("ProcessorRangeBearing", 2, _params) { // } @@ -43,21 +42,22 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) // 1. get KF FrameBasePtr keyframe(nullptr); - if ( !buffer_frame_.empty() ) + if (!buffer_frame_.empty()) { // KeyFrame Callback received - keyframe = buffer_frame_.select( _capture->getTimeStamp(), getTimeTolerance() ); + keyframe = buffer_frame_.select(_capture->getTimeStamp(), getTimeTolerance()); - buffer_frame_.removeUpTo( _capture->getTimeStamp() ); + buffer_frame_.removeUpTo(_capture->getTimeStamp()); - assert( keyframe && "Callback KF is not close enough to _capture!"); + assert(keyframe && "Callback KF is not close enough to _capture!"); } if (!keyframe) { // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below) keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp()); - assert( (fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < getTimeTolerance()) && "Could not find a KF close enough to _capture!"); + assert((fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < getTimeTolerance()) && + "Could not find a KF close enough to _capture!"); } // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe @@ -68,14 +68,14 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++) { // extract info - int id = capture_rb->getId (i); - double range = capture_rb->getRange (i); - double bearing = capture_rb->getBearing(i); + int id = capture_rb->getId(i); + double range = capture_rb->getRange(i); + double bearing = capture_rb->getBearing(i); // 4. create or recover landmark LandmarkPoint2dPtr lmk; - auto lmk_it = known_lmks.find(id); - if ( lmk_it != known_lmks.end() ) + auto lmk_it = known_lmks.find(id); + if (lmk_it != known_lmks.end()) { // known landmarks : recover landmark lmk = std::static_pointer_cast<LandmarkPoint2d>(lmk_it->second); @@ -92,25 +92,16 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) } // 5. create feature - Vector2d measurement_rb(range,bearing); - auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb, - measurement_rb, - getSensor()->computeNoiseCov(measurement_rb)); + Vector2d measurement_rb(range, bearing); + auto ftr = FeatureBase::emplace<FeatureRangeBearing>( + capture_rb, measurement_rb, getSensor()->computeNoiseCov(measurement_rb)); // 6. create factor auto prc = shared_from_this(); - auto fac = FactorBase::emplace<FactorRangeBearing>(ftr, - capture_rb, - ftr, - lmk, - prc, - false, - FAC_ACTIVE); + auto fac = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb, ftr, lmk, prc, false, FAC_ACTIVE); } - } - Eigen::Vector2d ProcessorRangeBearing::observe(const Eigen::Vector2d& lmk_w) const { return polar(toSensor(lmk_w)); @@ -123,14 +114,14 @@ Eigen::Vector2d ProcessorRangeBearing::invObserve(double r, double b) const ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3d& _pose) const { - Trf H = Eigen::Translation<double,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2)) ; + Trf H = Eigen::Translation<double, 2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2)); return H; } ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const { - Trf H = Eigen::Translation<double,2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0)) ; + Trf H = Eigen::Translation<double, 2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0)); return H; } @@ -142,7 +133,7 @@ Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const { -// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState()); + // Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState()); Trf H_w_r = transform(getProblem()->getState().vector("PO")); return (H_w_r * H_r_s).inverse() * lmk_w; } @@ -162,11 +153,11 @@ Eigen::Vector2d ProcessorRangeBearing::rect(double range, double bearing) const bool ProcessorRangeBearing::storeKeyFrame(FrameBasePtr _frame_ptr) { - return true; + return true; } bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr) { - return false; + return false; } } /* namespace wolf */ @@ -175,5 +166,4 @@ bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr) namespace wolf { WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing) -} // namespace wolf - +} // namespace wolf diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h index e4b893c9ad60ef6bb5f2e8ad81866e0733379b7a..64e98aaf45e8a7a2c5ba4300ce1e9ba1440149bc 100644 --- a/demos/hello_wolf/processor_range_bearing.h +++ b/demos/hello_wolf/processor_range_bearing.h @@ -28,59 +28,72 @@ namespace wolf { - using namespace Eigen; WOLF_PTR_TYPEDEFS(ProcessorRangeBearing); class ProcessorRangeBearing : public ProcessorBase { - public: - typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf; + public: + typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf; + + ProcessorRangeBearing(const YAML::Node& _params); + ~ProcessorRangeBearing() override + { /* empty */ + } + void configure(SensorBasePtr _sensor) override; - ProcessorRangeBearing(const YAML::Node& _params); - ~ProcessorRangeBearing() override {/* empty */} - void configure(SensorBasePtr _sensor) override; + // Factory method for high level API + WOLF_PROCESSOR_CREATE(ProcessorRangeBearing); - // Factory method for high level API - WOLF_PROCESSOR_CREATE(ProcessorRangeBearing); + protected: + // Implementation of pure virtuals from ProcessorBase + void processCapture(CaptureBasePtr _capture) override; + void processKeyFrame(FrameBasePtr _keyframe_ptr) override{}; + bool triggerInCapture(CaptureBasePtr) const override + { + return true; + }; + bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override + { + return false; + } + bool voteForKeyFrame() const override + { + return false; + } - protected: - // Implementation of pure virtuals from ProcessorBase - void processCapture (CaptureBasePtr _capture) override; - void processKeyFrame (FrameBasePtr _keyframe_ptr) override {}; - bool triggerInCapture (CaptureBasePtr) const override { return true;}; - bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr) const override {return false;} - bool voteForKeyFrame () const override {return false;} + /** \brief store key frame + * + * Returns true if the key frame should be stored + */ + bool storeKeyFrame(FrameBasePtr) override; - /** \brief store key frame - * - * Returns true if the key frame should be stored - */ - bool storeKeyFrame(FrameBasePtr) override; + /** \brief store capture + * + * Returns true if the capture should be stored + */ + bool storeCapture(CaptureBasePtr) override; - /** \brief store capture - * - * Returns true if the capture should be stored - */ - bool storeCapture(CaptureBasePtr) override; + private: + // control variables + Trf H_r_s; // transformation matrix, robot to sensor + std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far - private: - // control variables - Trf H_r_s; // transformation matrix, robot to sensor - std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far + protected: + // helper methods -- to be used only here -- they would be better off in a separate library e.g. + // range_bearing_tools.h + Eigen::Vector2d observe(const Eigen::Vector2d& lmk_w) const; + Eigen::Vector2d invObserve(double r, double b) const; - protected: - // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h - Eigen::Vector2d observe (const Eigen::Vector2d& lmk_w) const; - Eigen::Vector2d invObserve (double r, double b) const; - private: - // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h - Trf transform (const Eigen::Vector3d& _pose) const; - Trf transform (const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const; - Eigen::Vector2d fromSensor (const Eigen::Vector2d& lmk_s) const; - Eigen::Vector2d toSensor (const Eigen::Vector2d& lmk_w) const; - Eigen::Vector2d polar (const Eigen::Vector2d& rect) const; - Eigen::Vector2d rect (double range, double bearing) const; + private: + // helper methods -- to be used only here -- they would be better off in a separate library e.g. + // range_bearing_tools.h + Trf transform(const Eigen::Vector3d& _pose) const; + Trf transform(const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const; + Eigen::Vector2d fromSensor(const Eigen::Vector2d& lmk_s) const; + Eigen::Vector2d toSensor(const Eigen::Vector2d& lmk_w) const; + Eigen::Vector2d polar(const Eigen::Vector2d& rect) const; + Eigen::Vector2d rect(double range, double bearing) const; }; inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor) diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp index b16764688260de4c88e31f2a95887f2526ca0121..2aea3a8c42a4fa8b5fa10891beaa07fec5dbf985 100644 --- a/demos/hello_wolf/sensor_range_bearing.cpp +++ b/demos/hello_wolf/sensor_range_bearing.cpp @@ -22,7 +22,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(SensorRangeBearing); SensorRangeBearing::SensorRangeBearing(const YAML::Node& _params) diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h index 1ba1a2b0c7a90ef7642058d60a5865cd96faf8f7..aca391c4a1794f622d9c6196c0ff363a93e0810e 100644 --- a/demos/hello_wolf/sensor_range_bearing.h +++ b/demos/hello_wolf/sensor_range_bearing.h @@ -25,7 +25,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(SensorRangeBearing) class SensorRangeBearing : public SensorBase diff --git a/demos/solver/test_SPQR.cpp b/demos/solver/test_SPQR.cpp index 66a17bdeb11a51b4e4e52e840eaec751b0254373..010ecb423dd1d324d8472578c5e8f4888b957834 100644 --- a/demos/solver/test_SPQR.cpp +++ b/demos/solver/test_SPQR.cpp @@ -25,20 +25,20 @@ using namespace Eigen; -int main (int argc, char **argv) +int main(int argc, char **argv) { /////////////////////////////////////////////////////////////////////// // Eigen Support SPQR - SPQR < SparseMatrix<double> > solver; - //solver.setSPQROrdering(0); // no ordering -> segmentation fault + SPQR<SparseMatrix<double> > solver; + // solver.setSPQROrdering(0); // no ordering -> segmentation fault - SparseMatrix<double> matA(4,3); - matA.coeffRef(0,0) = 0.1; - matA.coeffRef(1,0) = 0.4; - matA.coeffRef(1,1) = 0.2; - matA.coeffRef(2,1) = 0.4; - matA.coeffRef(2,2) = 0.2; - matA.coeffRef(3,2) = 0.1; + SparseMatrix<double> matA(4, 3); + matA.coeffRef(0, 0) = 0.1; + matA.coeffRef(1, 0) = 0.4; + matA.coeffRef(1, 1) = 0.2; + matA.coeffRef(2, 1) = 0.4; + matA.coeffRef(2, 2) = 0.2; + matA.coeffRef(3, 2) = 0.1; std::cout << "matA: " << std::endl << matA << std::endl; @@ -60,46 +60,46 @@ int main (int argc, char **argv) /////////////////////////////////////////////////////////////////////// // Directly in suitesparse - cholmod_common Common, *cc ; - cholmod_sparse A ; - cholmod_dense *X, *B, *Residual ; - double rnorm, one [2] = {1,0}, minusone [2] = {-1,0} ; - int mtype ; + cholmod_common Common, *cc; + cholmod_sparse A; + cholmod_dense *X, *B, *Residual; + double rnorm, one[2] = {1, 0}, minusone[2] = {-1, 0}; + int mtype; // start CHOLMOD - cc = &Common ; - cholmod_l_start (cc) ; + cc = &Common; + cholmod_l_start(cc); // load A A = viewAsCholmod(matA); - //A = (cholmod_sparse *) cholmod_l_read_matrix (stdin, 1, &mtype, cc) ; + // A = (cholmod_sparse *) cholmod_l_read_matrix (stdin, 1, &mtype, cc) ; std::cout << "A.xtype " << A.xtype << std::endl; std::cout << "A.nrow " << A.nrow << std::endl; std::cout << "A.ncol " << A.ncol << std::endl; // B = ones (size (A,1),1) - B = cholmod_l_ones (A.nrow, 1, A.xtype, cc) ; + B = cholmod_l_ones(A.nrow, 1, A.xtype, cc); std::cout << "2" << std::endl; // X = A\B - //X = SuiteSparseQR <double> (0, SPQR_DEFAULT_TOL, &A, B, cc) ; - X = SuiteSparseQR <double> (&A, B, cc); + // X = SuiteSparseQR <double> (0, SPQR_DEFAULT_TOL, &A, B, cc) ; + X = SuiteSparseQR<double>(&A, B, cc); std::cout << "3" << std::endl; // rnorm = norm (B-A*X) - Residual = cholmod_l_copy_dense (B, cc) ; + Residual = cholmod_l_copy_dense(B, cc); std::cout << "4" << std::endl; - cholmod_l_sdmult (&A, 0, minusone, one, X, Residual, cc) ; + cholmod_l_sdmult(&A, 0, minusone, one, X, Residual, cc); std::cout << "5" << std::endl; - rnorm = cholmod_l_norm_dense (Residual, 2, cc) ; - printf ("2-norm of residual: %8.1e\n", rnorm) ; - printf ("rank %ld\n", cc->SPQR_istat [4]) ; + rnorm = cholmod_l_norm_dense(Residual, 2, cc); + printf("2-norm of residual: %8.1e\n", rnorm); + printf("rank %ld\n", cc->SPQR_istat[4]); // free everything and finish CHOLMOD - cholmod_l_free_dense (&Residual, cc) ; - //cholmod_l_free_sparse (A, cc) ; - cholmod_l_free_dense (&X, cc) ; - cholmod_l_free_dense (&B, cc) ; - cholmod_l_finish (cc) ; - return (0) ; + cholmod_l_free_dense(&Residual, cc); + // cholmod_l_free_sparse (A, cc) ; + cholmod_l_free_dense(&X, cc); + cholmod_l_free_dense(&B, cc); + cholmod_l_finish(cc); + return (0); } diff --git a/demos/solver/test_ccolamd.cpp b/demos/solver/test_ccolamd.cpp index 5a9bef0010a0fa7c56ff92834e99f13e38290d1b..5140f30ccd03340f34af8d8e5a53c17900a36a56 100644 --- a/demos/solver/test_ccolamd.cpp +++ b/demos/solver/test_ccolamd.cpp @@ -21,7 +21,7 @@ // Wolf includes #include "core/common/wolf.h" -//std includes +// std includes #include <cstdlib> #include <iostream> #include <fstream> @@ -42,7 +42,7 @@ using namespace Eigen; using namespace wolf; -//main +// main int main(int argc, char *argv[]) { if (argc != 2 || atoi(argv[1]) < 1) @@ -54,25 +54,25 @@ int main(int argc, char *argv[]) } SizeEigen size = atoi(argv[1]); - SparseMatrix<double, ColMajor, SizeEigen> A(size, size), Aordered(size, size); - CholmodSupernodalLLT < SparseMatrix<double, ColMajor, SizeEigen> > solver; - PermutationMatrix<Dynamic, Dynamic, SizeEigen> perm(size); - CCOLAMDOrdering<SizeEigen> ordering; + SparseMatrix<double, ColMajor, SizeEigen> A(size, size), Aordered(size, size); + CholmodSupernodalLLT<SparseMatrix<double, ColMajor, SizeEigen> > solver; + PermutationMatrix<Dynamic, Dynamic, SizeEigen> perm(size); + CCOLAMDOrdering<SizeEigen> ordering; Matrix<SizeEigen, Dynamic, 1> ordering_factors = Matrix<SizeEigen, Dynamic, 1>::Ones(size); - VectorXd b(size), bordered(size), xordered(size), x(size); - clock_t t1, t2, t3; - double time1, time2, time3; + VectorXd b(size), bordered(size), xordered(size), x(size); + clock_t t1, t2, t3; + double time1, time2, time3; // BUILD THE PROBLEM ---------------------------- - //Fill A & b + // Fill A & b A.insert(0, 0) = 5; - b(0) = 1; + b(0) = 1; for (int i = 1; i < size; i++) { - A.insert(i, i) = 5; + A.insert(i, i) = 5; A.insert(i, i - 1) = 1; A.insert(i - 1, i) = 1; - b(i) = i + 1; + b(i) = i + 1; } A.insert(size - 1, 0) = 2; A.insert(0, size - 1) = 2; @@ -89,15 +89,15 @@ int main(int argc, char *argv[]) std::cout << "decomposition failed" << std::endl; return 0; } - x = solver.solve(b); - time1 = ((double) clock() - t1) / CLOCKS_PER_SEC; + x = solver.solve(b); + time1 = ((double)clock() - t1) / CLOCKS_PER_SEC; std::cout << "solved in " << time1 << "seconds" << std::endl; std::cout << "x = " << x.transpose() << std::endl; // SOLVING AFTER REORDERING ------------------------------------ // ordering factors - ordering_factors(size-1) = 2; - ordering_factors(0) = 2; + ordering_factors(size - 1) = 2; + ordering_factors(0) = 2; // ordering t2 = clock(); @@ -121,7 +121,7 @@ int main(int argc, char *argv[]) return 0; } xordered = solver.solve(bordered); - time2 = ((double) clock() - t2) / CLOCKS_PER_SEC; + time2 = ((double)clock() - t2) / CLOCKS_PER_SEC; std::cout << "solved in " << time2 << "seconds" << std::endl; std::cout << "x = " << (perm.inverse() * xordered).transpose() << std::endl; std::cout << "x = " << x.transpose() << " (solution without reordering)" << std::endl; @@ -135,9 +135,8 @@ int main(int argc, char *argv[]) std::cout << "decomposition failed" << std::endl; return 0; } - x = solver2.solve(b); - time3 = ((double) clock() - t3) / CLOCKS_PER_SEC; + x = solver2.solve(b); + time3 = ((double)clock() - t3) / CLOCKS_PER_SEC; std::cout << "solved in " << time3 << "seconds" << std::endl; std::cout << "x = " << x.transpose() << std::endl; } - diff --git a/demos/solver/test_ccolamd_blocks.cpp b/demos/solver/test_ccolamd_blocks.cpp index 3351beb346b22daad3d0aeb6a74aa49509477e6b..b3011a4c80ace99f688f34526549447abf416387 100644 --- a/demos/solver/test_ccolamd_blocks.cpp +++ b/demos/solver/test_ccolamd_blocks.cpp @@ -18,7 +18,7 @@ // 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/>. -//std includes +// std includes #include <cstdlib> #include <iostream> #include <fstream> @@ -38,36 +38,43 @@ using namespace Eigen; -void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols) +void eraseSparseBlock(SparseMatrix<double>& original, + const unsigned int& row, + const unsigned int& Nrows, + const unsigned int& col, + const unsigned int& Ncols) { - for (unsigned int i = row; i < row + Nrows; i++) - for (unsigned int j = col; j < row + Ncols; j++) - original.coeffRef(i,j) = 0.0; + for (unsigned int i = row; i < row + Nrows; i++) + for (unsigned int j = col; j < row + Ncols; j++) original.coeffRef(i, j) = 0.0; - original.makeCompressed(); + original.makeCompressed(); } -void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col) +void addSparseBlock(const MatrixXd& ins, + SparseMatrix<double>& original, + const unsigned int& row, + const unsigned int& col) { - for (unsigned int r=0; r<ins.rows(); ++r) - for (unsigned int c = 0; c < ins.cols(); c++) - original.coeffRef(r + row, c + col) += ins(r,c); + for (unsigned int r = 0; r < ins.rows(); ++r) + for (unsigned int c = 0; c < ins.cols(); c++) original.coeffRef(r + row, c + col) += ins(r, c); } -void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size) +void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int>& perm, + PermutationMatrix<Dynamic, Dynamic, int>& perm_blocks, + const int dim, + const int size) { ArrayXXi idx(dim, size); idx.row(0) = dim * perm.indices().transpose(); - for (int i = 1; i<dim; i++) - idx.row(i) = idx.row(i-1) + 1; - Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1); + for (int i = 1; i < dim; i++) idx.row(i) = idx.row(i - 1) + 1; + Map<ArrayXi> idx_blocks(idx.data(), dim * size, 1); perm_blocks.indices() = idx_blocks; } -//main -int main(int argc, char *argv[]) +// main +int main(int argc, char* argv[]) { - if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1) + if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1) { std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl; std::cout << " - SIZE: integer size of the problem" << std::endl; @@ -76,43 +83,45 @@ int main(int argc, char *argv[]) return -1; } int size = atoi(argv[1]); - int dim = atoi(argv[2]); - - SparseMatrix<double> H(size * dim, size * dim), H_ordered(size * dim, size * dim), H_block_ordered(size * dim, size * dim); - SparseMatrix<int> FactorMatrix(size,size); - CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3; - PermutationMatrix<Dynamic, Dynamic, int> perm(size), perm_blocks(size * dim); - CCOLAMDOrdering<int> ordering; - VectorXi block_ordering_factors = VectorXi::Ones(size); - VectorXi ordering_factors = VectorXi::Ones(size*dim); - VectorXd b(size * dim), b_ordered(size * dim), b_block_ordered(size * dim), x_block_ordered(size * dim), x_ordered(size * dim), x(size * dim); + int dim = atoi(argv[2]); + + SparseMatrix<double> H(size * dim, size * dim), H_ordered(size * dim, size * dim), + H_block_ordered(size * dim, size * dim); + SparseMatrix<int> FactorMatrix(size, size); + CholmodSupernodalLLT<SparseMatrix<double> > solver, solver2, solver3; + PermutationMatrix<Dynamic, Dynamic, int> perm(size), perm_blocks(size * dim); + CCOLAMDOrdering<int> ordering; + VectorXi block_ordering_factors = VectorXi::Ones(size); + VectorXi ordering_factors = VectorXi::Ones(size * dim); + VectorXd b(size * dim), b_ordered(size * dim), b_block_ordered(size * dim), x_block_ordered(size * dim), + x_ordered(size * dim), x(size * dim); clock_t t1, t2, t3; - double time1, time2, time3; + double time1, time2, time3; MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim); // BUILD THE PROBLEM ---------------------------- - //Fill H & b + // Fill H & b for (int i = 0; i < size; i++) { - addSparseBlock(5*omega, H, i*dim, i*dim); - FactorMatrix.insert(i,i) = 1; + addSparseBlock(5 * omega, H, i * dim, i * dim); + FactorMatrix.insert(i, i) = 1; if (i > 0) { - addSparseBlock(omega, H, i*dim, (i-1)*dim); - addSparseBlock(omega, H, (i-1)*dim, i*dim); - FactorMatrix.insert(i,i-1) = 1; - FactorMatrix.insert(i-1,i) = 1; + addSparseBlock(omega, H, i * dim, (i - 1) * dim); + addSparseBlock(omega, H, (i - 1) * dim, i * dim); + FactorMatrix.insert(i, i - 1) = 1; + FactorMatrix.insert(i - 1, i) = 1; } - b.segment(i*dim, dim) = VectorXd::Constant(dim, i+1); + b.segment(i * dim, dim) = VectorXd::Constant(dim, i + 1); } - addSparseBlock(2*omega, H, 0, (size - 1)*dim); - addSparseBlock(2*omega, H, (size-1)*dim, 0); - FactorMatrix.insert(0,size-1) = 1; - FactorMatrix.insert(size-1,0) = 1; + addSparseBlock(2 * omega, H, 0, (size - 1) * dim); + addSparseBlock(2 * omega, H, (size - 1) * dim, 0); + FactorMatrix.insert(0, size - 1) = 1; + FactorMatrix.insert(size - 1, 0) = 1; std::cout << "Solving factor graph:" << std::endl; - std::cout << "Factors: " << std::endl << FactorMatrix * MatrixXi::Identity(size,size) << std::endl << std::endl; + std::cout << "Factors: " << std::endl << FactorMatrix * MatrixXi::Identity(size, size) << std::endl << std::endl; // SOLVING WITHOUT REORDERING ------------------------------------ // solve Hx = b @@ -123,13 +132,13 @@ int main(int argc, char *argv[]) std::cout << "decomposition failed" << std::endl; return 0; } - x = solver.solve(b); - time1 = ((double) clock() - t1) / CLOCKS_PER_SEC; + x = solver.solve(b); + time1 = ((double)clock() - t1) / CLOCKS_PER_SEC; // SOLVING AFTER REORDERING ------------------------------------ // ordering factors - ordering_factors.segment(dim * (size-1), dim) = VectorXi::Constant(dim,2); - ordering_factors.segment(0, dim) = VectorXi::Constant(dim,2); + ordering_factors.segment(dim * (size - 1), dim) = VectorXi::Constant(dim, 2); + ordering_factors.segment(0, dim) = VectorXi::Constant(dim, 2); // variable ordering t2 = clock(); @@ -151,23 +160,25 @@ int main(int argc, char *argv[]) } x_ordered = solver2.solve(b_ordered); x_ordered = perm.inverse() * x_ordered; - time2 = ((double) clock() - t2) / CLOCKS_PER_SEC; + time2 = ((double)clock() - t2) / CLOCKS_PER_SEC; // SOLVING AFTER BLOCK REORDERING ------------------------------------ // ordering factors - block_ordering_factors(size-1) = 2; - block_ordering_factors(0) = 2; + block_ordering_factors(size - 1) = 2; + block_ordering_factors(0) = 2; // block ordering t3 = clock(); FactorMatrix.makeCompressed(); std::cout << "Reordering using Block CCOLAMD:" << std::endl; - std::cout << "block_ordering_factors = " << std::endl << block_ordering_factors.transpose() << std::endl << std::endl; + std::cout << "block_ordering_factors = " << std::endl + << block_ordering_factors.transpose() << std::endl + << std::endl; ordering(FactorMatrix, perm_blocks, block_ordering_factors.data()); // variable ordering - permutation_2_block_permutation(perm_blocks, perm , dim, size); + permutation_2_block_permutation(perm_blocks, perm, dim, size); b_block_ordered = perm * b; H_block_ordered = H.twistedBy(perm); @@ -180,14 +191,13 @@ int main(int argc, char *argv[]) } x_block_ordered = solver3.solve(b_block_ordered); x_block_ordered = perm.inverse() * x_block_ordered; - time3 = ((double) clock() - t3) / CLOCKS_PER_SEC; + time3 = ((double)clock() - t3) / CLOCKS_PER_SEC; // RESULTS ------------------------------------ - std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl; - std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl; - std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl; - //std::cout << "x = " << x.transpose() << std::endl; - //std::cout << "x = " << x_ordered.transpose() << std::endl; - //std::cout << "x = " << x_block_ordered.transpose() << std::endl; + std::cout << "NO REORDERING: solved in " << time1 * 1e3 << " ms" << std::endl; + std::cout << "REORDERING: solved in " << time2 * 1e3 << " ms" << std::endl; + std::cout << "BLOCK REORDERING: solved in " << time3 * 1e3 << " ms" << std::endl; + // std::cout << "x = " << x.transpose() << std::endl; + // std::cout << "x = " << x_ordered.transpose() << std::endl; + // std::cout << "x = " << x_block_ordered.transpose() << std::endl; } - diff --git a/demos/solver/test_iQR.cpp b/demos/solver/test_iQR.cpp index 4ab0605124df3d852b869c5707c613e061dd017c..4b09adc757924a167df88bcb74130db1f5dffa6a 100644 --- a/demos/solver/test_iQR.cpp +++ b/demos/solver/test_iQR.cpp @@ -18,7 +18,7 @@ // 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/>. -//std includes +// std includes #include <cstdlib> #include <iostream> #include <fstream> @@ -40,72 +40,76 @@ using namespace Eigen; class block_pruning { - public: - int col, row, Nrows, Ncols; - block_pruning(int _col, int _row, int _Nrows, int _Ncols) : - col(_col), - row(_row), - Nrows(_Nrows), - Ncols(_Ncols) - { - // - } - bool operator()(int i, int j, double) const - { - return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); - } + public: + int col, row, Nrows, Ncols; + block_pruning(int _col, int _row, int _Nrows, int _Ncols) : col(_col), row(_row), Nrows(_Nrows), Ncols(_Ncols) + { + // + } + bool operator()(int i, int j, double) const + { + return (i < row || i > row + Nrows - 1) || (j < col || j > col + Ncols - 1); + } }; -void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols) +void eraseSparseBlock(SparseMatrix<double>& original, + const unsigned int& row, + const unsigned int& col, + const unsigned int& Nrows, + const unsigned int& Ncols) { // prune all non-zero elements that not satisfy the 'keep' operand // elements that are not in the block rows or are not in the block columns should be kept - //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); }); + // original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + + // Ncols-1); }); block_pruning bp(row, col, Nrows, Ncols); original.prune(bp); -// for (unsigned int i = row; i < row + Nrows; i++) -// for (unsigned int j = col; j < row + Ncols; j++) -// original.coeffRef(i,j) = 0.0; -// -// original.prune(0); + // for (unsigned int i = row; i < row + Nrows; i++) + // for (unsigned int j = col; j < row + Ncols; j++) + // original.coeffRef(i,j) = 0.0; + // + // original.prune(0); } -void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col) +void addSparseBlock(const MatrixXd& ins, + SparseMatrix<double>& original, + const unsigned int& row, + const unsigned int& col) { - for (unsigned int r=0; r<ins.rows(); ++r) - for (unsigned int c = 0; c < ins.cols(); c++) - if (ins(r,c) != 0) - original.coeffRef(r + row, c + col) += ins(r,c); + for (unsigned int r = 0; r < ins.rows(); ++r) + for (unsigned int c = 0; c < ins.cols(); c++) + if (ins(r, c) != 0) original.coeffRef(r + row, c + col) += ins(r, c); } -void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables, const int dim) +void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int>& perm_nodes, + PermutationMatrix<Dynamic, Dynamic, int>& perm_variables, + const int dim) { ArrayXXi idx(dim, perm_nodes.indices().rows()); idx.row(0) = dim * perm_nodes.indices().transpose(); - for (int i = 1; i<dim; i++) - idx.row(i) = idx.row(i-1) + 1; - Map<ArrayXi> idx_blocks(idx.data(), dim*perm_nodes.indices().rows(), 1); + for (int i = 1; i < dim; i++) idx.row(i) = idx.row(i - 1) + 1; + Map<ArrayXi> idx_blocks(idx.data(), dim * perm_nodes.indices().rows(), 1); perm_variables.indices() = idx_blocks; } -void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size) +void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int>& perm, const int new_size) { - int old_size = perm.indices().size(); - int dim = new_size - old_size; + int old_size = perm.indices().size(); + int dim = new_size - old_size; VectorXi new_indices(new_size); - new_indices.head(old_size)= perm.indices(); - new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1); + new_indices.head(old_size) = perm.indices(); + new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size - 1); perm.resize(new_size); perm.indices() = new_indices; } -//main -int main(int argc, char *argv[]) +// main +int main(int argc, char* argv[]) { - if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1) + if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1) { std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl; std::cout << " - SIZE: integer size of the problem" << std::endl; @@ -114,32 +118,27 @@ int main(int argc, char *argv[]) return -1; } int size = atoi(argv[1]); - int dim = atoi(argv[2]); + int dim = atoi(argv[2]); // Problem variables - SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_ordered, solver_unordered, solver_ordered_partial; - - MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim); - SparseMatrix<double> A(0,0), - A_ordered(0,0), - R(0,0); - VectorXd b, - x, - x_ordered, - x_ordered_partial; - int n_measurements = 0; - int n_nodes = 0; + SparseQR<SparseMatrix<double>, NaturalOrdering<int>> solver_ordered, solver_unordered, solver_ordered_partial; + + MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim); + SparseMatrix<double> A(0, 0), A_ordered(0, 0), R(0, 0); + VectorXd b, x, x_ordered, x_ordered_partial; + int n_measurements = 0; + int n_nodes = 0; // ordering variables - SparseMatrix<int> A_nodes_ordered(0,0); + SparseMatrix<int> A_nodes_ordered(0, 0); PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix(0); CCOLAMDOrdering<int> ordering, partial_ordering; - VectorXi nodes_ordering_factors; + VectorXi nodes_ordering_factors; // results variables clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4; - double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0; + double time_ordering = 0, time_solving_unordered = 0, time_solving_ordered = 0, time_solving_ordered_partial = 0; std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl; @@ -148,20 +147,20 @@ int main(int argc, char *argv[]) for (int i = 0; i < size; i++) { std::vector<int> meas(0); - if (i == 0) //prior + if (i == 0) // prior { meas.push_back(0); measurements.push_back(meas); meas.clear(); } - else //odometry + else // odometry { - meas.push_back(i-1); + meas.push_back(i - 1); meas.push_back(i); measurements.push_back(meas); meas.clear(); } - if (i > size / 2) // loop closures + if (i > size / 2) // loop closures { meas.push_back(0); meas.push_back(i); @@ -178,22 +177,22 @@ int main(int argc, char *argv[]) // AUGMENT THE PROBLEM ---------------------------- n_measurements++; - while (n_nodes < measurement.back()+1) + while (n_nodes < measurement.back() + 1) { n_nodes++; // Resize accumulated permutations augment_permutation(acc_permutation_nodes_matrix, n_nodes); // Resize state - x.conservativeResize(n_nodes*dim); - x_ordered.conservativeResize(n_nodes*dim); - x_ordered_partial.conservativeResize(n_nodes*dim); + x.conservativeResize(n_nodes * dim); + x_ordered.conservativeResize(n_nodes * dim); + x_ordered_partial.conservativeResize(n_nodes * dim); } - A.conservativeResize(n_measurements*dim,n_nodes*dim); - A_ordered.conservativeResize(n_measurements*dim,n_nodes*dim); - R.conservativeResize(n_nodes*dim,n_nodes*dim); - b.conservativeResize(n_measurements*dim); - A_nodes_ordered.conservativeResize(n_measurements,n_nodes); + A.conservativeResize(n_measurements * dim, n_nodes * dim); + A_ordered.conservativeResize(n_measurements * dim, n_nodes * dim); + R.conservativeResize(n_nodes * dim, n_nodes * dim); + b.conservativeResize(n_measurements * dim); + A_nodes_ordered.conservativeResize(n_measurements, n_nodes); // ADD MEASUREMENTS int min_ordered_node = n_nodes; @@ -201,31 +200,32 @@ int main(int argc, char *argv[]) { int ordered_node = acc_permutation_nodes_matrix.indices()(measurement.at(j)); - addSparseBlock(2*omega, A, A.rows()-dim, measurement.at(j) * dim); - addSparseBlock(2*omega, A_ordered, A_ordered.rows()-dim, ordered_node * dim); + addSparseBlock(2 * omega, A, A.rows() - dim, measurement.at(j) * dim); + addSparseBlock(2 * omega, A_ordered, A_ordered.rows() - dim, ordered_node * dim); - A_nodes_ordered.coeffRef(A_nodes_ordered.rows()-1, ordered_node) = 1; + A_nodes_ordered.coeffRef(A_nodes_ordered.rows() - 1, ordered_node) = 1; - b.segment(b.size() - dim, dim) = VectorXd::LinSpaced(dim, b.size()-dim, b.size()-1); + b.segment(b.size() - dim, dim) = VectorXd::LinSpaced(dim, b.size() - dim, b.size() - 1); // store minimum ordered node - if (min_ordered_node > ordered_node) - min_ordered_node = ordered_node; + if (min_ordered_node > ordered_node) min_ordered_node = ordered_node; } -// std::cout << "Solving Ax = b" << std::endl; -// std::cout << "A_nodes_ordered: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << std::endl; -// std::cout << "A: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A << std::endl << std::endl; -// std::cout << "A_ordered: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A_ordered << std::endl << std::endl; -// std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl; + // std::cout << "Solving Ax = b" << std::endl; + // std::cout << "A_nodes_ordered: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), + // A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << std::endl; std::cout << "A: " << std::endl + // << MatrixXd::Identity(A.rows(), A.rows()) * A << std::endl << std::endl; std::cout << "A_ordered: " + // << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A_ordered << std::endl << std::endl; + // std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl; // BLOCK REORDERING ------------------------------------ - t_ordering = clock(); - int ordered_nodes = n_nodes - min_ordered_node; + t_ordering = clock(); + int ordered_nodes = n_nodes - min_ordered_node; int unordered_nodes = n_nodes - ordered_nodes; - if (n_nodes > 1 && ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones + if (n_nodes > 1 && + ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones { // SUBPROBLEM ORDERING (from first node variable to last one) - std::cout << "ordering partial problem: " << min_ordered_node << " to "<< n_nodes - 1 << std::endl; + std::cout << "ordering partial problem: " << min_ordered_node << " to " << n_nodes - 1 << std::endl; SparseMatrix<int> sub_A_nodes_ordered = A_nodes_ordered.rightCols(ordered_nodes); // partial ordering factors @@ -234,26 +234,36 @@ int main(int argc, char *argv[]) // computing nodes partial ordering A_nodes_ordered.makeCompressed(); PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes_matrix(ordered_nodes); - partial_ordering(sub_A_nodes_ordered, partial_permutation_nodes_matrix, nodes_partial_ordering_factors.data()); + partial_ordering( + sub_A_nodes_ordered, partial_permutation_nodes_matrix, nodes_partial_ordering_factors.data()); // node ordering to variable ordering PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_matrix(A_ordered.cols()); - permutation_2_block_permutation(partial_permutation_nodes_matrix, partial_permutation_matrix , dim); + permutation_2_block_permutation(partial_permutation_nodes_matrix, partial_permutation_matrix, dim); // apply partial orderings - A_nodes_ordered.rightCols(ordered_nodes) = (A_nodes_ordered.rightCols(ordered_nodes) * partial_permutation_nodes_matrix.transpose()).sparseView(); - A_ordered.rightCols(ordered_nodes * dim) = (A_ordered.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView(); - R.rightCols(ordered_nodes * dim) = (R.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView(); + A_nodes_ordered.rightCols(ordered_nodes) = + (A_nodes_ordered.rightCols(ordered_nodes) * partial_permutation_nodes_matrix.transpose()).sparseView(); + A_ordered.rightCols(ordered_nodes * dim) = + (A_ordered.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView(); + R.rightCols(ordered_nodes * dim) = + (R.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView(); // ACCUMULATING PERMUTATIONS - PermutationMatrix<Dynamic, Dynamic, int> permutation_nodes_matrix(VectorXi::LinSpaced(n_nodes, 0, n_nodes - 1)); // identity permutation - permutation_nodes_matrix.indices().tail(ordered_nodes) = partial_permutation_nodes_matrix.indices() + VectorXi::Constant(ordered_nodes, n_nodes - ordered_nodes); + PermutationMatrix<Dynamic, Dynamic, int> permutation_nodes_matrix( + VectorXi::LinSpaced(n_nodes, 0, n_nodes - 1)); // identity permutation + permutation_nodes_matrix.indices().tail(ordered_nodes) = + partial_permutation_nodes_matrix.indices() + + VectorXi::Constant(ordered_nodes, n_nodes - ordered_nodes); acc_permutation_nodes_matrix = permutation_nodes_matrix * acc_permutation_nodes_matrix; } - time_ordering += ((double) clock() - t_ordering) / CLOCKS_PER_SEC; - // std::cout << "incrementally ordered A Block structure: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << std::endl; - //std::cout << "ordered A: " << std::endl << MatrixXd::Identity(A_ordered.rows(), A_ordered.rows()) * A_ordered << std::endl << std::endl; - //std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl; + time_ordering += ((double)clock() - t_ordering) / CLOCKS_PER_SEC; + // std::cout << "incrementally ordered A Block structure: " << std::endl << + // MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << + // std::endl; + // std::cout << "ordered A: " << std::endl << MatrixXd::Identity(A_ordered.rows(), A_ordered.rows()) * + // A_ordered << std::endl << std::endl; std::cout << "b: " << std::endl << b.transpose() << std::endl << + // std::endl; // SOLVING // solving ordered subproblem @@ -263,21 +273,27 @@ int main(int argc, char *argv[]) // finding measurements block SparseMatrix<int> measurements_to_initial = A_nodes_ordered.col(min_ordered_node); -// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl; -// std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl; + // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl; + // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << + // measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl; int initial_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]]; - SparseMatrix<double> A_ordered_partial = A_ordered.bottomRightCorner((n_nodes - initial_measurement) * dim, ordered_nodes * dim); + SparseMatrix<double> A_ordered_partial = + A_ordered.bottomRightCorner((n_nodes - initial_measurement) * dim, ordered_nodes * dim); solver_ordered_partial.compute(A_ordered_partial); if (solver_ordered_partial.info() != Success) { std::cout << "decomposition failed" << std::endl; return 0; } - std::cout << "R new" << std::endl << MatrixXd::Identity(ordered_nodes * dim, ordered_nodes * dim) * solver_ordered_partial.matrixR() << std::endl; + std::cout << "R new" << std::endl + << MatrixXd::Identity(ordered_nodes * dim, ordered_nodes * dim) * solver_ordered_partial.matrixR() + << std::endl; x_ordered_partial.tail(ordered_nodes * dim) = solver_ordered_partial.solve(b.tail(ordered_nodes * dim)); - std::cout << "x_ordered_partial.tail(ordered_nodes * dim)" << std::endl << x_ordered_partial.tail(ordered_nodes * dim).transpose() << std::endl; - // store new part of R (equivalent to R.bottomRightCorner(ordered_nodes * dim, ordered_nodes * dim) = solver3.matrixR();) + std::cout << "x_ordered_partial.tail(ordered_nodes * dim)" << std::endl + << x_ordered_partial.tail(ordered_nodes * dim).transpose() << std::endl; + // store new part of R (equivalent to R.bottomRightCorner(ordered_nodes * dim, ordered_nodes * dim) = + // solver3.matrixR();) eraseSparseBlock(R, unordered_nodes * dim, unordered_nodes * dim, ordered_nodes * dim, ordered_nodes * dim); addSparseBlock(solver_ordered_partial.matrixR(), R, unordered_nodes * dim, unordered_nodes * dim); std::cout << "R" << std::endl << MatrixXd::Identity(R.rows(), R.rows()) * R << std::endl; @@ -297,13 +313,14 @@ int main(int argc, char *argv[]) std::cout << "decomposition failed" << std::endl; return 0; } - x_ordered_partial.head(unordered_nodes * dim) = solver_ordered_partial.solve(b.head(unordered_nodes * dim) - R2 * x_ordered_partial.tail(ordered_nodes * dim)); + x_ordered_partial.head(unordered_nodes * dim) = solver_ordered_partial.solve( + b.head(unordered_nodes * dim) - R2 * x_ordered_partial.tail(ordered_nodes * dim)); } // undo ordering PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(A_ordered.cols()); - permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix , dim); + permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix, dim); x_ordered_partial = acc_permutation_matrix.inverse() * x_ordered_partial; - time_solving_ordered_partial += ((double) clock() - t_solving_ordered_partial) / CLOCKS_PER_SEC; + time_solving_ordered_partial += ((double)clock() - t_solving_ordered_partial) / CLOCKS_PER_SEC; // SOLVING // full ordered problem @@ -317,12 +334,13 @@ int main(int argc, char *argv[]) return 0; } x_ordered = solver_ordered.solve(b); - std::cout << "solver_ordered.matrixR()" << std::endl << MatrixXd::Identity(A_ordered.cols(), A_ordered.cols()) * solver_ordered.matrixR() << std::endl; + std::cout << "solver_ordered.matrixR()" << std::endl + << MatrixXd::Identity(A_ordered.cols(), A_ordered.cols()) * solver_ordered.matrixR() << std::endl; // undo ordering PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix2(A_ordered.cols()); - permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix2 , dim); + permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix2, dim); x_ordered = acc_permutation_matrix.inverse() * x_ordered; - time_solving_ordered += ((double) clock() - t_solving_ordered_full) / CLOCKS_PER_SEC; + time_solving_ordered += ((double)clock() - t_solving_ordered_full) / CLOCKS_PER_SEC; // WITHOUT ORDERING t_solving_unordered = clock(); @@ -333,24 +351,29 @@ int main(int argc, char *argv[]) std::cout << "decomposition failed" << std::endl; return 0; } - //std::cout << "no ordering? " << solver_unordered.colsPermutation().indices().transpose() << std::endl; + // std::cout << "no ordering? " << solver_unordered.colsPermutation().indices().transpose() << std::endl; x = solver_unordered.solve(b); - std::cout << "solver_unordered.matrixR()" << std::endl << MatrixXd::Identity(A.cols(), A.cols()) * solver_unordered.matrixR() << std::endl; - time_solving_unordered += ((double) clock() - t_solving_unordered) / CLOCKS_PER_SEC; + std::cout << "solver_unordered.matrixR()" << std::endl + << MatrixXd::Identity(A.cols(), A.cols()) * solver_unordered.matrixR() << std::endl; + time_solving_unordered += ((double)clock() - t_solving_unordered) / CLOCKS_PER_SEC; // RESULTS ------------------------------------ std::cout << "========================= RESULTS " << i << ":" << std::endl; - std::cout << "NO REORDERING: solved in " << time_solving_unordered*1e3 << " ms | " << solver_unordered.matrixR().nonZeros() << " nonzeros in R"<< std::endl; - std::cout << "BLOCK REORDERING: solved in " << time_solving_ordered*1e3 << " ms | " << solver_ordered.matrixR().nonZeros() << " nonzeros in R"<< std::endl; - std::cout << "BLOCK REORDERING 2: solved in " << time_solving_ordered_partial*1e3 << " ms | " << R.nonZeros() << " nonzeros in R"<< std::endl; + std::cout << "NO REORDERING: solved in " << time_solving_unordered * 1e3 << " ms | " + << solver_unordered.matrixR().nonZeros() << " nonzeros in R" << std::endl; + std::cout << "BLOCK REORDERING: solved in " << time_solving_ordered * 1e3 << " ms | " + << solver_ordered.matrixR().nonZeros() << " nonzeros in R" << std::endl; + std::cout << "BLOCK REORDERING 2: solved in " << time_solving_ordered_partial * 1e3 << " ms | " << R.nonZeros() + << " nonzeros in R" << std::endl; std::cout << "x = " << x.transpose() << std::endl; std::cout << "x_ordered = " << x_ordered.transpose() << std::endl; std::cout << "x_ordered_partial = " << x_ordered_partial.transpose() << std::endl; - if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10) - std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl; + if ((x_ordered_partial - x_ordered).maxCoeff() < 1e-10) + std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial - x_ordered).maxCoeff() << ")" + << std::endl; else - std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl; + std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial - x_ordered).maxCoeff() + << std::endl; } } - diff --git a/demos/solver/test_iQR_wolf.cpp b/demos/solver/test_iQR_wolf.cpp index 041057ba18076f53edb210bb0c1c6e36c154d519..ec7ecc7512014f8a91477c2e24ae131756f36564 100644 --- a/demos/solver/test_iQR_wolf.cpp +++ b/demos/solver/test_iQR_wolf.cpp @@ -18,7 +18,7 @@ // 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/>. -//std includes +// std includes #include <cstdlib> #include <string> #include <iostream> @@ -41,457 +41,477 @@ using namespace Eigen; class block_pruning { - public: - int col, row, Nrows, Ncols; - block_pruning(int _col, int _row, int _Nrows, int _Ncols) : - col(_col), - row(_row), - Nrows(_Nrows), - Ncols(_Ncols) - { - // - } - bool operator()(int i, int j, double) const - { - return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); - } + public: + int col, row, Nrows, Ncols; + block_pruning(int _col, int _row, int _Nrows, int _Ncols) : col(_col), row(_row), Nrows(_Nrows), Ncols(_Ncols) + { + // + } + bool operator()(int i, int j, double) const + { + return (i < row || i > row + Nrows - 1) || (j < col || j > col + Ncols - 1); + } }; -void eraseSparseBlock(SparseMatrix<double, ColMajor>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols) +void eraseSparseBlock(SparseMatrix<double, ColMajor>& original, + const unsigned int& row, + const unsigned int& col, + const unsigned int& Nrows, + const unsigned int& Ncols) { // prune all non-zero elements that not satisfy the 'keep' operand // elements that are not in the block rows or are not in the block columns should be kept - //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); }); + // original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + + // Ncols-1); }); block_pruning bp(row, col, Nrows, Ncols); original.prune(bp); -// for (unsigned int i = row; i < row + Nrows; i++) -// for (unsigned int j = col; j < row + Ncols; j++) -// original.coeffRef(i,j) = 0.0; -// -// original.prune(0); + // for (unsigned int i = row; i < row + Nrows; i++) + // for (unsigned int j = col; j < row + Ncols; j++) + // original.coeffRef(i,j) = 0.0; + // + // original.prune(0); } -void addSparseBlock(const MatrixXd& ins, SparseMatrix<double, ColMajor>& original, const unsigned int& row, const unsigned int& col) +void addSparseBlock(const MatrixXd& ins, + SparseMatrix<double, ColMajor>& original, + const unsigned int& row, + const unsigned int& col) { - for (unsigned int r=0; r<ins.rows(); ++r) - for (unsigned int c = 0; c < ins.cols(); c++) - if (ins(r,c) != 0) - original.coeffRef(r + row, c + col) += ins(r,c); + for (unsigned int r = 0; r < ins.rows(); ++r) + for (unsigned int c = 0; c < ins.cols(); c++) + if (ins(r, c) != 0) original.coeffRef(r + row, c + col) += ins(r, c); } struct node { - public: - int id; - int dim; - int location; - int order; - - node(const int _id, const int _dim, const int _location, const int _order) : - id(_id), - dim(_dim), - location(_location), - order(_order) - { - - } + public: + int id; + int dim; + int location; + int order; + + node(const int _id, const int _dim, const int _location, const int _order) + : id(_id), dim(_dim), location(_location), order(_order) + { + } }; struct measurement { - public: - std::vector<MatrixXd> jacobians; - std::vector<int> nodes_idx; - VectorXd error; - int dim; - bool odometry_type; - int location; - - measurement(const MatrixXd & _jacobian1, const int _idx1, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) : - jacobians({_jacobian1}), - nodes_idx({_idx1}), - error(_error), - dim(_meas_dim), - odometry_type(_odometry_type), - location(0) - { - //jacobians.push_back(_jacobian1); - } - - measurement(const MatrixXd & _jacobian1, const int _idx1, const MatrixXd & _jacobian2, const int _idx2, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) : - jacobians({_jacobian1, _jacobian2}), - nodes_idx({_idx1, _idx2}), - error(_error), - dim(_meas_dim), - odometry_type(_odometry_type), - location(0) - { + public: + std::vector<MatrixXd> jacobians; + std::vector<int> nodes_idx; + VectorXd error; + int dim; + bool odometry_type; + int location; + + measurement(const MatrixXd& _jacobian1, + const int _idx1, + const VectorXd& _error, + const int _meas_dim, + bool _odometry_type = false) + : jacobians({_jacobian1}), + nodes_idx({_idx1}), + error(_error), + dim(_meas_dim), + odometry_type(_odometry_type), + location(0) + { + // jacobians.push_back(_jacobian1); + } - } + measurement(const MatrixXd& _jacobian1, + const int _idx1, + const MatrixXd& _jacobian2, + const int _idx2, + const VectorXd& _error, + const int _meas_dim, + bool _odometry_type = false) + : jacobians({_jacobian1, _jacobian2}), + nodes_idx({_idx1, _idx2}), + error(_error), + dim(_meas_dim), + odometry_type(_odometry_type), + location(0) + { + } }; class SolverQR { - protected: - std::string name_; - SparseQR < SparseMatrix<double, ColMajor>, NaturalOrdering<int>> solver_; - SparseMatrix<double, ColMajor> A_, R_; - VectorXd b_, x_incr_; - int n_measurements; - int n_nodes_; - std::vector<node> nodes_; - std::vector<measurement> measurements_; - - // ordering - SparseMatrix<int, ColMajor> A_nodes_; - PermutationMatrix<Dynamic, Dynamic, int> acc_node_permutation_; - - CCOLAMDOrdering<int> orderer_; - VectorXi node_ordering_restrictions_; - int first_ordered_node_; - - // time - clock_t t_ordering_, t_solving_, t_managing_; - double time_ordering_, time_solving_, time_managing_; - - public: - SolverQR(const std::string &_name) : - name_(_name), - A_(0,0), - R_(0,0), -// b_(0), -// x_(0), - n_measurements(0), - n_nodes_(0), - A_nodes_(0,0), - acc_node_permutation_(0), -// nodes_(0), -// measurements_(0), - first_ordered_node_(0), - t_ordering_(0), - t_solving_(0), - t_managing_(0), - time_ordering_(0), - time_solving_(0), - time_managing_(0) - { - // - } + protected: + std::string name_; + SparseQR<SparseMatrix<double, ColMajor>, NaturalOrdering<int>> solver_; + SparseMatrix<double, ColMajor> A_, R_; + VectorXd b_, x_incr_; + int n_measurements; + int n_nodes_; + std::vector<node> nodes_; + std::vector<measurement> measurements_; + + // ordering + SparseMatrix<int, ColMajor> A_nodes_; + PermutationMatrix<Dynamic, Dynamic, int> acc_node_permutation_; + + CCOLAMDOrdering<int> orderer_; + VectorXi node_ordering_restrictions_; + int first_ordered_node_; + + // time + clock_t t_ordering_, t_solving_, t_managing_; + double time_ordering_, time_solving_, time_managing_; + + public: + SolverQR(const std::string& _name) + : name_(_name), + A_(0, 0), + R_(0, 0), + // b_(0), + // x_(0), + n_measurements(0), + n_nodes_(0), + A_nodes_(0, 0), + acc_node_permutation_(0), + // nodes_(0), + // measurements_(0), + first_ordered_node_(0), + t_ordering_(0), + t_solving_(0), + t_managing_(0), + time_ordering_(0), + time_solving_(0), + time_managing_(0) + { + // + } - virtual ~SolverQR() - { - - } + virtual ~SolverQR() {} - void add_state_unit(const int node_dim, const int node_idx) - { - t_managing_ = clock(); + void add_state_unit(const int node_dim, const int node_idx) + { + t_managing_ = clock(); - n_nodes_++; - nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1)); + n_nodes_++; + nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_ - 1)); - // Resize accumulated permutations - augment_permutation(acc_node_permutation_, n_nodes_); + // Resize accumulated permutations + augment_permutation(acc_node_permutation_, n_nodes_); - // Resize state - x_incr_.conservativeResize(x_incr_.size() + node_dim); + // Resize state + x_incr_.conservativeResize(x_incr_.size() + node_dim); - // Resize problem - A_.conservativeResize(A_.rows(), A_.cols() + node_dim); - R_.conservativeResize(R_.cols() + node_dim, R_.cols() + node_dim); - //A_nodes_.conservativeResize(n_measurements, n_nodes); // not necessary + // Resize problem + A_.conservativeResize(A_.rows(), A_.cols() + node_dim); + R_.conservativeResize(R_.cols() + node_dim, R_.cols() + node_dim); + // A_nodes_.conservativeResize(n_measurements, n_nodes); // not necessary - time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC; - } + time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC; + } - void addFactor(const measurement& _meas) + void addFactor(const measurement& _meas) + { + t_managing_ = clock(); + + assert(_meas.jacobians.size() == _meas.nodes_idx.size()); + assert(_meas.error.size() == _meas.dim); + + n_measurements++; + measurements_.push_back(_meas); + measurements_.back().location = A_.rows(); + + // Resize problem + A_.conservativeResize(A_.rows() + _meas.dim, A_.cols()); + b_.conservativeResize(b_.size() + _meas.dim); + A_nodes_.conservativeResize(n_measurements, n_nodes_); + + // ADD MEASUREMENTS + first_ordered_node_ = n_nodes_; + for (unsigned int j = 0; j < _meas.nodes_idx.size(); j++) { - t_managing_ = clock(); + assert(acc_node_permutation_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order); - assert(_meas.jacobians.size() == _meas.nodes_idx.size()); - assert(_meas.error.size() == _meas.dim); + int ordered_node = + nodes_.at(_meas.nodes_idx.at(j)).order; // acc_permutation_nodes_.indices()(_nodes_idx.at(j)); - n_measurements++; - measurements_.push_back(_meas); - measurements_.back().location = A_.rows(); + addSparseBlock( + _meas.jacobians.at(j), A_, A_.rows() - _meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location); - // Resize problem - A_.conservativeResize(A_.rows() + _meas.dim, A_.cols()); - b_.conservativeResize(b_.size() + _meas.dim); - A_nodes_.conservativeResize(n_measurements,n_nodes_); + A_nodes_.coeffRef(A_nodes_.rows() - 1, ordered_node) = 1; - // ADD MEASUREMENTS - first_ordered_node_ = n_nodes_; - for (unsigned int j = 0; j < _meas.nodes_idx.size(); j++) - { - assert(acc_node_permutation_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order); + assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim); + assert(_meas.jacobians.at(j).rows() == _meas.dim); - int ordered_node = nodes_.at(_meas.nodes_idx.at(j)).order;//acc_permutation_nodes_.indices()(_nodes_idx.at(j)); + // store minimum ordered node + if (first_ordered_node_ > ordered_node) first_ordered_node_ = ordered_node; + } - addSparseBlock(_meas.jacobians.at(j), A_, A_.rows()-_meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location); + // error + b_.tail(_meas.dim) = _meas.error; - A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1; + time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC; + } - assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim); - assert(_meas.jacobians.at(j).rows() == _meas.dim); + void ordering(const int& _first_ordered_node) + { + t_ordering_ = clock(); - // store minimum ordered node - if (first_ordered_node_ > ordered_node) - first_ordered_node_ = ordered_node; - } + // full problem ordering + if (_first_ordered_node == 0) + { + // ordering ordering factors + node_ordering_restrictions_.resize(n_nodes_); + node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose(); + + // computing nodes partial ordering_ + A_nodes_.makeCompressed(); + PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_); + orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data()); + + // node ordering to variable ordering + PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols()); + nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation); - // error - b_.tail(_meas.dim) = _meas.error; + // apply partial_ordering orderings + A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView(); + A_ = (A_ * incr_permutation.transpose()).sparseView(); - time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC; + // ACCUMULATING PERMUTATIONS + accumulatePermutation(incr_permutation_nodes); } - void ordering(const int & _first_ordered_node) + // partial ordering + else { - t_ordering_ = clock(); - - // full problem ordering - if (_first_ordered_node == 0) + int ordered_nodes = n_nodes_ - _first_ordered_node; + int unordered_nodes = n_nodes_ - ordered_nodes; + if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones { - // ordering ordering factors - node_ordering_restrictions_.resize(n_nodes_); - node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose(); + // SUBPROBLEM ORDERING (from first node variable to last one) + // std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 + // << std::endl; + SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes); + + // _partial_ordering ordering_ factors + node_ordering_restrictions_.resize(ordered_nodes); + node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose(); // computing nodes partial ordering_ - A_nodes_.makeCompressed(); - PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_); - orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data()); + sub_A_nodes_.makeCompressed(); + PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes); + orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data()); // node ordering to variable ordering - PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols()); - nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation); + PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols()); + nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation); // apply partial_ordering orderings - A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView(); - A_ = (A_ * incr_permutation.transpose()).sparseView(); + int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location; + A_nodes_.rightCols(ordered_nodes) = + (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView(); + A_.rightCols(ordered_variables) = + (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView(); + R_.rightCols(ordered_variables) = + (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView(); // ACCUMULATING PERMUTATIONS - accumulatePermutation(incr_permutation_nodes); + accumulatePermutation(partial_permutation_nodes); } + } + time_ordering_ += ((double)clock() - t_ordering_) / CLOCKS_PER_SEC; + } + + bool solve(const int mode) + { + bool batch = (mode != 2 || first_ordered_node_ == 0); + bool order = (mode != 0 && n_nodes_ > 1); - // partial ordering - else + // BATCH + if (batch) + { + // REORDER + if (order) ordering(0); + + // print_problem(); + + // SOLVE + t_solving_ = clock(); + A_.makeCompressed(); + solver_.compute(A_); + if (solver_.info() != Success) { - int ordered_nodes = n_nodes_ - _first_ordered_node; - int unordered_nodes = n_nodes_ - ordered_nodes; - if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones - { - // SUBPROBLEM ORDERING (from first node variable to last one) - //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl; - SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes); - - // _partial_ordering ordering_ factors - node_ordering_restrictions_.resize(ordered_nodes); - node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose(); - - // computing nodes partial ordering_ - sub_A_nodes_.makeCompressed(); - PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes); - orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data()); - - // node ordering to variable ordering - PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols()); - nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation); - - // apply partial_ordering orderings - int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location; - A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView(); - A_.rightCols(ordered_variables) = (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView(); - R_.rightCols(ordered_variables) = (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView(); - - // ACCUMULATING PERMUTATIONS - accumulatePermutation(partial_permutation_nodes); - } + std::cout << "decomposition failed" << std::endl; + return 0; } - time_ordering_ += ((double) clock() - t_ordering_) / CLOCKS_PER_SEC; + x_incr_ = solver_.solve(b_); + R_ = solver_.matrixR(); + // std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl; + time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC; } - - bool solve(const int mode) + // INCREMENTAL + else { - bool batch = (mode !=2 || first_ordered_node_ == 0); - bool order = (mode !=0 && n_nodes_ > 1); - - // BATCH - if (batch) + // REORDER SUBPROBLEM + ordering(first_ordered_node_); + // print_problem(); + + // SOLVE ORDERED SUBPROBLEM + t_solving_ = clock(); + A_nodes_.makeCompressed(); + A_.makeCompressed(); + + // finding measurements block + SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_); + // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl; + // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << + // measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl; + int first_ordered_measurement = + measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]]; + int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location; + int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location; + int unordered_variables = nodes_.at(first_ordered_node_).location; + + SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables); + solver_.compute(A_partial); + if (solver_.info() != Success) { - // REORDER - if (order) - ordering(0); - - //print_problem(); - - // SOLVE - t_solving_ = clock(); - A_.makeCompressed(); - solver_.compute(A_); - if (solver_.info() != Success) - { - std::cout << "decomposition failed" << std::endl; - return 0; - } - x_incr_ = solver_.solve(b_); - R_ = solver_.matrixR(); - //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl; - time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC; + std::cout << "decomposition failed" << std::endl; + return 0; } - // INCREMENTAL - else + // std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * + // solver_.matrixR() << std::endl; + x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements)); + + // store new part of R + eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables); + // std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl; + addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables); + // std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl; + R_.makeCompressed(); + + // solving not ordered subproblem + if (unordered_variables > 0) { - // REORDER SUBPROBLEM - ordering(first_ordered_node_); - //print_problem(); - - // SOLVE ORDERED SUBPROBLEM - t_solving_= clock(); - A_nodes_.makeCompressed(); - A_.makeCompressed(); - - // finding measurements block - SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_); - // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl; - // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl; - int first_ordered_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]]; - int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location; - int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location; - int unordered_variables = nodes_.at(first_ordered_node_).location; - - SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables); - solver_.compute(A_partial); + // std::cout << "--------------------- solving unordered part" << std::endl; + SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables); + // std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl; + SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables); + // std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl; + solver_.compute(R1); if (solver_.info() != Success) { std::cout << "decomposition failed" << std::endl; return 0; } - //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl; - x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements)); - - // store new part of R - eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables); - //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl; - addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables); - //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl; - R_.makeCompressed(); - - // solving not ordered subproblem - if (unordered_variables > 0) - { - //std::cout << "--------------------- solving unordered part" << std::endl; - SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables); - //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl; - SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables); - //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl; - solver_.compute(R1); - if (solver_.info() != Success) - { - std::cout << "decomposition failed" << std::endl; - return 0; - } - x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables)); - } - + x_incr_.head(unordered_variables) = + solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables)); } - // UNDO ORDERING FOR RESULT - PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols()); - nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers - x_incr_ = acc_permutation.inverse() * x_incr_; + } + // UNDO ORDERING FOR RESULT + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols()); + nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers + x_incr_ = acc_permutation.inverse() * x_incr_; - time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC; + time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC; - return 1; - } + return 1; + } - void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables) - { - ArrayXi locations = perm_nodes_2_locations(_perm_nodes); + void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int>& _perm_nodes, + PermutationMatrix<Dynamic, Dynamic, int>& perm_variables) + { + ArrayXi locations = perm_nodes_2_locations(_perm_nodes); - int last_idx = 0; - for (unsigned int i = 0; i<locations.size(); i++) - { - perm_variables.indices().segment(last_idx, nodes_.at(i).dim) = VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i)+nodes_.at(i).dim-1); - last_idx += nodes_.at(i).dim; - } + int last_idx = 0; + for (unsigned int i = 0; i < locations.size(); i++) + { + perm_variables.indices().segment(last_idx, nodes_.at(i).dim) = + VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i) + nodes_.at(i).dim - 1); + last_idx += nodes_.at(i).dim; } + } - ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes) - { - ArrayXi indices = _perm_nodes.indices().array(); + ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int>& _perm_nodes) + { + ArrayXi indices = _perm_nodes.indices().array(); - for (unsigned int i = 0; i<indices.size(); i++) - indices = (indices > indices(i)).select(indices + nodes_.at(i).dim-1, indices); + for (unsigned int i = 0; i < indices.size(); i++) + indices = (indices > indices(i)).select(indices + nodes_.at(i).dim - 1, indices); - return indices; - } + return indices; + } - void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size) - { - int old_size = perm.indices().size(); - int dim = new_size - old_size; - VectorXi new_indices(new_size); - new_indices.head(old_size)= perm.indices(); - new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1); - perm.resize(new_size); - perm.indices() = new_indices; - } + void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int>& perm, const int new_size) + { + int old_size = perm.indices().size(); + int dim = new_size - old_size; + VectorXi new_indices(new_size); + new_indices.head(old_size) = perm.indices(); + new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size - 1); + perm.resize(new_size); + perm.indices() = new_indices; + } - void accumulatePermutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm) + void accumulatePermutation(const PermutationMatrix<Dynamic, Dynamic, int>& perm) + { + printName(); + // std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << + // std::endl; std::cout << "incr perm " << perm.indices().transpose() << std::endl; + + // acumulate permutation + if (perm.size() == acc_node_permutation_.size()) // full permutation + acc_node_permutation_ = perm * acc_node_permutation_; + else // partial permutation { - printName(); - //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl; - //std::cout << "incr perm " << perm.indices().transpose() << std::endl; - - // acumulate permutation - if (perm.size() == acc_node_permutation_.size()) //full permutation - acc_node_permutation_ = perm * acc_node_permutation_; - else //partial permutation - { - PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation - incr_permutation_nodes.indices().tail(perm.size()) = perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size()); - //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl; - acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_; - } - //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl; - - // update nodes orders and locations - ArrayXi locations = perm_nodes_2_locations(acc_node_permutation_); - for (unsigned int i = 0; i < nodes_.size(); i++) - { - nodes_.at(i).order = acc_node_permutation_.indices()(i); - nodes_.at(i).location = locations(i); - } + PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes( + VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation + incr_permutation_nodes.indices().tail(perm.size()) = + perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size()); + // std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl; + acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_; } + // std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl; - void printName() + // update nodes orders and locations + ArrayXi locations = perm_nodes_2_locations(acc_node_permutation_); + for (unsigned int i = 0; i < nodes_.size(); i++) { - std::cout << name_; + nodes_.at(i).order = acc_node_permutation_.indices()(i); + nodes_.at(i).location = locations(i); } + } - void printResults() - { - printName(); - std::cout << " solved in " << time_solving_*1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"<< std::endl; - std::cout << "x = " << x_incr_.transpose() << std::endl; - } + void printName() + { + std::cout << name_; + } - void printProblem() - { - printName(); - std::cout << std::endl << "A_nodes_: " << std::endl << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl; - std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl; - std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl; - } + void printResults() + { + printName(); + std::cout << " solved in " << time_solving_ * 1e3 << " ms | " << R_.nonZeros() << " nonzeros in R" + << std::endl; + std::cout << "x = " << x_incr_.transpose() << std::endl; + } + + void printProblem() + { + printName(); + std::cout << std::endl + << "A_nodes_: " << std::endl + << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl + << std::endl; + std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl; + std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl; + } }; -//main -int main(int argc, char *argv[]) +// main +int main(int argc, char* argv[]) { - if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1) + if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1) { std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl; std::cout << " - SIZE: integer size of the problem" << std::endl; @@ -500,7 +520,7 @@ int main(int argc, char *argv[]) return -1; } int size = atoi(argv[1]); - int dim = atoi(argv[2]); + int dim = atoi(argv[2]); // Problems SolverQR solver_ordered("FULL ORDERED"); @@ -511,7 +531,7 @@ int main(int argc, char *argv[]) // results variables clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4; - double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0; + double time_ordering = 0, time_solving_unordered = 0, time_solving_ordered = 0, time_solving_ordered_partial = 0; std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl; @@ -520,14 +540,16 @@ int main(int argc, char *argv[]) for (int i = 0; i < size; i++) { std::vector<int> meas(0); - if (i == 0) //prior - measurements.push_back(measurement(omega, 0, VectorXd::LinSpaced(dim, 0, dim-1), dim)); + if (i == 0) // prior + measurements.push_back(measurement(omega, 0, VectorXd::LinSpaced(dim, 0, dim - 1), dim)); - else //odometry - measurements.push_back(measurement(2*omega, i-1, 2*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim, true)); + else // odometry + measurements.push_back(measurement( + 2 * omega, i - 1, 2 * omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i + 1) - 1), dim, true)); - if (i > size / 2) //loop closures - measurements.push_back(measurement(4*omega, 0, 4*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim)); + if (i > size / 2) // loop closures + measurements.push_back( + measurement(4 * omega, 0, 4 * omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i + 1) - 1), dim)); } // INCREMENTAL LOOP @@ -536,11 +558,11 @@ int main(int argc, char *argv[]) std::cout << "========================= MEASUREMENT " << i << ":" << std::endl; // AUGMENT THE PROBLEM ---------------------------- - if (measurements.at(i).odometry_type || i == 0) // if odometry, augment the problem + if (measurements.at(i).odometry_type || i == 0) // if odometry, augment the problem { solver_unordered.add_state_unit(dim, i); solver_ordered.add_state_unit(dim, i); - solver_ordered_partial.add_state_unit(dim,i); + solver_ordered_partial.add_state_unit(dim, i); } // ADD MEASUREMENTS @@ -564,10 +586,11 @@ int main(int argc, char *argv[]) solver_ordered.printResults(); solver_ordered_partial.printResults(); -// if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10) -// std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl; -// else -// std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl; + // if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10) + // std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() + // << ")" << std::endl; + // else + // std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << + // (x_ordered_partial-x_ordered).maxCoeff() << std::endl; } } - diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp index 2371a3dd9e8c8095b99c8fdd2c28c5a46b0568ce..d87c9455bebcb9f916825be3da4fc78c32749043 100644 --- a/demos/solver/test_iQR_wolf2.cpp +++ b/demos/solver/test_iQR_wolf2.cpp @@ -18,7 +18,7 @@ // 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/>. -//std includes +// std includes #include <cstdlib> #include <string> #include <iostream> @@ -29,7 +29,7 @@ #include <ctime> #include <queue> -//Wolf includes +// Wolf includes #include "core/state_block/state_block.h" #include "core/factor/factor_base.h" #include "core/sensor/sensor_laser_2D.h" @@ -38,76 +38,76 @@ // wolf solver #include "solver/qr_solver.h" -//C includes for sleep, time and main args +// C includes for sleep, time and main args #include "unistd.h" -//faramotics includes +// faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" #include "btr-headers/pose3d.h" -//Ceres includes +// Ceres includes #include "glog/logging.h" #include "ceres_wrapper/solver_ceres.h" #include "iri-algorithms/laser_scan_utils/corner_detector.h" #include "iri-algorithms/laser_scan_utils/entities.h" -//function travel around -void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_) +// function travel around +void motionCampus(unsigned int ii, Cpose3d& pose, double& displacement_, double& rotation_) { if (ii <= 120) { displacement_ = 0.1; - rotation_ = 0; + rotation_ = 0; } else if ((ii > 120) && (ii <= 170)) { displacement_ = 0.2; - rotation_ = 1.8 * M_PI / 180; + rotation_ = 1.8 * M_PI / 180; } else if ((ii > 170) && (ii <= 220)) { displacement_ = 0; - rotation_ = -1.8 * M_PI / 180; + rotation_ = -1.8 * M_PI / 180; } else if ((ii > 220) && (ii <= 310)) { displacement_ = 0.1; - rotation_ = 0; + rotation_ = 0; } else if ((ii > 310) && (ii <= 487)) { displacement_ = 0.1; - rotation_ = -1. * M_PI / 180; + rotation_ = -1. * M_PI / 180; } else if ((ii > 487) && (ii <= 600)) { displacement_ = 0.2; - rotation_ = 0; + rotation_ = 0; } else if ((ii > 600) && (ii <= 700)) { displacement_ = 0.1; - rotation_ = -1. * M_PI / 180; + rotation_ = -1. * M_PI / 180; } else if ((ii > 700) && (ii <= 780)) { displacement_ = 0; - rotation_ = -1. * M_PI / 180; + rotation_ = -1. * M_PI / 180; } else { displacement_ = 0.3; - rotation_ = 0.0 * M_PI / 180; + rotation_ = 0.0 * M_PI / 180; } pose.moveForward(displacement_); pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll()); } -//main -int main(int argc, char *argv[]) +// main +int main(int argc, char* argv[]) { using namespace Eigen; using namespace wolf; @@ -117,84 +117,100 @@ int main(int argc, char *argv[]) { std::cout << "Please call me with: [./test_ceres_manager NI MODE], where:" << std::endl; std::cout << " - NI is the number of iterations (0 < NI < 1100)" << std::endl; - std::cout << " - MODE is the solver mode (0 batch (no ordering), 1 batch (ordering), 2 incremental" << std::endl; + std::cout << " - MODE is the solver mode (0 batch (no ordering), 1 batch (ordering), 2 incremental" + << std::endl; std::cout << "EXIT due to bad user input" << std::endl << std::endl; return -1; } - bool complex_angle = false; - unsigned int solving_mode = (unsigned int) atoi(argv[2]); //solving mode - unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution + bool complex_angle = false; + unsigned int solving_mode = (unsigned int)atoi(argv[2]); // solving mode + unsigned int n_execution = (unsigned int)atoi(argv[1]); // number of iterations of the whole execution // INITIALIZATION ============================================================================================ - //init random generators - double odom_std_factor = 0.1; - double gps_std = 10; - std::default_random_engine generator(1); + // init random generators + double odom_std_factor = 0.1; + double gps_std = 10; + std::default_random_engine generator(1); std::normal_distribution<double> gaussian_distribution(0.0, 1); // Faramotics stuff - Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose; - vector < Cpose3d > devicePoses; - vector<float> scan1, scan2; - string modelFileName; + Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, + estimated_laser_2_pose; + vector<Cpose3d> devicePoses; + vector<float> scan1, scan2; + string modelFileName; - //model and initial view point + // model and initial view point modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"; - //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj"; - //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj"; + // modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj"; + // modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj"; devicePose.setPose(2, 8, 0.2, 0, 0, 0); viewPoint.setPose(devicePose); viewPoint.moveForward(10); - viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll()); + viewPoint.rt.setEuler( + viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll()); viewPoint.moveForward(-15); - //glut initialization + // glut initialization faramotics::initGLUT(argc, argv); - //create a viewer for the 3D model and scan points - CdynamicSceneRender* myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100); - myRender->loadAssimpModel(modelFileName, true); //with wireframe - //create scanner and load 3D model - CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4 + // create a viewer for the 3D model and scan points + CdynamicSceneRender* myRender = + new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100); + myRender->loadAssimpModel(modelFileName, true); // with wireframe + // create scanner and load 3D model + CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); // HOKUYO_UTM30LX_180DEG or LEUZE_RS4 myScanner->loadAssimpModel(modelFileName); - //variables + // variables Eigen::Vector3d odom_reading; Eigen::Vector2d gps_fix_reading; - Eigen::VectorXd pose_odom(3); //current odometry integred pose - Eigen::VectorXd ground_truth(n_execution * 3); //all true poses - Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory + Eigen::VectorXd pose_odom(3); // current odometry integred pose + Eigen::VectorXd ground_truth(n_execution * 3); // all true poses + Eigen::VectorXd odom_trajectory(n_execution * 3); // open loop trajectory Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7); - clock_t t1, t2; + clock_t t1, t2; // Wolf manager initialization Eigen::Vector3d odom_pose = Eigen::Vector3d::Zero(); - Eigen::Vector3d gps_pose = Eigen::Vector3d::Zero(); - Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta - laser_1_pose << 1.2, 0, 0, 0; //laser 1 - laser_2_pose << -1.2, 0, 0, M_PI; //laser 2 - SensorOdom2D odom_sensor(std::make_shared<StatePoint2d>(odom_pose.head(2)), std::make_shared<StateAngle>(odom_pose.tail(1)), odom_std_factor, odom_std_factor); - SensorGPSFix gps_sensor(std::make_shared<StatePoint2d>(gps_pose.head(2)), std::make_shared<StateAngle>(gps_pose.tail(1)), gps_std); - SensorLaser2D laser_1_sensor(std::make_shared<StatePoint2d>(laser_1_pose.head(2)), std::make_shared<StateAngle>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01})); - SensorLaser2D laser_2_sensor(std::make_shared<StatePoint2d>(laser_2_pose.head(2)), std::make_shared<StateAngle>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01})); + Eigen::Vector3d gps_pose = Eigen::Vector3d::Zero(); + Eigen::Vector4d laser_1_pose, laser_2_pose; // xyz + theta + laser_1_pose << 1.2, 0, 0, 0; // laser 1 + laser_2_pose << -1.2, 0, 0, M_PI; // laser 2 + SensorOdom2D odom_sensor(std::make_shared<StatePoint2d>(odom_pose.head(2)), + std::make_shared<StateAngle>(odom_pose.tail(1)), + odom_std_factor, + odom_std_factor); + SensorGPSFix gps_sensor( + std::make_shared<StatePoint2d>(gps_pose.head(2)), std::make_shared<StateAngle>(gps_pose.tail(1)), gps_std); + SensorLaser2D laser_1_sensor( + std::make_shared<StatePoint2d>(laser_1_pose.head(2)), + std::make_shared<StateAngle>(laser_1_pose.tail(1)), + laserscanutils::LaserScanParams({M_PI / 2, -M_PI / 2, -M_PI / 720, 0.01, 0.2, 100, 0.01, 0.01})); + SensorLaser2D laser_2_sensor( + std::make_shared<StatePoint2d>(laser_2_pose.head(2)), + std::make_shared<StateAngle>(laser_2_pose.tail(1)), + laserscanutils::LaserScanParams({M_PI / 2, -M_PI / 2, -M_PI / 720, 0.01, 0.2, 100, 0.01, 0.01})); // Initial pose pose_odom << 2, 8, 0; - ground_truth.head(3) = pose_odom; + ground_truth.head(3) = pose_odom; odom_trajectory.head(3) = pose_odom; - WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01); - WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01); + WolfManager* wolf_manager_QR = new WolfManager( + FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution * 10, 0.01); + WolfManager* wolf_manager_ceres = new WolfManager( + FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution * 10, 0.01); // Ceres initialization ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + ceres_options.minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;LINE_SEARCH ceres_options.max_line_search_step_contraction = 1e-3; // ceres_options.minimizer_progress_to_stdout = false; // ceres_options.line_search_direction_type = ceres::LBFGS; // ceres_options.max_num_iterations = 100; - SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options); - std::ofstream log_file, landmark_file; //output file + SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options); + std::ofstream log_file, landmark_file; // output file // Own solver SolverQR solver_(wolf_manager_QR->getProblem()); @@ -204,11 +220,11 @@ int main(int argc, char *argv[]) // START TRAJECTORY ============================================================================================ for (unsigned int step = 1; step < n_execution; step++) { - //get init time + // get init time t2 = clock(); // ROBOT MOVEMENT --------------------------- - //std::cout << "ROBOT MOVEMENT..." << std::endl; + // std::cout << "ROBOT MOVEMENT..." << std::endl; // moves the device position t1 = clock(); motionCampus(step, devicePose, odom_reading(0), odom_reading(2)); @@ -216,14 +232,16 @@ int main(int argc, char *argv[]) devicePoses.push_back(devicePose); // SENSOR DATA --------------------------- - //std::cout << "SENSOR DATA..." << std::endl; + // std::cout << "SENSOR DATA..." << std::endl; // store groundtruth ground_truth.segment(step * 3, 3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head(); // compute odometry - odom_reading(0) += gaussian_distribution(generator) * odom_std_factor * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0)); + odom_reading(0) += + gaussian_distribution(generator) * odom_std_factor * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0)); odom_reading(1) += gaussian_distribution(generator) * odom_std_factor * 1e-6; - odom_reading(2) += gaussian_distribution(generator) * odom_std_factor * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2)); + odom_reading(2) += + gaussian_distribution(generator) * odom_std_factor * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2)); // odometry integration pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)) - odom_reading(1) * sin(pose_odom(2)); @@ -236,7 +254,7 @@ int main(int argc, char *argv[]) gps_fix_reading(0) += gaussian_distribution(generator) * gps_std; gps_fix_reading(1) += gaussian_distribution(generator) * gps_std; - //compute scans + // compute scans scan1.clear(); scan2.clear(); // scan 1 @@ -249,94 +267,115 @@ int main(int argc, char *argv[]) laser2Pose.rt.setEuler(laser2Pose.rt.head() + M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll()); myScanner->computeScan(laser2Pose, scan2); - mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC; + mean_times(0) += ((double)clock() - t1) / CLOCKS_PER_SEC; // ADD CAPTURES --------------------------- - //std::cout << "ADD CAPTURES..." << std::endl; + // std::cout << "ADD CAPTURES..." << std::endl; // adding new sensor captures - wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2))); - wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3))); - wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2))); - wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3))); - //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1)); - //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2)); + wolf_manager_QR->addCapture( + new CaptureOdom2D(TimeStamp(), + TimeStamp(), + &odom_sensor, + odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2))); + wolf_manager_QR->addCapture(new CaptureGPSFix( + TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3, 3))); + wolf_manager_ceres->addCapture( + new CaptureOdom2D(TimeStamp(), + TimeStamp(), + &odom_sensor, + odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2))); + wolf_manager_ceres->addCapture(new CaptureGPSFix( + TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3, 3))); + // wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1)); + // wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2)); // updating problem wolf_manager_QR->update(); wolf_manager_ceres->update(); // UPDATING SOLVER --------------------------- - //std::cout << "UPDATING..." << std::endl; + // std::cout << "UPDATING..." << std::endl; // update state units and factors in ceres solver_.update(); // PRINT PROBLEM - //solver_.printProblem(); + // solver_.printProblem(); // SOLVE OPTIMIZATION --------------------------- - //std::cout << "SOLVING..." << std::endl; + // std::cout << "SOLVING..." << std::endl; ceres::Solver::Summary summary = ceres_manager->solve(); solver_.solve(solving_mode); std::cout << "========================= RESULTS " << step << ":" << std::endl; - //solver_.printResults(); + // solver_.printResults(); std::cout << "QR vehicle pose " << wolf_manager_QR->getVehiclePose().transpose() << std::endl; std::cout << "ceres vehicle pose " << wolf_manager_ceres->getVehiclePose().transpose() << std::endl; // COMPUTE COVARIANCES --------------------------- - //std::cout << "COMPUTING COVARIANCES..." << std::endl; + // std::cout << "COMPUTING COVARIANCES..." << std::endl; // TODO // DRAWING STUFF --------------------------- // draw detected corners -// std::list < laserscanutils::Corner > corner_list; -// std::vector<double> corner_vector; -// CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1); -// last_scan.extractCorners(corner_list); -// for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++) -// { -// corner_vector.push_back(corner_it->pt_(0)); -// corner_vector.push_back(corner_it->pt_(1)); -// } -// myRender->drawCorners(laser1Pose, corner_vector); + // std::list < laserscanutils::Corner > corner_list; + // std::vector<double> corner_vector; + // CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1); + // last_scan.extractCorners(corner_list); + // for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != + // corner_list.end(); corner_it++) + // { + // corner_vector.push_back(corner_it->pt_(0)); + // corner_vector.push_back(corner_it->pt_(1)); + // } + // myRender->drawCorners(laser1Pose, corner_vector); // draw landmarks std::vector<double> landmark_vector; for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList()) { double* position_ptr = landmark->getP()->get(); - landmark_vector.push_back(*position_ptr); //x - landmark_vector.push_back(*(position_ptr + 1)); //y - landmark_vector.push_back(0.2); //z + landmark_vector.push_back(*position_ptr); // x + landmark_vector.push_back(*(position_ptr + 1)); // y + landmark_vector.push_back(0.2); // z } myRender->drawLandmarks(landmark_vector); // draw localization and sensors - estimated_vehicle_pose.setPose(wolf_manager_QR->getVehiclePose()(0), wolf_manager_QR->getVehiclePose()(1), 0.2, wolf_manager_QR->getVehiclePose()(2), 0, 0); + estimated_vehicle_pose.setPose(wolf_manager_QR->getVehiclePose()(0), + wolf_manager_QR->getVehiclePose()(1), + 0.2, + wolf_manager_QR->getVehiclePose()(2), + 0, + 0); estimated_laser_1_pose.setPose(estimated_vehicle_pose); estimated_laser_1_pose.moveForward(laser_1_pose(0)); estimated_laser_2_pose.setPose(estimated_vehicle_pose); // instead of laser 2 we draw ceres solution - //estimated_laser_2_pose.moveForward(laser_2_pose(0)); - //estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll()); - estimated_laser_2_pose.setPose(wolf_manager_ceres->getVehiclePose()(0), wolf_manager_ceres->getVehiclePose()(1), 0.2, wolf_manager_ceres->getVehiclePose()(2), 0, 0); - - myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose }); - - //Set view point and render the scene - //locate visualization view point, somewhere behind the device + // estimated_laser_2_pose.moveForward(laser_2_pose(0)); + // estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, + // estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll()); + estimated_laser_2_pose.setPose(wolf_manager_ceres->getVehiclePose()(0), + wolf_manager_ceres->getVehiclePose()(1), + 0.2, + wolf_manager_ceres->getVehiclePose()(2), + 0, + 0); + + myRender->drawPoseAxisVector({estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose}); + + // Set view point and render the scene + // locate visualization view point, somewhere behind the device myRender->setViewPoint(viewPoint); myRender->drawPoseAxis(devicePose); - myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan + myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); // draw scan myRender->render(); // TIME MANAGEMENT --------------------------- - double dt = ((double) clock() - t2) / CLOCKS_PER_SEC; + double dt = ((double)clock() - t2) / CLOCKS_PER_SEC; mean_times(6) += dt; - if (dt < 0.1) - usleep(100000 - 1e6 * dt); + if (dt < 0.1) usleep(100000 - 1e6 * dt); -// std::cout << "\nTree after step..." << std::endl; -// wolf_manager->getProblem()->print(); + // std::cout << "\nTree after step..." << std::endl; + // wolf_manager->getProblem()->print(); } // DISPLAY RESULTS ============================================================================================ @@ -350,8 +389,8 @@ int main(int argc, char *argv[]) std::cout << " results drawing: " << mean_times(5) << std::endl; std::cout << " loop time: " << mean_times(6) << std::endl; -// std::cout << "\nTree before deleting..." << std::endl; -// wolf_manager->getProblem()->print(); + // std::cout << "\nTree before deleting..." << std::endl; + // wolf_manager->getProblem()->print(); // Draw Final result ------------------------- std::cout << "Drawing final results..." << std::endl; @@ -359,15 +398,15 @@ int main(int argc, char *argv[]) for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList()) { double* position_ptr = landmark->getP()->get(); - landmark_vector.push_back(*position_ptr); //x - landmark_vector.push_back(*(position_ptr + 1)); //y - landmark_vector.push_back(0.2); //z + landmark_vector.push_back(*position_ptr); // x + landmark_vector.push_back(*(position_ptr + 1)); // y + landmark_vector.push_back(0.2); // z } myRender->drawLandmarks(landmark_vector); -// viewPoint.setPose(devicePoses.front()); -// viewPoint.moveForward(10); -// viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() ); -// viewPoint.moveForward(-10); + // viewPoint.setPose(devicePoses.front()); + // viewPoint.moveForward(10); + // viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() ); + // viewPoint.moveForward(-10); myRender->setViewPoint(viewPoint); myRender->render(); @@ -375,12 +414,13 @@ int main(int argc, char *argv[]) std::cout << "Printing results in a file..." << std::endl; // Vehicle poses std::cout << "Vehicle poses..." << std::endl; - int i = 0; + int i = 0; Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->size() * 3); for (auto frame : wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap()) { if (complex_angle) - state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), atan2(*frame->getO()->get(), *(frame->getO()->get() + 1)); + state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), + atan2(*frame->getO()->get(), *(frame->getO()->get() + 1)); else state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), *frame->getO()->get(); i += 3; @@ -398,28 +438,33 @@ int main(int argc, char *argv[]) } // Print log files - std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt")); - log_file.open(filepath, std::ofstream::out); //open log file + std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") + : std::string("/Desktop/log_file_2.txt")); + log_file.open(filepath, std::ofstream::out); // open log file if (log_file.is_open()) { log_file << 0 << std::endl; for (unsigned int ii = 0; ii < n_execution; ii++) - log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl; - log_file.close(); //close log file + log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" + << ground_truth.segment(ii * 3, 3).transpose() << "\t" + << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" + << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl; + log_file.close(); // close log file std::cout << std::endl << "Result file " << filepath << std::endl; } else std::cout << std::endl << "Failed to write the log file " << filepath << std::endl; - std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt")); - landmark_file.open(filepath2, std::ofstream::out); //open log file + std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") + : std::string("/Desktop/landmarks_file_2.txt")); + landmark_file.open(filepath2, std::ofstream::out); // open log file if (landmark_file.is_open()) { for (unsigned int ii = 0; ii < landmarks.size(); ii += 2) landmark_file << landmarks.segment(ii, 2).transpose() << std::endl; - landmark_file.close(); //close log file + landmark_file.close(); // close log file std::cout << std::endl << "Landmark file " << filepath << std::endl; } else @@ -436,7 +481,6 @@ int main(int argc, char *argv[]) std::cout << " ========= END ===========" << std::endl << std::endl; - //exit + // exit return 0; } - diff --git a/demos/solver/test_incremental_ccolamd_blocks.cpp b/demos/solver/test_incremental_ccolamd_blocks.cpp index 07f9b7f6a41e65b6ebf0d9f1b90cd82deee30bbb..1e099edfead3a8569d5934222a7f6c9c63827fe7 100644 --- a/demos/solver/test_incremental_ccolamd_blocks.cpp +++ b/demos/solver/test_incremental_ccolamd_blocks.cpp @@ -18,7 +18,7 @@ // 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/>. -//std includes +// std includes #include <cstdlib> #include <iostream> #include <fstream> @@ -38,38 +38,45 @@ using namespace Eigen; -void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols) +void eraseSparseBlock(SparseMatrix<double>& original, + const unsigned int& row, + const unsigned int& Nrows, + const unsigned int& col, + const unsigned int& Ncols) { - for (unsigned int i = row; i < row + Nrows; i++) - for (unsigned int j = col; j < row + Ncols; j++) - original.coeffRef(i,j) = 0.0; + for (unsigned int i = row; i < row + Nrows; i++) + for (unsigned int j = col; j < row + Ncols; j++) original.coeffRef(i, j) = 0.0; - original.makeCompressed(); + original.makeCompressed(); } -void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col) +void addSparseBlock(const MatrixXd& ins, + SparseMatrix<double>& original, + const unsigned int& row, + const unsigned int& col) { - for (unsigned int r=0; r<ins.rows(); ++r) - for (unsigned int c = 0; c < ins.cols(); c++) - if (ins(r,c) != 0) - original.coeffRef(r + row, c + col) += ins(r,c); + for (unsigned int r = 0; r < ins.rows(); ++r) + for (unsigned int c = 0; c < ins.cols(); c++) + if (ins(r, c) != 0) original.coeffRef(r + row, c + col) += ins(r, c); } -void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size) +void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int>& perm, + PermutationMatrix<Dynamic, Dynamic, int>& perm_blocks, + const int dim, + const int size) { ArrayXXi idx(dim, size); idx.row(0) = dim * perm.indices().transpose(); - for (int i = 1; i<dim; i++) - idx.row(i) = idx.row(i-1) + 1; - Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1); + for (int i = 1; i < dim; i++) idx.row(i) = idx.row(i - 1) + 1; + Map<ArrayXi> idx_blocks(idx.data(), dim * size, 1); perm_blocks.indices() = idx_blocks; } -//main -int main(int argc, char *argv[]) +// main +int main(int argc, char* argv[]) { - if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1) + if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1) { std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl; std::cout << " - SIZE: integer size of the problem" << std::endl; @@ -78,43 +85,34 @@ int main(int argc, char *argv[]) return -1; } int size = atoi(argv[1]); - int dim = atoi(argv[2]); + int dim = atoi(argv[2]); // Problem variables - //CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3; - SparseLU < SparseMatrix<double>, NaturalOrdering<int> > solver, solver2, solver3; - MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim); - SparseMatrix<double> H(dim,dim), - H_ordered(dim,dim), - H_b_ordered(dim,dim); - VectorXd b(dim), - b_ordered(dim), - b_b_ordered(dim), - x_b_ordered(dim), - x_ordered(dim), - x(dim); + // CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3; + SparseLU<SparseMatrix<double>, NaturalOrdering<int> > solver, solver2, solver3; + MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim); + SparseMatrix<double> H(dim, dim), H_ordered(dim, dim), H_b_ordered(dim, dim); + VectorXd b(dim), b_ordered(dim), b_b_ordered(dim), x_b_ordered(dim), x_ordered(dim), x(dim); // ordering variables - SparseMatrix<int> factors(1,1), factors_ordered(1,1); - ArrayXi acc_permutation(dim), - acc_permutation_b(dim), - acc_permutation_factors(1); - acc_permutation = ArrayXi::LinSpaced(dim,0,dim-1); - acc_permutation_b = acc_permutation; + SparseMatrix<int> factors(1, 1), factors_ordered(1, 1); + ArrayXi acc_permutation(dim), acc_permutation_b(dim), acc_permutation_factors(1); + acc_permutation = ArrayXi::LinSpaced(dim, 0, dim - 1); + acc_permutation_b = acc_permutation; acc_permutation_factors(0) = 0; CCOLAMDOrdering<int> ordering; - VectorXi factor_ordering_factors(1); - VectorXi ordering_factors(1); + VectorXi factor_ordering_factors(1); + VectorXi ordering_factors(1); // results variables clock_t t1, t2, t3; - double time1=0, time2=0, time3=0; + double time1 = 0, time2 = 0, time3 = 0; // INITIAL STATE - addSparseBlock(5*omega, H, 0, 0); - factors.insert(0,0) = 1; - b.head(dim) = VectorXd::LinSpaced(Sequential, dim, 0, dim-1); + addSparseBlock(5 * omega, H, 0, 0); + factors.insert(0, 0) = 1; + b.head(dim) = VectorXd::LinSpaced(Sequential, dim, 0, dim - 1); std::cout << "STARTING INCREMENTAL TEST" << std::endl << std::endl; @@ -123,40 +121,43 @@ int main(int argc, char *argv[]) { std::cout << "========================= STEP " << i << ":" << std::endl; // AUGMENT THE PROBLEM ---------------------------- - H.conservativeResize((i+1)*dim,(i+1)*dim); - H_ordered.conservativeResize((i+1)*dim,(i+1)*dim); - H_b_ordered.conservativeResize((i+1)*dim,(i+1)*dim); - b.conservativeResize((i+1)*dim); - b_ordered.conservativeResize((i+1)*dim); - b_b_ordered.conservativeResize((i+1)*dim); - x.conservativeResize((i+1)*dim); - x_ordered.conservativeResize((i+1)*dim); - x_b_ordered.conservativeResize((i+1)*dim); - factors.conservativeResize(i+1, i+1); + H.conservativeResize((i + 1) * dim, (i + 1) * dim); + H_ordered.conservativeResize((i + 1) * dim, (i + 1) * dim); + H_b_ordered.conservativeResize((i + 1) * dim, (i + 1) * dim); + b.conservativeResize((i + 1) * dim); + b_ordered.conservativeResize((i + 1) * dim); + b_b_ordered.conservativeResize((i + 1) * dim); + x.conservativeResize((i + 1) * dim); + x_ordered.conservativeResize((i + 1) * dim); + x_b_ordered.conservativeResize((i + 1) * dim); + factors.conservativeResize(i + 1, i + 1); // Odometry - addSparseBlock(5*omega, H, i*dim, i*dim); - addSparseBlock(omega, H, i*dim, (i-1)*dim); - addSparseBlock(omega, H, (i-1)*dim, i*dim); - factors.insert(i,i) = 1; - factors.insert(i,i-1) = 1; - factors.insert(i-1,i) = 1; + addSparseBlock(5 * omega, H, i * dim, i * dim); + addSparseBlock(omega, H, i * dim, (i - 1) * dim); + addSparseBlock(omega, H, (i - 1) * dim, i * dim); + factors.insert(i, i) = 1; + factors.insert(i, i - 1) = 1; + factors.insert(i - 1, i) = 1; // Loop Closure - if (i == size-1) + if (i == size - 1) { - addSparseBlock(2*omega, H, 0, i*dim); - addSparseBlock(2*omega, H, i*dim, 0); - factors.insert(0,i) = 1; - factors.insert(i,0) = 1; + addSparseBlock(2 * omega, H, 0, i * dim); + addSparseBlock(2 * omega, H, i * dim, 0); + factors.insert(0, i) = 1; + factors.insert(i, 0) = 1; } // r.h.v - b.segment(i*dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim*i, dim *(i+1)-1); + b.segment(i * dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim * i, dim * (i + 1) - 1); std::cout << "Solving factor graph:" << std::endl; - std::cout << "Factors: " << std::endl << factors * MatrixXi::Identity((i+1), (i+1)) << std::endl << std::endl; -// std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << std::endl; + std::cout << "Factors: " << std::endl + << factors * MatrixXi::Identity((i + 1), (i + 1)) << std::endl + << std::endl; + // std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << + // std::endl; // SOLVING WITHOUT REORDERING ------------------------------------ // solve Hx = b @@ -168,33 +169,34 @@ int main(int argc, char *argv[]) return 0; } x = solver.solve(b); - time1 += ((double) clock() - t1) / CLOCKS_PER_SEC; + time1 += ((double)clock() - t1) / CLOCKS_PER_SEC; // SOLVING WITH REORDERING ------------------------------------ // Order with previous orderings - acc_permutation.conservativeResize(dim*(i+1)); - acc_permutation.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1); - PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(dim*(i+1)); + acc_permutation.conservativeResize(dim * (i + 1)); + acc_permutation.tail(dim) = ArrayXi::LinSpaced(dim, dim * i, dim * (i + 1) - 1); + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(dim * (i + 1)); acc_permutation_matrix.indices() = acc_permutation; - b_ordered = acc_permutation_matrix * b; - H_ordered = H.twistedBy(acc_permutation_matrix); + b_ordered = acc_permutation_matrix * b; + H_ordered = H.twistedBy(acc_permutation_matrix); // ordering factors - ordering_factors.resize(dim*(i+1)); - ordering_factors = ((H_ordered.rightCols(3) * MatrixXd::Ones(3,1)).array() == 0).select(VectorXi::Zero(dim*(i+1)),VectorXi::Ones(dim*(i+1))); + ordering_factors.resize(dim * (i + 1)); + ordering_factors = ((H_ordered.rightCols(3) * MatrixXd::Ones(3, 1)).array() == 0) + .select(VectorXi::Zero(dim * (i + 1)), VectorXi::Ones(dim * (i + 1))); // variable ordering t2 = clock(); H_ordered.makeCompressed(); - PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(dim*(i+1)); + PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(dim * (i + 1)); ordering(H_ordered, permutation_matrix, ordering_factors.data()); // applying ordering acc_permutation_matrix = permutation_matrix * acc_permutation_matrix; - acc_permutation = acc_permutation_matrix.indices(); - b_ordered = permutation_matrix * b_ordered; - H_ordered = H_ordered.twistedBy(permutation_matrix); + acc_permutation = acc_permutation_matrix.indices(); + b_ordered = permutation_matrix * b_ordered; + H_ordered = H_ordered.twistedBy(permutation_matrix); // solve Hx = b solver2.compute(H_ordered); @@ -205,22 +207,22 @@ int main(int argc, char *argv[]) } x_ordered = solver2.solve(b_ordered); x_ordered = acc_permutation_matrix.inverse() * x_ordered; - time2 += ((double) clock() - t2) / CLOCKS_PER_SEC; + time2 += ((double)clock() - t2) / CLOCKS_PER_SEC; // SOLVING WITH BLOCK REORDERING ------------------------------------ // Order with previous orderings - acc_permutation_b.conservativeResize(dim*(i+1)); - acc_permutation_b.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1); - PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_b_matrix(dim*(i+1)); + acc_permutation_b.conservativeResize(dim * (i + 1)); + acc_permutation_b.tail(dim) = ArrayXi::LinSpaced(dim, dim * i, dim * (i + 1) - 1); + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_b_matrix(dim * (i + 1)); acc_permutation_b_matrix.indices() = acc_permutation_b; - b_b_ordered = acc_permutation_b_matrix * b; - H_b_ordered = H.twistedBy(acc_permutation_b_matrix); + b_b_ordered = acc_permutation_b_matrix * b; + H_b_ordered = H.twistedBy(acc_permutation_b_matrix); - acc_permutation_factors.conservativeResize(i+1); + acc_permutation_factors.conservativeResize(i + 1); acc_permutation_factors(i) = i; - PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_factors_matrix(dim*(i+1)); + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_factors_matrix(dim * (i + 1)); acc_permutation_factors_matrix.indices() = acc_permutation_factors; - factors_ordered = factors.twistedBy(acc_permutation_factors_matrix); + factors_ordered = factors.twistedBy(acc_permutation_factors_matrix); // ordering factors factor_ordering_factors.resize(i); @@ -230,17 +232,17 @@ int main(int argc, char *argv[]) t3 = clock(); factors_ordered.makeCompressed(); - PermutationMatrix<Dynamic, Dynamic, int> permutation_factors_matrix(i+1); + PermutationMatrix<Dynamic, Dynamic, int> permutation_factors_matrix(i + 1); ordering(factors_ordered, permutation_factors_matrix, factor_ordering_factors.data()); // applying ordering - permutation_2_block_permutation(permutation_factors_matrix, permutation_matrix , dim, i+1); + permutation_2_block_permutation(permutation_factors_matrix, permutation_matrix, dim, i + 1); acc_permutation_factors_matrix = permutation_factors_matrix * acc_permutation_factors_matrix; - acc_permutation_factors = acc_permutation_factors_matrix.indices(); - acc_permutation_b_matrix = permutation_matrix * acc_permutation_b_matrix; - acc_permutation_b = acc_permutation_b_matrix.indices(); - b_b_ordered = permutation_matrix * b_b_ordered; - H_b_ordered = H_b_ordered.twistedBy(permutation_matrix); + acc_permutation_factors = acc_permutation_factors_matrix.indices(); + acc_permutation_b_matrix = permutation_matrix * acc_permutation_b_matrix; + acc_permutation_b = acc_permutation_b_matrix.indices(); + b_b_ordered = permutation_matrix * b_b_ordered; + H_b_ordered = H_b_ordered.twistedBy(permutation_matrix); // solve Hx = b solver3.compute(H_b_ordered); @@ -251,25 +253,24 @@ int main(int argc, char *argv[]) } x_b_ordered = solver3.solve(b_b_ordered); x_b_ordered = acc_permutation_b_matrix.inverse() * x_b_ordered; - time3 += ((double) clock() - t3) / CLOCKS_PER_SEC; + time3 += ((double)clock() - t3) / CLOCKS_PER_SEC; // RESULTS ------------------------------------ std::cout << "========================= RESULTS " << i << ":" << std::endl; - std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl; - std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl; - std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl; + std::cout << "NO REORDERING: solved in " << time1 * 1e3 << " ms" << std::endl; + std::cout << "REORDERING: solved in " << time2 * 1e3 << " ms" << std::endl; + std::cout << "BLOCK REORDERING: solved in " << time3 * 1e3 << " ms" << std::endl; std::cout << "x1 = " << x.transpose() << std::endl; std::cout << "x2 = " << x_ordered.transpose() << std::endl; std::cout << "x3 = " << x_b_ordered.transpose() << std::endl; } // RESULTS ------------------------------------ - std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl; - std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl; - std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl; + std::cout << "NO REORDERING: solved in " << time1 * 1e3 << " ms" << std::endl; + std::cout << "REORDERING: solved in " << time2 * 1e3 << " ms" << std::endl; + std::cout << "BLOCK REORDERING: solved in " << time3 * 1e3 << " ms" << std::endl; - //std::cout << "x = " << x.transpose() << std::endl; - //std::cout << "x = " << x_ordered.transpose() << std::endl; - //std::cout << "x = " << x_b_ordered.transpose() << std::endl; + // std::cout << "x = " << x.transpose() << std::endl; + // std::cout << "x = " << x_ordered.transpose() << std::endl; + // std::cout << "x = " << x_b_ordered.transpose() << std::endl; } - diff --git a/demos/solver/test_permutations.cpp b/demos/solver/test_permutations.cpp index 94dcad3d9e9696e9bf9f022376dfe964c716fac1..887522e03b5efeb2899c74a3568d3928b06304eb 100644 --- a/demos/solver/test_permutations.cpp +++ b/demos/solver/test_permutations.cpp @@ -18,7 +18,7 @@ // 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/>. -//std includes +// std includes #include <cstdlib> #include <iostream> #include <fstream> @@ -33,7 +33,7 @@ using namespace Eigen; -//main +// main int main(int argc, char *argv[]) { PermutationMatrix<Dynamic, Dynamic, int> P1(5), P2(5), P3(5), P4(5); @@ -41,8 +41,8 @@ int main(int argc, char *argv[]) P2.setIdentity(); P3.setIdentity(); - VectorXd a = VectorXd::LinSpaced(5,1,5); - MatrixXd A= a.asDiagonal(); + VectorXd a = VectorXd::LinSpaced(5, 1, 5); + MatrixXd A = a.asDiagonal(); SparseMatrix<double> B = A.sparseView(); B.makeCompressed(); @@ -66,35 +66,42 @@ int main(int argc, char *argv[]) std::cout << "(P1 * B).bottomRows(1)" << std::endl << C.bottomRows(1) << std::endl << std::endl; std::cout << "Post-multiplying: Permutating cols" << std::endl; - std::cout << "A * P1.transpose()" << std::endl << A * P1.transpose()<< std::endl << std::endl; - std::cout << "B * P1.transpose()" << std::endl << B * P1.transpose()<< std::endl << std::endl; + std::cout << "A * P1.transpose()" << std::endl << A * P1.transpose() << std::endl << std::endl; + std::cout << "B * P1.transpose()" << std::endl << B * P1.transpose() << std::endl << std::endl; std::cout << "Pre&post-multiplying:" << std::endl; std::cout << "P1 * A * P1.transpose()" << std::endl << P1 * A * P1.transpose() << std::endl << std::endl; - std::cout << "P2 * P1 * A * P1.transpose() * P2.transpose()" << std::endl << P2 * P1 * A * P1.transpose() * P2.transpose() << std::endl << std::endl; - std::cout << "P1 * P2 * A * P2.transpose() * P1.transpose()" << std::endl << P1 * P2 * A * P2.transpose() * P1.transpose() << std::endl << std::endl; + std::cout << "P2 * P1 * A * P1.transpose() * P2.transpose()" << std::endl + << P2 * P1 * A * P1.transpose() * P2.transpose() << std::endl + << std::endl; + std::cout << "P1 * P2 * A * P2.transpose() * P1.transpose()" << std::endl + << P1 * P2 * A * P2.transpose() * P1.transpose() << std::endl + << std::endl; P3 = P1 * P2; std::cout << "Permutation P3 = P1 * P2" << std::endl << P3.indices().transpose() << std::endl << std::endl; - std::cout << "P3 * A * P3.transpose()" << std::endl << P3 * A * P3.transpose() << std::endl << std::endl; + std::cout << "P3 * A * P3.transpose()" << std::endl << P3 * A * P3.transpose() << std::endl << std::endl; std::cout << "PERMUTATING INDICES" << std::endl; ArrayXi acc_permutations(5); - acc_permutations << 0,1,2,3,4; + acc_permutations << 0, 1, 2, 3, 4; std::cout << "acc_permutations: " << acc_permutations.transpose() << std::endl; std::cout << "P1: " << P1.indices().transpose() << std::endl; std::cout << "P1 * acc_permutations: " << (P1 * acc_permutations.matrix()).transpose() << std::endl; - std::cout << "P1.inverse() * acc_permutations: " << (P1.inverse() * acc_permutations.matrix()).transpose() << std::endl; + std::cout << "P1.inverse() * acc_permutations: " << (P1.inverse() * acc_permutations.matrix()).transpose() + << std::endl; std::cout << "P2: " << P2.indices().transpose() << std::endl; std::cout << "P2 * (P1 * acc_permutations): " << (P2 * (P1 * acc_permutations.matrix())).transpose() << std::endl; - std::cout << "(P2 * P1).inverse() * acc_permutations): " << ((P2 * P1).inverse() * acc_permutations.matrix()).transpose() << std::endl; + std::cout << "(P2 * P1).inverse() * acc_permutations): " + << ((P2 * P1).inverse() * acc_permutations.matrix()).transpose() << std::endl; P4 = P1 * P2 * P3; std::cout << "Permutation P4 = P1 * P2 * P3: " << P4.indices().transpose() << std::endl; - std::cout << "P3 * (P2 * (P1 * acc_permutations)): " << (P3 * (P2 * (P1 * acc_permutations.matrix()))).transpose() << std::endl; + std::cout << "P3 * (P2 * (P1 * acc_permutations)): " << (P3 * (P2 * (P1 * acc_permutations.matrix()))).transpose() + << std::endl; std::cout << "accumulated permutations can not be stored in vectors..." << std::endl; std::cout << std::endl << "PARTIALL PERMUTATIONS" << std::endl; @@ -104,10 +111,13 @@ int main(int argc, char *argv[]) std::cout << "P5 (equivalent to P1 but partiall): " << std::endl << P5.indices().transpose() << std::endl; std::cout << "P2: " << P2.indices().transpose() << std::endl << std::endl; std::cout << "A * P2.transpose(): " << std::endl << A * P2.transpose() << std::endl << std::endl; - std::cout << "(A * P2.transpose()).rightCols(2) * P5.transpose(): " << std::endl << (A * P2.transpose()).rightCols(2) * P5.transpose() << std::endl << std::endl; + std::cout << "(A * P2.transpose()).rightCols(2) * P5.transpose(): " << std::endl + << (A * P2.transpose()).rightCols(2) * P5.transpose() << std::endl + << std::endl; PermutationMatrix<Dynamic, Dynamic, int> P6 = P2; - P6.indices().tail(2) = P5 * P6.indices().tail(2); - std::cout << "P6 = P2, P6.indices().tail(2) = P5 * P6.indices().tail(2): " << P6.indices().transpose() << std::endl << std::endl; + P6.indices().tail(2) = P5 * P6.indices().tail(2); + std::cout << "P6 = P2, P6.indices().tail(2) = P5 * P6.indices().tail(2): " << P6.indices().transpose() << std::endl + << std::endl; std::cout << "A * P6.transpose(): " << std::endl << A * P6.transpose() << std::endl << std::endl; std::cout << "(P1 * P2): " << std::endl << (P1 * P2).indices().transpose() << std::endl << std::endl; std::cout << "A * (P1 * P2).transpose(): " << std::endl << A * (P1 * P2).transpose() << std::endl << std::endl; @@ -122,5 +132,5 @@ int main(int argc, char *argv[]) std::cout << "mapped_a = " << mapped_a.transpose() << std::endl << std::endl; std::cout << "maps are affected of the reorderings in mapped vectors" << std::endl; -// Map<> + // Map<> } diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h index 2310e147c6ba61dd0171bc64549c9fd4dc5837a8..63c7ed69e4940c9a922f9c1f2c596bd24a961897 100644 --- a/include/core/capture/capture_diff_drive.h +++ b/include/core/capture/capture_diff_drive.h @@ -20,11 +20,11 @@ #pragma once -//wolf includes +// wolf includes #include "core/capture/capture_motion.h" -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(CaptureDiffDrive) /** @@ -34,22 +34,20 @@ WOLF_PTR_TYPEDEFS(CaptureDiffDrive) */ class CaptureDiffDrive : public CaptureMotion { - -public: - - /** - * \brief Constructor - **/ - CaptureDiffDrive(const TimeStamp& _ts, - const SensorBasePtr& _sensor_ptr, - const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr, - StateBlockPtr _p_ptr = nullptr, - StateBlockPtr _o_ptr = nullptr, - StateBlockPtr _intr_ptr = nullptr); - - ~CaptureDiffDrive() override = default; + public: + /** + * \brief Constructor + **/ + CaptureDiffDrive(const TimeStamp& _ts, + const SensorBasePtr& _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + CaptureBasePtr _capture_origin_ptr, + StateBlockPtr _p_ptr = nullptr, + StateBlockPtr _o_ptr = nullptr, + StateBlockPtr _intr_ptr = nullptr); + + ~CaptureDiffDrive() override = default; }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h index f186ae1e9ad9e7c4cdcf08173e92415ad282a354..3dffe5bf7668e70b43a560136753a94cb9d7d895 100644 --- a/include/core/capture/capture_landmarks_external.h +++ b/include/core/capture/capture_landmarks_external.h @@ -20,41 +20,46 @@ #pragma once -//Wolf includes +// Wolf includes #include "core/capture/capture_base.h" -namespace wolf { - +namespace wolf +{ struct LandmarkDetection { - int id; // id of landmark + int id; // id of landmark Eigen::VectorXd measure; // either pose or position Eigen::MatrixXd covariance; // covariance of the measure - double quality; // [0, 1] quality of the detection + double quality; // [0, 1] quality of the detection }; WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal); -//class CaptureLandmarksExternal +// class CaptureLandmarksExternal class CaptureLandmarksExternal : public CaptureBase { - protected: - std::vector<LandmarkDetection> detections_; - - public: + protected: + std::vector<LandmarkDetection> detections_; - CaptureLandmarksExternal(const TimeStamp& _ts, - SensorBasePtr _sensor_ptr, - const std::vector<int>& _ids = {}, - const std::vector<Eigen::VectorXd>& _detections = {}, - const std::vector<Eigen::MatrixXd>& _covs = {}, - const std::vector<double>& _qualities = {}); + public: + CaptureLandmarksExternal(const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, + const std::vector<int>& _ids = {}, + const std::vector<Eigen::VectorXd>& _detections = {}, + const std::vector<Eigen::MatrixXd>& _covs = {}, + const std::vector<double>& _qualities = {}); - ~CaptureLandmarksExternal() override; + ~CaptureLandmarksExternal() override; - std::vector<LandmarkDetection> getDetections() const {return detections_;}; + std::vector<LandmarkDetection> getDetections() const + { + return detections_; + }; - void addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& quality); + void addDetection(const int& _id, + const Eigen::VectorXd& _detection, + const Eigen::MatrixXd& _cov, + const double& quality); }; -} //namespace wolf +} // namespace wolf diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h index 51de061db9c8d3b007f71d742352e210400b3ef5..b00f186bd42cfe8fec38481da63b5eda5d0e1cbc 100644 --- a/include/core/capture/capture_motion.h +++ b/include/core/capture/capture_motion.h @@ -30,10 +30,9 @@ #include <iterator> #include <utility> -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(CaptureMotion); - /** \brief Base class for motion Captures. * @@ -49,79 +48,82 @@ WOLF_PTR_TYPEDEFS(CaptureMotion); * - until the frame of this capture. * * Once a keyframe is generated, this buffer is frozen and kept in the Capture for eventual later uses. - * It is then used to compute the factor that links the Frame of this capture to the previous key-frame in the Trajectory. + * It is then used to compute the factor that links the Frame of this capture to the previous key-frame in the + * Trajectory. */ class CaptureMotion : public CaptureBase { - - // public interface: - - public: - CaptureMotion(const std::string & _type, - const TimeStamp& _ts, - SensorBasePtr _sensor_ptr, - const Eigen::VectorXd& _data, - CaptureBasePtr _capture_origin_ptr); - - CaptureMotion(const std::string & _type, - const TimeStamp& _ts, - SensorBasePtr _sensor_ptr, - const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr, - StateBlockPtr _p_ptr = nullptr, - StateBlockPtr _o_ptr = nullptr, - StateBlockPtr _intr_ptr = nullptr); - - ~CaptureMotion() override; - - // Type - bool isMotion() const override { return true; } - - // Data - const Eigen::VectorXd& getData() const; - const Eigen::MatrixXd& getDataCovariance() const; - void setData(const Eigen::VectorXd& _data); - void setDataCovariance(const Eigen::MatrixXd& _data_cov); - - // Buffer - MotionBuffer& getBuffer(); - const MotionBuffer& getBuffer() const; - - // Buffer's initial conditions for pre-integration - VectorXd getCalibrationPreint() const; - void setCalibrationPreint(const VectorXd& _calib_preint); - MatrixXd getJacobianCalib() const; - MatrixXd getJacobianCalib(const TimeStamp& _ts) const; - - // Get delta preintegrated, and corrected for changes on calibration params - VectorXd getDeltaPreint() const; - VectorXd getDeltaPreint(const TimeStamp& _ts) const; - MatrixXd getDeltaPreintCov() const; - MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const; - - // Origin frame and capture - CaptureBasePtr getOriginCapture(); - CaptureBaseConstPtr getOriginCapture() const; - void setOriginCapture(CaptureBasePtr _capture_origin_ptr); - - bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const; - - void printHeader(int depth, // - bool factored_by, // - bool metric, // - bool state_blocks, - std::ostream& stream , - std::string _tabs = "") const override; - - // member data: - private: - Eigen::VectorXd data_; ///< Motion data in form of vector mandatory - Eigen::MatrixXd data_cov_; ///< Motion data covariance - Eigen::VectorXd calib_preint_; ///< Calibration parameters used during pre-integration - MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one. - CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion + // public interface: + + public: + CaptureMotion(const std::string& _type, + const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + CaptureBasePtr _capture_origin_ptr); + + CaptureMotion(const std::string& _type, + const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + CaptureBasePtr _capture_origin_ptr, + StateBlockPtr _p_ptr = nullptr, + StateBlockPtr _o_ptr = nullptr, + StateBlockPtr _intr_ptr = nullptr); + + ~CaptureMotion() override; + + // Type + bool isMotion() const override + { + return true; + } + + // Data + const Eigen::VectorXd& getData() const; + const Eigen::MatrixXd& getDataCovariance() const; + void setData(const Eigen::VectorXd& _data); + void setDataCovariance(const Eigen::MatrixXd& _data_cov); + + // Buffer + MotionBuffer& getBuffer(); + const MotionBuffer& getBuffer() const; + + // Buffer's initial conditions for pre-integration + VectorXd getCalibrationPreint() const; + void setCalibrationPreint(const VectorXd& _calib_preint); + MatrixXd getJacobianCalib() const; + MatrixXd getJacobianCalib(const TimeStamp& _ts) const; + + // Get delta preintegrated, and corrected for changes on calibration params + VectorXd getDeltaPreint() const; + VectorXd getDeltaPreint(const TimeStamp& _ts) const; + MatrixXd getDeltaPreintCov() const; + MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const; + + // Origin frame and capture + CaptureBasePtr getOriginCapture(); + CaptureBaseConstPtr getOriginCapture() const; + void setOriginCapture(CaptureBasePtr _capture_origin_ptr); + + bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const; + + void printHeader(int depth, // + bool factored_by, // + bool metric, // + bool state_blocks, + std::ostream& stream, + std::string _tabs = "") const override; + + // member data: + private: + Eigen::VectorXd data_; ///< Motion data in form of vector mandatory + Eigen::MatrixXd data_cov_; ///< Motion data covariance + Eigen::VectorXd calib_preint_; ///< Calibration parameters used during pre-integration + MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one. + CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion }; inline const Eigen::VectorXd& CaptureMotion::getData() const @@ -212,4 +214,4 @@ inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const return getBuffer().getMotion(_ts).delta_integr_cov_; } -} // namespace wolf +} // namespace wolf diff --git a/include/core/capture/capture_odom_2d.h b/include/core/capture/capture_odom_2d.h index b111f605c200856e77c84ce84867ae9152fc0a9d..6b52e5c357cf4646ee0003708b76dd93621a63e6 100644 --- a/include/core/capture/capture_odom_2d.h +++ b/include/core/capture/capture_odom_2d.h @@ -26,25 +26,23 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(CaptureOdom2d); class CaptureOdom2d : public CaptureMotion { - public: - CaptureOdom2d(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, - const Eigen::VectorXd& _data, - CaptureBasePtr _capture_origin_ptr = nullptr); - - CaptureOdom2d(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, - const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr = nullptr); - - ~CaptureOdom2d() override; - + public: + CaptureOdom2d(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + CaptureBasePtr _capture_origin_ptr = nullptr); + + CaptureOdom2d(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + CaptureBasePtr _capture_origin_ptr = nullptr); + + ~CaptureOdom2d() override; }; } /* namespace wolf */ diff --git a/include/core/capture/capture_odom_3d.h b/include/core/capture/capture_odom_3d.h index 13463ab08db4a543588f774045bdb64c5f33a069..e93dc3c05a464b7b9e6d0433369ec709031b9be2 100644 --- a/include/core/capture/capture_odom_3d.h +++ b/include/core/capture/capture_odom_3d.h @@ -26,25 +26,23 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(CaptureOdom3d); class CaptureOdom3d : public CaptureMotion { - public: - CaptureOdom3d(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, - const Eigen::VectorXd& _data, - CaptureBasePtr _capture_origin_ptr = nullptr); - - CaptureOdom3d(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, - const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr = nullptr); - - ~CaptureOdom3d() override; - + public: + CaptureOdom3d(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + CaptureBasePtr _capture_origin_ptr = nullptr); + + CaptureOdom3d(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + CaptureBasePtr _capture_origin_ptr = nullptr); + + ~CaptureOdom3d() override; }; } /* namespace wolf */ diff --git a/include/core/capture/capture_pose.h b/include/core/capture/capture_pose.h index 64b95810032c5fd58a624ef795018d1d026a8d56..7ff804a0116dc79f537427e150b4fbe976a3e8af 100644 --- a/include/core/capture/capture_pose.h +++ b/include/core/capture/capture_pose.h @@ -20,40 +20,46 @@ #pragma once -//Wolf includes +// Wolf includes #include "core/capture/capture_base.h" #include "core/factor/factor_pose_2d.h" #include "core/factor/factor_pose_3d.h" #include "core/feature/feature_pose.h" -//std includes +// std includes // -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(CapturePose); -//class CapturePose +// class CapturePose class CapturePose : public CaptureBase { - protected: - Eigen::VectorXd data_; ///< Raw data. - Eigen::MatrixXd data_covariance_; ///< Noise of the capture. - - public: + protected: + Eigen::VectorXd data_; ///< Raw data. + Eigen::MatrixXd data_covariance_; ///< Noise of the capture. - CapturePose(const TimeStamp& _ts, - SensorBasePtr _sensor_ptr, - const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_covariance); + public: + CapturePose(const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_covariance); - ~CapturePose() override; + ~CapturePose() override; - virtual void emplaceFeatureAndFactor(const ProcessorBasePtr& _processor=nullptr, bool _apply_loss_function=false); - Eigen::VectorXd getData() const { return data_;} - Eigen::MatrixXd getDataCovariance() const { return data_covariance_;} - void setData(const Eigen::VectorXd& _data); - void setDataCovariance(const Eigen::MatrixXd& _data_covariance); + virtual void emplaceFeatureAndFactor(const ProcessorBasePtr& _processor = nullptr, + bool _apply_loss_function = false); + Eigen::VectorXd getData() const + { + return data_; + } + Eigen::MatrixXd getDataCovariance() const + { + return data_covariance_; + } + void setData(const Eigen::VectorXd& _data); + void setDataCovariance(const Eigen::MatrixXd& _data_covariance); }; -} //namespace wolf +} // namespace wolf diff --git a/include/core/capture/capture_void.h b/include/core/capture/capture_void.h index 8360c6216851440df783fafd443818d1fd28f950..1b5653cba1f0b25cfae052b033fd92f290b6c641 100644 --- a/include/core/capture/capture_void.h +++ b/include/core/capture/capture_void.h @@ -20,21 +20,19 @@ #pragma once -//Wolf includes +// Wolf includes #include "core/capture/capture_base.h" -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(CaptureVoid); - - -//class CaptureVoid + +// class CaptureVoid class CaptureVoid : public CaptureBase { - public: - CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr); - ~CaptureVoid() override; - + public: + CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr); + ~CaptureVoid() override; }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/ceres_wrapper/cost_function_wrapper.h b/include/core/ceres_wrapper/cost_function_wrapper.h index 0fa474d37fa178119385e1f05d57fc433f4e46dc..d60fe7937a5c9ce2931194f666173d41ff265512 100644 --- a/include/core/ceres_wrapper/cost_function_wrapper.h +++ b/include/core/ceres_wrapper/cost_function_wrapper.h @@ -30,40 +30,35 @@ // EIGEN #include <Eigen/StdVector> -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(CostFunctionWrapper); class CostFunctionWrapper : public ceres::CostFunction { - private: - - FactorBasePtr factor_ptr_; + private: + FactorBasePtr factor_ptr_; - public: + public: + CostFunctionWrapper(FactorBasePtr _factor_ptr); - CostFunctionWrapper(FactorBasePtr _factor_ptr); + ~CostFunctionWrapper() override; - ~CostFunctionWrapper() override; + bool Evaluate(const double* const* parameters, double* residuals, double** jacobians) const override; - bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const override; - - FactorBasePtr getFactor() const; + FactorBasePtr getFactor() const; }; -inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr) : - ceres::CostFunction(), factor_ptr_(_factor_ptr) +inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr) + : ceres::CostFunction(), factor_ptr_(_factor_ptr) { - for (auto st_block_size : factor_ptr_->getStateSizes()) - mutable_parameter_block_sizes()->push_back(st_block_size); + for (auto st_block_size : factor_ptr_->getStateSizes()) mutable_parameter_block_sizes()->push_back(st_block_size); set_num_residuals(factor_ptr_->getSize()); } -inline CostFunctionWrapper::~CostFunctionWrapper() -{ -} +inline CostFunctionWrapper::~CostFunctionWrapper() {} -inline bool CostFunctionWrapper::Evaluate(const double* const * parameters, double* residuals, double** jacobians) const +inline bool CostFunctionWrapper::Evaluate(const double* const* parameters, double* residuals, double** jacobians) const { return factor_ptr_->evaluate(parameters, residuals, jacobians); } @@ -73,4 +68,4 @@ inline FactorBasePtr CostFunctionWrapper::getFactor() const return factor_ptr_; } -} // namespace wolf +} // namespace wolf diff --git a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h index 4447ff577fef3529060f1c22cf8b628485c6b9de..cbde48524e2231eb837a2245052dacea6930ac91 100644 --- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h +++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h @@ -26,35 +26,58 @@ // Factors #include "core/factor/factor_base.h" -namespace wolf { - +namespace wolf +{ // Wolf ceres auto_diff creator template <class T> -std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize, - T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size, - T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> > createNumericDiffCostFunctionCeres(FactorBasePtr _factor_ptr) +std::shared_ptr<ceres::NumericDiffCostFunction<T, + ceres::CENTRAL, + T::residualSize, + T::block0Size, + T::block1Size, + T::block2Size, + T::block3Size, + T::block4Size, + T::block5Size, + T::block6Size, + T::block7Size, + T::block8Size, + T::block9Size> > +createNumericDiffCostFunctionCeres(FactorBasePtr _factor_ptr) { - return std::make_shared<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize, - T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size, - T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_factor_ptr).get()); + return std::make_shared<ceres::NumericDiffCostFunction<T, + ceres::CENTRAL, + T::residualSize, + T::block0Size, + T::block1Size, + T::block2Size, + T::block3Size, + T::block4Size, + T::block5Size, + T::block6Size, + T::block7Size, + T::block8Size, + T::block9Size> >( + std::static_pointer_cast<T>(_factor_ptr).get()); }; inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(FactorBasePtr _fac_ptr) { -// switch (_fac_ptr->getTypeId()) -// { -// // just for testing -// case FAC_ODOM_2d: -// return createNumericDiffCostFunctionCeres<FactorOdom2d>(_fac_ptr); -// -// /* For adding a new factor, add the #include and a case: -// case FAC_ENUM: -// return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr); -// */ -// -// default: - throw std::invalid_argument( "Unknown factor type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" ); -// } + // switch (_fac_ptr->getTypeId()) + // { + // // just for testing + // case FAC_ODOM_2d: + // return createNumericDiffCostFunctionCeres<FactorOdom2d>(_fac_ptr); + // + // /* For adding a new factor, add the #include and a case: + // case FAC_ENUM: + // return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr); + // */ + // + // default: + throw std::invalid_argument( + "Unknown factor type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h"); + // } } -} // namespace wolf +} // namespace wolf diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h index dd01fb5bb69966078ebf32d66141766c5430c8bf..4a88d7e7c172b2a2e50b072a09f63aaffbf47b16 100644 --- a/include/core/ceres_wrapper/iteration_update_callback.h +++ b/include/core/ceres_wrapper/iteration_update_callback.h @@ -23,48 +23,50 @@ #include "core/problem/problem.h" #include "ceres/ceres.h" -namespace wolf { - +namespace wolf +{ class IterationUpdateCallback : public ceres::IterationCallback { - public: - explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false) - : problem_(_problem) - , min_num_iterations_(min_num_iterations) - , initial_cost_(0) - , verbose_(verbose) - {} + public: + explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false) + : problem_(_problem), min_num_iterations_(min_num_iterations), initial_cost_(0), verbose_(verbose) + { + } - ~IterationUpdateCallback() {} + ~IterationUpdateCallback() {} - ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override - { - if (summary.iteration == 0) - initial_cost_ = summary.cost; + ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override + { + if (summary.iteration == 0) + initial_cost_ = summary.cost; - else if (summary.iteration >= min_num_iterations_ and - (problem_->getStateBlockNotificationMapSize() != 0 or - problem_->getFactorNotificationMapSize() != 0)) + else if (summary.iteration >= min_num_iterations_ and + (problem_->getStateBlockNotificationMapSize() != 0 or problem_->getFactorNotificationMapSize() != 0)) + { + if (summary.cost >= initial_cost_) + { + WOLF_INFO_COND( + verbose_, + "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ", + summary.iteration); + return ceres::SOLVER_ABORT; + } + else { - if (summary.cost >= initial_cost_) - { - WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ", summary.iteration); - return ceres::SOLVER_ABORT; - } - else - { - WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ", summary.iteration); - return ceres::SOLVER_TERMINATE_SUCCESSFULLY; - } + WOLF_INFO_COND(verbose_, + "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ", + summary.iteration); + return ceres::SOLVER_TERMINATE_SUCCESSFULLY; } - return ceres::SOLVER_CONTINUE; } + return ceres::SOLVER_CONTINUE; + } - private: - ProblemPtr problem_; - int min_num_iterations_; - double initial_cost_; - bool verbose_; + private: + ProblemPtr problem_; + int min_num_iterations_; + double initial_cost_; + bool verbose_; }; -} +} // namespace wolf diff --git a/include/core/ceres_wrapper/local_parametrization_wrapper.h b/include/core/ceres_wrapper/local_parametrization_wrapper.h index bce52b1fade779278c7fbe04c55f00fd70abd282..03cd030e8d070d11295cffe79e4ca154f2c009b1 100644 --- a/include/core/ceres_wrapper/local_parametrization_wrapper.h +++ b/include/core/ceres_wrapper/local_parametrization_wrapper.h @@ -21,48 +21,48 @@ #pragma once // Fwd refs -namespace wolf{ +namespace wolf +{ class LocalParametrizationBase; } -//Ceres includes +// Ceres includes #include "core/common/wolf.h" #include "ceres/ceres.h" -namespace wolf { - +namespace wolf +{ class LocalParametrizationWrapper : public ceres::LocalParameterization { - private: - LocalParametrizationBasePtr local_parametrization_ptr_; + private: + LocalParametrizationBasePtr local_parametrization_ptr_; - public: + public: + LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr); - LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr); + ~LocalParametrizationWrapper() override = default; - ~LocalParametrizationWrapper() override = default; + bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override; - bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; - bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override; - int GlobalSize() const override; + int LocalSize() const override; - int LocalSize() const override; - - LocalParametrizationBasePtr getLocalParametrization() const; + LocalParametrizationBasePtr getLocalParametrization() const; }; using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>; -} // namespace wolf +} // namespace wolf #include "core/state_block/local_parametrization_base.h" -namespace wolf { - -inline LocalParametrizationWrapper::LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr) : - local_parametrization_ptr_(_local_parametrization_ptr) +namespace wolf +{ +inline LocalParametrizationWrapper::LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr) + : local_parametrization_ptr_(_local_parametrization_ptr) { } @@ -81,4 +81,4 @@ inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametr return local_parametrization_ptr_; } -} // namespace wolf +} // namespace wolf diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h index 7b961de2b9794de7941155f44e1ebde52602f632..6087e4bf218d7753ffcf0e7de8798852aac220ab 100644 --- a/include/core/ceres_wrapper/qr_manager.h +++ b/include/core/ceres_wrapper/qr_manager.h @@ -25,48 +25,45 @@ namespace wolf { - class QRManager : public SolverManager { - protected: - Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_; - Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_; - Eigen::SparseMatrixd A_; - Eigen::VectorXd b_; - std::map<StateBlockPtr, unsigned int> sb_2_col_; - std::map<FactorBasePtr, unsigned int> fac_2_row_; - bool any_state_block_removed_; - unsigned int new_state_blocks_; - unsigned int N_batch_; - bool pending_changes_; - - public: - - QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch); + protected: + Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_; + Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_; + Eigen::SparseMatrixd A_; + Eigen::VectorXd b_; + std::map<StateBlockPtr, unsigned int> sb_2_col_; + std::map<FactorBasePtr, unsigned int> fac_2_row_; + bool any_state_block_removed_; + unsigned int new_state_blocks_; + unsigned int N_batch_; + bool pending_changes_; - virtual ~QRManager(); + public: + QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch); - virtual std::string solve(const unsigned int& _report_level); + virtual ~QRManager(); - virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS); + virtual std::string solve(const unsigned int& _report_level); - virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list); + virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS); - private: + virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list); - bool computeDecomposition(); + private: + bool computeDecomposition(); - virtual void addFactor(FactorBasePtr _fac_ptr); + virtual void addFactor(FactorBasePtr _fac_ptr); - virtual void removeFactor(FactorBasePtr _fac_ptr); + virtual void removeFactor(FactorBasePtr _fac_ptr); - virtual void addStateBlock(StateBlockPtr _st_ptr); + virtual void addStateBlock(StateBlockPtr _st_ptr); - virtual void removeStateBlock(StateBlockPtr _st_ptr); + virtual void removeStateBlock(StateBlockPtr _st_ptr); - virtual void updateStateBlockStatus(StateBlockPtr _st_ptr); + virtual void updateStateBlockStatus(StateBlockPtr _st_ptr); - void relinearizeFactor(FactorBasePtr _fac_ptr); + void relinearizeFactor(FactorBasePtr _fac_ptr); }; } /* namespace wolf */ diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h index 7d3047dd84810175b6c9a3ba2ea3c98be47fabd8..7d3d1350fbc17f6f0bde7010d392ae8277ddcd69 100644 --- a/include/core/ceres_wrapper/solver_ceres.h +++ b/include/core/ceres_wrapper/solver_ceres.h @@ -20,99 +20,100 @@ #pragma once -//Ceres includes +// Ceres includes #include "ceres/jet.h" #include "ceres/ceres.h" #include "glog/logging.h" -//wolf includes +// wolf includes #include "core/solver/solver_manager.h" #include "yaml-cpp/yaml.h" -namespace ceres { -typedef std::shared_ptr<CostFunction> CostFunctionPtr; +namespace ceres +{ +typedef std::shared_ptr<CostFunction> CostFunctionPtr; } -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(SolverCeres); WOLF_PTR_TYPEDEFS(LocalParametrizationWrapper); WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres); struct ParamsCeres : public ParamsSolver { - bool interrupt_on_problem_change = false; - int min_num_iterations = 1; - ceres::Solver::Options solver_options; - ceres::Problem::Options problem_options; + bool interrupt_on_problem_change = false; + int min_num_iterations = 1; + ceres::Solver::Options solver_options; + ceres::Problem::Options problem_options; ceres::Covariance::Options covariance_options; - ParamsCeres() : - ParamsSolver() + ParamsCeres() : ParamsSolver() { loadHardcodedValues(); } - ParamsCeres(const YAML::Node& _input_node) : - ParamsSolver(_input_node) + ParamsCeres(const YAML::Node& _input_node) : ParamsSolver(_input_node) { loadHardcodedValues(); // interrupt solver whenever the problem is updated (via ceres::iterationCallback) - interrupt_on_problem_change = _input_node["interrupt_on_problem_change"].as<bool>(); - if (interrupt_on_problem_change) - min_num_iterations = _input_node["min_num_iterations"].as<int>(); + interrupt_on_problem_change = _input_node["interrupt_on_problem_change"].as<bool>(); + if (interrupt_on_problem_change) min_num_iterations = _input_node["min_num_iterations"].as<int>(); // CERES SOLVER OPTIONS - solver_options.max_num_iterations = _input_node["max_num_iterations"].as<int>(); - solver_options.function_tolerance = _input_node["function_tolerance"].as<double>(); - solver_options.gradient_tolerance = _input_node["gradient_tolerance"].as<double>(); - solver_options.num_threads = _input_node["n_threads"].as<int>(); - covariance_options.num_threads = solver_options.num_threads; + solver_options.max_num_iterations = _input_node["max_num_iterations"].as<int>(); + solver_options.function_tolerance = _input_node["function_tolerance"].as<double>(); + solver_options.gradient_tolerance = _input_node["gradient_tolerance"].as<double>(); + solver_options.num_threads = _input_node["n_threads"].as<int>(); + covariance_options.num_threads = solver_options.num_threads; // minimizer type - std::string minimizer = _input_node["minimizer"].as<std::string>(); + std::string minimizer = _input_node["minimizer"].as<std::string>(); if (minimizer == "LEVENBERG_MARQUARDT" or minimizer == "levenberg_marquardt") { - solver_options.minimizer_type = ceres::TRUST_REGION; + solver_options.minimizer_type = ceres::TRUST_REGION; solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; } else if (minimizer == "DOGLEG" or minimizer == "dogleg") { - solver_options.minimizer_type = ceres::TRUST_REGION; + solver_options.minimizer_type = ceres::TRUST_REGION; solver_options.trust_region_strategy_type = ceres::DOGLEG; } else if (minimizer == "LBFGS" or minimizer == "lbfgs") { - solver_options.minimizer_type = ceres::LINE_SEARCH; + solver_options.minimizer_type = ceres::LINE_SEARCH; solver_options.line_search_direction_type = ceres::LBFGS; } else if (minimizer == "BFGS" or minimizer == "bfgs") { - solver_options.minimizer_type = ceres::LINE_SEARCH; + solver_options.minimizer_type = ceres::LINE_SEARCH; solver_options.line_search_direction_type = ceres::BFGS; } else { - throw std::runtime_error("ParamsCeres: Wrong parameter 'minimizer'. Should be 'LEVENBERG_MARQUARDT', 'DOGLEG', 'LBFGS' or 'BFGS' (upper or lowercase)"); + throw std::runtime_error( + "ParamsCeres: Wrong parameter 'minimizer'. Should be 'LEVENBERG_MARQUARDT', 'DOGLEG', 'LBFGS' or " + "'BFGS' (upper or lowercase)"); } // specific options for TRUST REGION if (solver_options.minimizer_type == ceres::TRUST_REGION) { - solver_options.use_nonmonotonic_steps = _input_node["use_nonmonotonic_steps"].as<bool>(); + solver_options.use_nonmonotonic_steps = _input_node["use_nonmonotonic_steps"].as<bool>(); if (solver_options.use_nonmonotonic_steps) - solver_options.max_consecutive_nonmonotonic_steps = _input_node["max_consecutive_nonmonotonic_steps"].as<int>(); + solver_options.max_consecutive_nonmonotonic_steps = + _input_node["max_consecutive_nonmonotonic_steps"].as<int>(); } } void loadHardcodedValues() { - solver_options = ceres::Solver::Options(); - problem_options = ceres::Problem::Options(); - covariance_options = ceres::Covariance::Options(); - problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP; + solver_options = ceres::Solver::Options(); + problem_options = ceres::Problem::Options(); + covariance_options = ceres::Covariance::Options(); + problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP; problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; covariance_options.apply_loss_function = false; @@ -123,91 +124,87 @@ struct ParamsCeres : public ParamsSolver class SolverCeres : public SolverManager { - protected: - - std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_; - std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_; - - // this map is only for handling automatic destruction of localParametrizationWrapper objects - std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_; - - ceres::Solver::Summary summary_; - std::unique_ptr<ceres::Problem> ceres_problem_; - std::unique_ptr<ceres::Covariance> covariance_; + protected: + std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_; + std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_; - ParamsCeresPtr params_ceres_; + // this map is only for handling automatic destruction of localParametrizationWrapper objects + std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_; - // profiling - unsigned int n_iter_acc_, n_iter_max_; - unsigned int n_convergence_, n_interrupted_, n_no_convergence_; + ceres::Solver::Summary summary_; + std::unique_ptr<ceres::Problem> ceres_problem_; + std::unique_ptr<ceres::Covariance> covariance_; - public: + ParamsCeresPtr params_ceres_; - SolverCeres(const ProblemPtr& _wolf_problem); + // profiling + unsigned int n_iter_acc_, n_iter_max_; + unsigned int n_convergence_, n_interrupted_, n_no_convergence_; - SolverCeres(const ProblemPtr& _wolf_problem, - const ParamsCeresPtr& _params); + public: + SolverCeres(const ProblemPtr& _wolf_problem); - WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres); + SolverCeres(const ProblemPtr& _wolf_problem, const ParamsCeresPtr& _params); - ~SolverCeres() override; + WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres); - ceres::Solver::Summary getSummary(); + ~SolverCeres() override; - std::unique_ptr<ceres::Problem>& getCeresProblem(); + ceres::Solver::Summary getSummary(); - bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks - = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; + std::unique_ptr<ceres::Problem>& getCeresProblem(); - bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override; + bool computeCovariancesDerived( + CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; - bool hasConverged() override; - bool wasStopped() override; - unsigned int iterations() override; - double initialCost() override; - double finalCost() override; - double totalTime() override; + bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override; - ceres::Solver::Options& getSolverOptions(); + bool hasConverged() override; + bool wasStopped() override; + unsigned int iterations() override; + double initialCost() override; + double finalCost() override; + double totalTime() override; - const Eigen::SparseMatrixd computeHessian() const; + ceres::Solver::Options& getSolverOptions(); - virtual int numStateBlocksDerived() const override; + const Eigen::SparseMatrixd computeHessian() const; - virtual int numFactorsDerived() const override; + virtual int numStateBlocksDerived() const override; - protected: + virtual int numFactorsDerived() const override; - bool checkDerived(std::string prefix="") const override; + protected: + bool checkDerived(std::string prefix = "") const override; - std::string solveDerived(const ReportVerbosity report_level) override; + std::string solveDerived(const ReportVerbosity report_level) override; - void addFactorDerived(const FactorBasePtr& fac_ptr) override; + void addFactorDerived(const FactorBasePtr& fac_ptr) override; - void removeFactorDerived(const FactorBasePtr& fac_ptr) override; + void removeFactorDerived(const FactorBasePtr& fac_ptr) override; - void addStateBlockDerived(const StateBlockPtr& state_ptr) override; + void addStateBlockDerived(const StateBlockPtr& state_ptr) override; - void removeStateBlockDerived(const StateBlockPtr& state_ptr) override; + void removeStateBlockDerived(const StateBlockPtr& state_ptr) override; - void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override; + void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override; - void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override; + void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override; - ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr); + ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr); - bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override; + bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override; - bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override; + bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override; - bool isStateBlockFixedDerived(const StateBlockPtr& st) override; + bool isStateBlockFixedDerived(const StateBlockPtr& st) override; - bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, - const LocalParametrizationBasePtr& local_param) override; + bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) override; - bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override; + bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override; - void printProfilingDerived(std::ostream& _stream) const override; + void printProfilingDerived(std::ostream& _stream) const override; }; inline ceres::Solver::Summary SolverCeres::getSummary() @@ -227,21 +224,18 @@ inline ceres::Solver::Options& SolverCeres::getSolverOptions() inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const { - return fac_2_residual_idx_.count(fac_ptr) == 1 and - fac_2_costfunction_.count(fac_ptr) == 1; + return fac_2_residual_idx_.count(fac_ptr) == 1 and fac_2_costfunction_.count(fac_ptr) == 1; } inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const { - if (state_blocks_.count(state_ptr) == 0) - return false; + if (state_blocks_.count(state_ptr) == 0) return false; return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)); } inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st) { - if (state_blocks_.count(st) == 0) - return false; + if (state_blocks_.count(st) == 0) return false; return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st)); }; @@ -260,4 +254,4 @@ inline int SolverCeres::numFactorsDerived() const return ceres_problem_->NumResidualBlocks(); }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h index aa8d3ec2b2877ce127601e8d0e77dcfed602ddef..173c2b88958112023c494ac63b6d4a321974b554 100644 --- a/include/core/ceres_wrapper/sparse_utils.h +++ b/include/core/ceres_wrapper/sparse_utils.h @@ -25,62 +25,80 @@ namespace wolf { - -void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsigned int& _row, const unsigned int& _n_rows) +void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, + const unsigned int& _row, + const unsigned int& _n_rows) { - A.middleRows(_row,_n_rows) = Eigen::SparseMatrixd(_n_rows,A.cols()); + A.middleRows(_row, _n_rows) = Eigen::SparseMatrixd(_n_rows, A.cols()); A.makeCompressed(); } -void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsigned int& _row, const unsigned int& _n_rows) +void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, + const unsigned int& _row, + const unsigned int& _n_rows) { A.prune([&](int i, int, double) { return i >= _row && i < _row + _n_rows; }); A.makeCompressed(); } -void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsigned int& _col, const unsigned int& _n_cols) +void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, + const unsigned int& _col, + const unsigned int& _n_cols) { - A.middleCols(_col,_n_cols) = Eigen::SparseMatrixd(A.rows(),_n_cols); + A.middleCols(_col, _n_cols) = Eigen::SparseMatrixd(A.rows(), _n_cols); A.makeCompressed(); } -void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsigned int& _col, const unsigned int& _n_cols) +void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, + const unsigned int& _col, + const unsigned int& _n_cols) { A.prune([&](int, int j, double) { return j >= _col && j < _col + _n_cols; }); A.makeCompressed(); } -template<int _Options, typename _StorageIndex> -void assignSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col) +template <int _Options, typename _StorageIndex> +void assignSparseBlock(const Eigen::MatrixXd& ins, + Eigen::SparseMatrix<double, _Options, _StorageIndex>& original, + const unsigned int& row, + const unsigned int& col) { for (auto ins_row = 0; ins_row < ins.rows(); ins_row++) for (auto ins_col = 0; ins_col < ins.cols(); ins_col++) - original.coeffRef(row+ins_row, col+ins_col) = ins(ins_row,ins_col); + original.coeffRef(row + ins_row, col + ins_col) = ins(ins_row, ins_col); original.makeCompressed(); } -template<int _Options, typename _StorageIndex> -void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col) +template <int _Options, typename _StorageIndex> +void addSparseBlock(const Eigen::MatrixXd& ins, + Eigen::SparseMatrix<double, _Options, _StorageIndex>& original, + const unsigned int& row, + const unsigned int& col) { for (auto ins_row = 0; ins_row < ins.rows(); ins_row++) for (auto ins_col = 0; ins_col < ins.cols(); ins_col++) - original.coeffRef(row+ins_row, col+ins_col) += ins(ins_row,ins_col); + original.coeffRef(row + ins_row, col + ins_col) += ins(ins_row, ins_col); original.makeCompressed(); } -template<int _Options, typename _StorageIndex> -void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col) +template <int _Options, typename _StorageIndex> +void insertSparseBlock(const Eigen::MatrixXd& ins, + Eigen::SparseMatrix<double, _Options, _StorageIndex>& original, + const unsigned int& row, + const unsigned int& col) { for (auto ins_row = 0; ins_row < ins.rows(); ins_row++) for (auto ins_col = 0; ins_col < ins.cols(); ins_col++) - original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col); + original.insert(row + ins_row, col + ins_col) = ins(ins_row, ins_col); original.makeCompressed(); } -void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen::SparseMatrix<double, Eigen::RowMajor>& ins, const unsigned int& _row) +void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, + const Eigen::SparseMatrix<double, Eigen::RowMajor>& ins, + const unsigned int& _row) { assert(A.rows() >= _row + ins.rows() && A.cols() == ins.cols()); A.middleRows(_row, ins.rows()) = ins; @@ -89,10 +107,10 @@ void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen Eigen::SparseMatrixd createBlockDiagonal(const std::vector<Eigen::MatrixXd>& _diag_blocs) { - unsigned int dim = _diag_blocs.front().rows(); + unsigned int dim = _diag_blocs.front().rows(); unsigned int size = dim * _diag_blocs.size(); - Eigen::SparseMatrixd M(size,size); + Eigen::SparseMatrixd M(size, size); unsigned int pos = 0; for (const Eigen::MatrixXd& omega_k : _diag_blocs) @@ -106,13 +124,14 @@ Eigen::SparseMatrixd createBlockDiagonal(const std::vector<Eigen::MatrixXd>& _di return M; } -template<int _Options, typename _StorageIndex> -void getDiagonalBlocks(const Eigen::SparseMatrix<double,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXd>& diag_, const unsigned int& dim) +template <int _Options, typename _StorageIndex> +void getDiagonalBlocks(const Eigen::SparseMatrix<double, _Options, _StorageIndex>& _M, + std::vector<Eigen::MatrixXd>& diag_, + const unsigned int& dim) { assert(_M.rows() % dim == 0 && "number of rows must be multiple of dimension"); diag_.clear(); - for (auto i = 0; i < _M.rows(); i += dim) - diag_.push_back(Eigen::MatrixXd(_M.block(i,i,dim,dim))); + for (auto i = 0; i < _M.rows(); i += dim) diag_.push_back(Eigen::MatrixXd(_M.block(i, i, dim, dim))); } -} // namespace wolf +} // namespace wolf diff --git a/include/core/ceres_wrapper/wolf_jet.h b/include/core/ceres_wrapper/wolf_jet.h index a4a5888dd49ad928e06b00e405c1e5b7fdd0f67c..962be7f6e8602f781dbe95c5af8e2d76b875bbab 100644 --- a/include/core/ceres_wrapper/wolf_jet.h +++ b/include/core/ceres_wrapper/wolf_jet.h @@ -25,8 +25,8 @@ #include "core/math/rotations.h" -namespace ceres{ - +namespace ceres +{ // // efficient implementation of pi2pi for jets (without while) // template<int N> // inline ceres::Jet<double, N> pi2pi(const ceres::Jet<double, N>& _angle) @@ -39,11 +39,11 @@ namespace ceres{ using std::remainder; -template<int N> +template <int N> inline Jet<double, N> remainder(const Jet<double, N>& _x, long double _y) { Jet<double, N> res = _x; - res.a = remainder(_x.a, _y); + res.a = remainder(_x.a, _y); return res; } -} +} // namespace ceres diff --git a/include/core/common/factory.h b/include/core/common/factory.h index d7a874faf080aec8959040255798ff7987b019ed..32f0ed481238bfc6db79ee24463e1a7dbbe60825 100644 --- a/include/core/common/factory.h +++ b/include/core/common/factory.h @@ -31,7 +31,6 @@ namespace wolf { - /** \brief Singleton template factory * * This template class implements a generic factory as a singleton. @@ -288,7 +287,7 @@ class Factory static Factory& get(); public: - Factory(const Factory&) = delete; + Factory(const Factory&) = delete; void operator=(Factory const&) = delete; private: @@ -391,7 +390,6 @@ inline std::list<std::string> Factory<TypeBase, TypeInput...>::getRegisteredKeys namespace wolf { - #ifdef __GNUC__ #define WOLF_UNUSED __attribute__((used)) #elif defined _MSC_VER diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h index 4873417445ee629ab006dae22727223c367f52dc..d5636ed9a244c74401eaf50e0ba6b39fc021b4e1 100644 --- a/include/core/common/node_base.h +++ b/include/core/common/node_base.h @@ -24,8 +24,8 @@ #include "core/common/wolf.h" #include "core/utils/check_log.h" -namespace wolf { - +namespace wolf +{ /** \brief Base class for Nodes * * Base class for all Nodes in the Wolf tree. Each node has @@ -46,7 +46,8 @@ namespace wolf { * - "MAP" * - "LANDMARK" * - * - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples follow: + * - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples + *follow: * - "SensorCamera" * - "SensorLaser2d" * - "LandmarkPoint3d" @@ -73,54 +74,56 @@ namespace wolf { class NodeBase { - friend Problem; - private: - static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory) + friend Problem; + + private: + static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory) - struct Serializer; + struct Serializer; - protected: - ProblemWPtr problem_ptr_; + protected: + ProblemWPtr problem_ptr_; - unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree - std::string node_category_; ///< Text label identifying the category of node ("SENSOR", "FEATURE", etc) - std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc) - std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc) + unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree + std::string node_category_; ///< Text label identifying the category of node ("SENSOR", "FEATURE", etc) + std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc) + std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", + ///< "Andrew", etc) - bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). + bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). - virtual void setProblem(ProblemPtr _prob_ptr); - public: + virtual void setProblem(ProblemPtr _prob_ptr); - NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = ""); - virtual ~NodeBase() = default; + public: + NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = ""); + virtual ~NodeBase() = default; - unsigned int nodeId() const; - std::string getCategory() const; - std::string getType() const; - std::string getName() const; - bool isRemoving() const; - virtual bool hasChildren() const = 0; + unsigned int nodeId() const; + std::string getCategory() const; + std::string getType() const; + std::string getName() const; + bool isRemoving() const; + virtual bool hasChildren() const = 0; - void setType(const std::string& _type); - void setName(const std::string& _name); - ProblemConstPtr getProblem() const; - ProblemPtr getProblem(); + void setType(const std::string& _type); + void setName(const std::string& _name); + ProblemConstPtr getProblem() const; + ProblemPtr getProblem(); }; -} // namespace wolf +} // namespace wolf #include <iostream> -namespace wolf{ - -inline NodeBase::NodeBase(const std::string& _category, const std::string& _type, const std::string& _name) : - problem_ptr_(), // nullptr - node_id_(++node_id_count_), - node_category_(_category), - node_type_(_type), - node_name_(_name), - is_removing_(false) +namespace wolf +{ +inline NodeBase::NodeBase(const std::string& _category, const std::string& _type, const std::string& _name) + : problem_ptr_(), // nullptr + node_id_(++node_id_count_), + node_category_(_category), + node_type_(_type), + node_name_(_name), + is_removing_(false) { // } @@ -175,4 +178,4 @@ inline void NodeBase::setProblem(ProblemPtr _prob_ptr) problem_ptr_ = _prob_ptr; } -} // namespace wolf +} // namespace wolf diff --git a/include/core/common/node_state_blocks.h b/include/core/common/node_state_blocks.h index 8f0215435a78e03888d3273271aafb553e72edac..ea5d6ed8d0100f03328c88f9d19bde1921baf9d9 100644 --- a/include/core/common/node_state_blocks.h +++ b/include/core/common/node_state_blocks.h @@ -101,9 +101,9 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod * **/ void emplaceFactorStateBlock(const char& _key, - const Eigen::VectorXd& _x, - const Eigen::MatrixXd& _cov, - unsigned int _start_idx = 0); + const Eigen::VectorXd& _x, + const Eigen::MatrixXd& _cov, + unsigned int _start_idx = 0); void emplacePriors(const SpecStateComposite& _specs); @@ -160,9 +160,7 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod bool _state_blocks, std::ostream& _stream, std::string _tabs) const; - CheckLog checkStatesAndFactoredBy(bool _verbose, - std::ostream& _stream, - std::string _tabs) const; + CheckLog checkStatesAndFactoredBy(bool _verbose, std::ostream& _stream, std::string _tabs) const; private: Composite<StateBlockPtr> state_block_composite_; @@ -180,7 +178,6 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod namespace wolf { - inline NodeStateBlocks::~NodeStateBlocks() { // diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h index 8ae98bcabfd39f63314523fe5c18222f811b3c81..05a198bb37eb7112adcfbf5c5c88caf526323d61 100644 --- a/include/core/common/params_base.h +++ b/include/core/common/params_base.h @@ -22,22 +22,21 @@ #include "yaml-cpp/yaml.h" -namespace wolf +namespace wolf { - struct ParamsBase { - ParamsBase() = default; - ParamsBase(const YAML::Node& _n) - { - // - } - - virtual ~ParamsBase() = default; - virtual std::string print() const = 0; + ParamsBase() = default; + ParamsBase(const YAML::Node& _n) + { + // + } + + virtual ~ParamsBase() = default; + virtual std::string print() const = 0; }; -template<typename Derived> +template <typename Derived> std::string toString(const Eigen::DenseBase<Derived>& mat) { std::stringstream ss; @@ -108,4 +107,4 @@ inline std::string toString(long double _arg) { return std::to_string(_arg); } -} +} // namespace wolf diff --git a/include/core/common/time_stamp.h b/include/core/common/time_stamp.h index 36aa22711dab73deb1fd9962ceed68b2acd46e1b..f0f91e051fc9cea2c0140d76e58735ac272ec16e 100644 --- a/include/core/common/time_stamp.h +++ b/include/core/common/time_stamp.h @@ -20,230 +20,229 @@ #pragma once -//wolf includes +// wolf includes #include "core/common/wolf.h" -//C, std +// C, std #include <sys/time.h> #include <iostream> #include <iomanip> static const unsigned int NANOSECS = 1000000000; -namespace wolf { - +namespace wolf +{ /** * \brief TimeStamp implements basic functionalities for time stamps - * + * * TimeStamp implements basic functionalities for time stamps */ class TimeStamp { - protected: - unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970. - bool is_valid_; // time stamp has a valid value - - public: - /** \brief Constructor - * - * Constructor without arguments. Sets time stamp to now. - * - */ - TimeStamp(); - - /** \brief Copy constructor - * - * Copy constructor - * - */ - TimeStamp(const TimeStamp& _ts); - - /** \brief Constructor with argument - * - * Constructor with arguments - * - */ - TimeStamp(const double& _ts); - - /** \brief Constructor from sec and nsec - * - * Constructor from sec and nsec - * - */ - TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec); - - /** \brief Destructor - * - * Destructor - * - */ - ~TimeStamp(); - - /** \brief Value of time stamp is valid - * - */ - static TimeStamp Invalid ( ); - bool ok ( ) const; - void setOk ( ); - void setNOk ( ); - - static TimeStamp Now(); - - /** \brief Time stamp to now - */ - void setToNow(); - - /** \brief Set time stamp - * - * Sets time stamp to a given value passed as a timeval struct - */ - void set(const timeval& ts); - - /** \brief Set time stamp - * - * Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds) - * - */ - void set(const unsigned long int& sec, const unsigned long int& nanosec); - - /** \brief Set time stamp - * - * Sets time stamp to a given value passed as a scalar_t (seconds) - * - */ - void set(const double& ts); - - /** \brief Get time stamp - * - * Returns time stamp - * - */ - double get() const; - - /** \brief Get time stamp (only seconds) - * - * Returns seconds of time stamp - * - */ - unsigned long int getSeconds() const; - - /** \brief Get time stamp (only nano seconds) - * - * Returns nanoseconds part of time stamp - * - */ - unsigned long int getNanoSeconds() const; - - /** \brief Assignement operator - * - * Assignement operator - * - */ - void operator =(const double& ts); - - /** \brief Assignement operator - * - * Assignement operator given a scalar_t (seconds) - * - */ - void operator =(const TimeStamp& ts); - - /** \brief comparison operator - * - * Comparison operator - * - */ - bool operator !=(const TimeStamp& ts) const; - bool operator ==(const TimeStamp& ts) const; - bool operator <(const TimeStamp& ts) const; - bool operator >(const TimeStamp& ts) const; - - /** \brief comparison operator - * - * Comparison operator - * - */ - bool operator <=(const TimeStamp& ts) const; - bool operator >=(const TimeStamp& ts) const; - - /** \brief Add-assign operator given a scalar_t (seconds) - */ - void operator +=(const double& dt); - - /** \brief Add-assign operator given a scalar_t (seconds) - */ - TimeStamp operator +(const double& dt) const; - - /** \brief Substraction-assign operator given a scalar_t (seconds) - */ - void operator -=(const double& dt); - - /** \brief difference operator - * - * difference operator that returns a scalar_t (seconds) - * - */ - TimeStamp operator -(const double& ts) const; - - /** \brief difference operator - * - * difference operator that returns a Timestamp (seconds) - * - */ - double operator -(const TimeStamp& ts) const; - - /** \brief Prints time stamp to a given ostream - * - * Prints time stamp to a given ostream - * - */ - void print(std::ostream & ost = std::cout) const; - - friend std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts); - + protected: + unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970. + bool is_valid_; // time stamp has a valid value + + public: + /** \brief Constructor + * + * Constructor without arguments. Sets time stamp to now. + * + */ + TimeStamp(); + + /** \brief Copy constructor + * + * Copy constructor + * + */ + TimeStamp(const TimeStamp& _ts); + + /** \brief Constructor with argument + * + * Constructor with arguments + * + */ + TimeStamp(const double& _ts); + + /** \brief Constructor from sec and nsec + * + * Constructor from sec and nsec + * + */ + TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec); + + /** \brief Destructor + * + * Destructor + * + */ + ~TimeStamp(); + + /** \brief Value of time stamp is valid + * + */ + static TimeStamp Invalid(); + bool ok() const; + void setOk(); + void setNOk(); + + static TimeStamp Now(); + + /** \brief Time stamp to now + */ + void setToNow(); + + /** \brief Set time stamp + * + * Sets time stamp to a given value passed as a timeval struct + */ + void set(const timeval& ts); + + /** \brief Set time stamp + * + * Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds) + * + */ + void set(const unsigned long int& sec, const unsigned long int& nanosec); + + /** \brief Set time stamp + * + * Sets time stamp to a given value passed as a scalar_t (seconds) + * + */ + void set(const double& ts); + + /** \brief Get time stamp + * + * Returns time stamp + * + */ + double get() const; + + /** \brief Get time stamp (only seconds) + * + * Returns seconds of time stamp + * + */ + unsigned long int getSeconds() const; + + /** \brief Get time stamp (only nano seconds) + * + * Returns nanoseconds part of time stamp + * + */ + unsigned long int getNanoSeconds() const; + + /** \brief Assignement operator + * + * Assignement operator + * + */ + void operator=(const double& ts); + + /** \brief Assignement operator + * + * Assignement operator given a scalar_t (seconds) + * + */ + void operator=(const TimeStamp& ts); + + /** \brief comparison operator + * + * Comparison operator + * + */ + bool operator!=(const TimeStamp& ts) const; + bool operator==(const TimeStamp& ts) const; + bool operator<(const TimeStamp& ts) const; + bool operator>(const TimeStamp& ts) const; + + /** \brief comparison operator + * + * Comparison operator + * + */ + bool operator<=(const TimeStamp& ts) const; + bool operator>=(const TimeStamp& ts) const; + + /** \brief Add-assign operator given a scalar_t (seconds) + */ + void operator+=(const double& dt); + + /** \brief Add-assign operator given a scalar_t (seconds) + */ + TimeStamp operator+(const double& dt) const; + + /** \brief Substraction-assign operator given a scalar_t (seconds) + */ + void operator-=(const double& dt); + + /** \brief difference operator + * + * difference operator that returns a scalar_t (seconds) + * + */ + TimeStamp operator-(const double& ts) const; + + /** \brief difference operator + * + * difference operator that returns a Timestamp (seconds) + * + */ + double operator-(const TimeStamp& ts) const; + + /** \brief Prints time stamp to a given ostream + * + * Prints time stamp to a given ostream + * + */ + void print(std::ostream& ost = std::cout) const; + + friend std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts); }; -inline wolf::TimeStamp TimeStamp::Invalid ( ) +inline wolf::TimeStamp TimeStamp::Invalid() { return TimeStamp(-1.0); } -inline bool TimeStamp::ok ( ) const +inline bool TimeStamp::ok() const { return is_valid_; } -inline void TimeStamp::setOk ( ) +inline void TimeStamp::setOk() { is_valid_ = true; } -inline void TimeStamp::setNOk ( ) +inline void TimeStamp::setNOk() { is_valid_ = false; } inline void TimeStamp::set(const double& ts) { - time_stamp_nano_ = (ts >= 0 ? (unsigned long int)(ts*NANOSECS) : 0); - is_valid_ = (ts >= 0); + time_stamp_nano_ = (ts >= 0 ? (unsigned long int)(ts * NANOSECS) : 0); + is_valid_ = (ts >= 0); } inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec) { - time_stamp_nano_ = sec*NANOSECS+nanosec; - is_valid_ = true; + time_stamp_nano_ = sec * NANOSECS + nanosec; + is_valid_ = true; } inline void TimeStamp::set(const timeval& ts) { - time_stamp_nano_ = (unsigned long int)(ts.tv_sec*NANOSECS) + (unsigned long int)(ts.tv_usec*1000); - is_valid_ = (ts.tv_sec >= 0 and ts.tv_usec >= 0); + time_stamp_nano_ = (unsigned long int)(ts.tv_sec * NANOSECS) + (unsigned long int)(ts.tv_usec * 1000); + is_valid_ = (ts.tv_sec >= 0 and ts.tv_usec >= 0); } inline double TimeStamp::get() const { - return ((double)( time_stamp_nano_))*1e-9; + return ((double)(time_stamp_nano_)) * 1e-9; } inline unsigned long int TimeStamp::getSeconds() const @@ -256,56 +255,57 @@ inline unsigned long int TimeStamp::getNanoSeconds() const return time_stamp_nano_ % NANOSECS; } -inline void TimeStamp::operator =(const TimeStamp& ts) +inline void TimeStamp::operator=(const TimeStamp& ts) { time_stamp_nano_ = ts.time_stamp_nano_; - is_valid_ = ts.is_valid_; + is_valid_ = ts.is_valid_; } -inline void TimeStamp::operator =(const double& ts) +inline void TimeStamp::operator=(const double& ts) { - time_stamp_nano_ = (unsigned long int)(ts*NANOSECS); - is_valid_ = (ts >= 0); + time_stamp_nano_ = (unsigned long int)(ts * NANOSECS); + is_valid_ = (ts >= 0); } -inline bool TimeStamp::operator ==(const TimeStamp& ts) const +inline bool TimeStamp::operator==(const TimeStamp& ts) const { return (time_stamp_nano_ == ts.time_stamp_nano_); } -inline bool TimeStamp::operator !=(const TimeStamp& ts) const +inline bool TimeStamp::operator!=(const TimeStamp& ts) const { return (time_stamp_nano_ != ts.time_stamp_nano_); } -inline bool TimeStamp::operator <(const TimeStamp& ts) const +inline bool TimeStamp::operator<(const TimeStamp& ts) const { return (time_stamp_nano_ < ts.time_stamp_nano_); } -inline bool TimeStamp::operator >(const TimeStamp& ts) const +inline bool TimeStamp::operator>(const TimeStamp& ts) const { return (time_stamp_nano_ > ts.time_stamp_nano_); } -inline bool TimeStamp::operator <=(const TimeStamp& ts) const +inline bool TimeStamp::operator<=(const TimeStamp& ts) const { return (time_stamp_nano_ <= ts.time_stamp_nano_); } -inline bool TimeStamp::operator >=(const TimeStamp& ts) const +inline bool TimeStamp::operator>=(const TimeStamp& ts) const { return (time_stamp_nano_ >= ts.time_stamp_nano_); } -inline void TimeStamp::operator +=(const double& dt) +inline void TimeStamp::operator+=(const double& dt) { - time_stamp_nano_ += (unsigned long int)(dt*NANOSECS); + time_stamp_nano_ += (unsigned long int)(dt * NANOSECS); } -inline double TimeStamp::operator -(const TimeStamp& ts) const +inline double TimeStamp::operator-(const TimeStamp& ts) const { - return double((long int)(time_stamp_nano_ - ts.time_stamp_nano_))*1e-9; // long int cast fix overflow in case of negative substraction result + return double((long int)(time_stamp_nano_ - ts.time_stamp_nano_)) * + 1e-9; // long int cast fix overflow in case of negative substraction result } -} // namespace wolf +} // namespace wolf diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h index f8fee3de0feffd1c1315090bd7344ea18176f1d8..9be1c0426bb8ef0c018ef9f3f5c8b9872879a5b5 100644 --- a/include/core/common/wolf.h +++ b/include/core/common/wolf.h @@ -27,17 +27,17 @@ // Folder Registry #include "core/utils/folder_registry.h" -//includes from Eigen lib +// includes from Eigen lib #include <Eigen/Dense> #include <Eigen/Geometry> #include <Eigen/Sparse> #include <libgen.h> -//includes from std lib +// includes from std lib #include <list> #include <map> -#include <memory> // shared_ptr and weak_ptr +#include <memory> // shared_ptr and weak_ptr #include <set> // System specifics @@ -47,8 +47,8 @@ #include "yaml-cpp/yaml.h" #include "yaml-schema-cpp/yaml_conversion.hpp" -namespace wolf { - +namespace wolf +{ /** * \brief Vector and Matrices size type for the Wolf project * @@ -68,15 +68,15 @@ typedef std::size_t SizeStd; #define M_TORAD 0.017453292519943295769236907684886127134 // pi / 180 #define M_TODEG 57.295779513082320876798154814105170332 // 180 / pi -namespace Constants{ - +namespace Constants +{ // Wolf standard tolerance const double EPS = 1e-8; // Wolf smmmmall tolerance const double EPS_SMALL = 1e-16; -} +} // namespace Constants -} // namespace wolf +} // namespace wolf /////////////////////////////////////////// // Construct types for any scalar defined in the typedef double above @@ -93,30 +93,30 @@ const double EPS_SMALL = 1e-16; namespace Eigen // Eigen namespace extension { // 1. Vectors and Matrices -typedef Matrix<double, 1, 1> Matrix1d; ///< 1x1 matrix of real double type -typedef Matrix<double, 5, 5> Matrix5d; ///< 5x5 matrix of real double type -typedef Matrix<double, 6, 6> Matrix6d; ///< 6x6 matrix of real double type -typedef Matrix<double, 7, 7> Matrix7d; ///< 7x7 matrix of real double type -typedef Matrix<double, 8, 8> Matrix8d; ///< 8x8 matrix of real double type -typedef Matrix<double, 9, 9> Matrix9d; ///< 9x9 matrix of real double type -typedef Matrix<double, 10, 10> Matrix10d; ///< 10x10 matrix of real double type -typedef Matrix<double, 1, 1> Vector1d; ///< 1-vector of real double type -typedef Matrix<double, 5, 1> Vector5d; ///< 5-vector of real double type -typedef Matrix<double, 6, 1> Vector6d; ///< 6-vector of real double type -typedef Matrix<double, 7, 1> Vector7d; ///< 7-vector of real double type -typedef Matrix<double, 8, 1> Vector8d; ///< 8-vector of real double type -typedef Matrix<double, 9, 1> Vector9d; ///< 9-vector of real double type -typedef Matrix<double, 10, 1> Vector10d; ///< 10-vector of real double type +typedef Matrix<double, 1, 1> Matrix1d; ///< 1x1 matrix of real double type +typedef Matrix<double, 5, 5> Matrix5d; ///< 5x5 matrix of real double type +typedef Matrix<double, 6, 6> Matrix6d; ///< 6x6 matrix of real double type +typedef Matrix<double, 7, 7> Matrix7d; ///< 7x7 matrix of real double type +typedef Matrix<double, 8, 8> Matrix8d; ///< 8x8 matrix of real double type +typedef Matrix<double, 9, 9> Matrix9d; ///< 9x9 matrix of real double type +typedef Matrix<double, 10, 10> Matrix10d; ///< 10x10 matrix of real double type +typedef Matrix<double, 1, 1> Vector1d; ///< 1-vector of real double type +typedef Matrix<double, 5, 1> Vector5d; ///< 5-vector of real double type +typedef Matrix<double, 6, 1> Vector6d; ///< 6-vector of real double type +typedef Matrix<double, 7, 1> Vector7d; ///< 7-vector of real double type +typedef Matrix<double, 8, 1> Vector8d; ///< 8-vector of real double type +typedef Matrix<double, 9, 1> Vector9d; ///< 9-vector of real double type +typedef Matrix<double, 10, 1> Vector10d; ///< 10-vector of real double type // 2. Sparse matrix typedef SparseMatrix<double> SparseMatrixd; // 3. Row major matrix -typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmajor matrix of real double type -} - -namespace wolf { +typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixRowXd; ///< dynamic rowmajor matrix of real double type +} // namespace Eigen +namespace wolf +{ ////////////////////////////////////////////////////////// /** Check Eigen Matrix sizes, both statically and dynamically * @@ -125,7 +125,8 @@ namespace wolf { * The WOLF project implements many template functions using Eigen Matrix and Quaterniond, in different versions * (Static size, Dynamic size, Map, Matrix expression). * - * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h): + * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's + * StaticAssert.h): * * EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE * EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE @@ -133,7 +134,8 @@ namespace wolf { * * but they do not work if the evaluated types are of dynamic size. * - * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind: + * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this + * kind: * * template<typename Derived> * inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){ @@ -150,43 +152,43 @@ namespace wolf { * The function : MatrixSizeCheck <Rows, Cols>::check(M) checks that the Matrix M is of size ( Rows x Cols ). * This check is performed statically or dynamically, depending on the type of argument provided. */ -template<int Size, int DesiredSize> +template <int Size, int DesiredSize> struct StaticSizeCheck { - template<typename T> - StaticSizeCheck(const T&) - { - static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match"); - } + template <typename T> + StaticSizeCheck(const T&) + { + static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match"); + } }; // -template<int DesiredSize> +template <int DesiredSize> struct StaticSizeCheck<Eigen::Dynamic, DesiredSize> { - template<typename T> - StaticSizeCheck(const T& t) - { - assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match"); - } + template <typename T> + StaticSizeCheck(const T& t) + { + assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match"); + } }; // -template<int DesiredR, int DesiredC> +template <int DesiredR, int DesiredC> struct MatrixSizeCheck { - /** \brief Assert matrix size dynamically or statically - * - * @param t the variable for which we wish to assert the size. - * - * Usage: to assert that t is size (Rows x Cols) - * - * MatrixSizeCheck<Rows, Cols>::check(t); - */ - template<typename T> - static void check(const T& t) - { - StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows()); - StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols()); - } + /** \brief Assert matrix size dynamically or statically + * + * @param t the variable for which we wish to assert the size. + * + * Usage: to assert that t is size (Rows x Cols) + * + * MatrixSizeCheck<Rows, Cols>::check(t); + */ + template <typename T> + static void check(const T& t) + { + StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows()); + StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols()); + } }; // @@ -196,58 +198,57 @@ struct MatrixSizeCheck // TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE ///////////////////////////////////////////////////////////////////////// -#define WOLF_DECLARED_PTR_TYPEDEFS(Name) \ - typedef std::shared_ptr<Name> Name##Ptr; \ - typedef std::shared_ptr<const Name> Name##ConstPtr; \ - typedef std::weak_ptr<Name> Name##WPtr; \ - typedef std::weak_ptr<const Name> Name##ConstWPtr; \ - -#define WOLF_PTR_TYPEDEFS(ClassName) \ - class ClassName; \ - WOLF_DECLARED_PTR_TYPEDEFS(ClassName); \ - -#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \ - struct StructName; \ - WOLF_DECLARED_PTR_TYPEDEFS(StructName); \ - -#define WOLF_LIST_TYPEDEFS(ClassName) \ - class ClassName; \ - typedef std::list<ClassName##Ptr> ClassName##PtrList; \ - typedef ClassName##PtrList::iterator ClassName##PtrListIter; \ - typedef ClassName##PtrList::const_iterator ClassName##PtrListConstIter; \ - typedef ClassName##PtrList::reverse_iterator ClassName##PtrListRevIter; \ - typedef std::list<ClassName##WPtr> ClassName##WPtrList; \ - typedef ClassName##WPtrList::iterator ClassName##WPtrListIter; \ - typedef ClassName##WPtrList::const_iterator ClassName##WPtrListConstIter; \ - typedef ClassName##WPtrList::reverse_iterator ClassName##WPtrListRevIter; \ - typedef std::list<ClassName##ConstPtr> ClassName##ConstPtrList; \ - typedef ClassName##ConstPtrList::iterator ClassName##ConstPtrListIter; \ - typedef ClassName##ConstPtrList::const_iterator ClassName##ConstPtrListConstIter; \ - typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstPtrListRevIter; \ - typedef std::list<ClassName##ConstWPtr> ClassName##ConstWPtrList; \ - typedef ClassName##ConstWPtrList::iterator ClassName##ConstWPtrListIter; \ - typedef ClassName##ConstWPtrList::const_iterator ClassName##ConstWPtrListConstIter; \ - typedef ClassName##ConstWPtrList::reverse_iterator ClassName##ConstWPtrListRevIter; \ - -#define WOLF_SET_TYPEDEFS(ClassName) \ - class ClassName; \ - typedef std::set<ClassName##Ptr> ClassName##PtrSet; \ - typedef ClassName##PtrSet::iterator ClassName##PtrSetIter; \ - typedef ClassName##PtrSet::const_iterator ClassName##PtrSetConstIter; \ - typedef ClassName##PtrSet::reverse_iterator ClassName##PtrSetRevIter; \ - typedef std::set<ClassName##WPtr> ClassName##WPtrSet; \ - typedef ClassName##WPtrSet::iterator ClassName##WPtrSetIter; \ - typedef ClassName##WPtrSet::const_iterator ClassName##WPtrSetConstIter; \ - typedef ClassName##WPtrSet::reverse_iterator ClassName##WPtrSetRevIter; \ - typedef std::set<ClassName##ConstPtr> ClassName##ConstPtrSet; \ - typedef ClassName##ConstPtrSet::iterator ClassName##ConstPtrSetIter; \ - typedef ClassName##ConstPtrSet::const_iterator ClassName##ConstPtrSetConstIter; \ - typedef ClassName##ConstPtrSet::reverse_iterator ClassName##ConstPtrSetRevIter; \ - typedef std::set<ClassName##ConstWPtr> ClassName##ConstWPtrSet; \ - typedef ClassName##ConstWPtrSet::iterator ClassName##ConstWPtrSetIter; \ - typedef ClassName##ConstWPtrSet::const_iterator ClassName##ConstWPtrSetConstIter; \ - typedef ClassName##ConstWPtrSet::reverse_iterator ClassName##ConstWPtrSetRevIter; \ - +#define WOLF_DECLARED_PTR_TYPEDEFS(Name) \ + typedef std::shared_ptr<Name> Name##Ptr; \ + typedef std::shared_ptr<const Name> Name##ConstPtr; \ + typedef std::weak_ptr<Name> Name##WPtr; \ + typedef std::weak_ptr<const Name> Name##ConstWPtr; + +#define WOLF_PTR_TYPEDEFS(ClassName) \ + class ClassName; \ + WOLF_DECLARED_PTR_TYPEDEFS(ClassName); + +#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \ + struct StructName; \ + WOLF_DECLARED_PTR_TYPEDEFS(StructName); + +#define WOLF_LIST_TYPEDEFS(ClassName) \ + class ClassName; \ + typedef std::list<ClassName##Ptr> ClassName##PtrList; \ + typedef ClassName##PtrList::iterator ClassName##PtrListIter; \ + typedef ClassName##PtrList::const_iterator ClassName##PtrListConstIter; \ + typedef ClassName##PtrList::reverse_iterator ClassName##PtrListRevIter; \ + typedef std::list<ClassName##WPtr> ClassName##WPtrList; \ + typedef ClassName##WPtrList::iterator ClassName##WPtrListIter; \ + typedef ClassName##WPtrList::const_iterator ClassName##WPtrListConstIter; \ + typedef ClassName##WPtrList::reverse_iterator ClassName##WPtrListRevIter; \ + typedef std::list<ClassName##ConstPtr> ClassName##ConstPtrList; \ + typedef ClassName##ConstPtrList::iterator ClassName##ConstPtrListIter; \ + typedef ClassName##ConstPtrList::const_iterator ClassName##ConstPtrListConstIter; \ + typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstPtrListRevIter; \ + typedef std::list<ClassName##ConstWPtr> ClassName##ConstWPtrList; \ + typedef ClassName##ConstWPtrList::iterator ClassName##ConstWPtrListIter; \ + typedef ClassName##ConstWPtrList::const_iterator ClassName##ConstWPtrListConstIter; \ + typedef ClassName##ConstWPtrList::reverse_iterator ClassName##ConstWPtrListRevIter; + +#define WOLF_SET_TYPEDEFS(ClassName) \ + class ClassName; \ + typedef std::set<ClassName##Ptr> ClassName##PtrSet; \ + typedef ClassName##PtrSet::iterator ClassName##PtrSetIter; \ + typedef ClassName##PtrSet::const_iterator ClassName##PtrSetConstIter; \ + typedef ClassName##PtrSet::reverse_iterator ClassName##PtrSetRevIter; \ + typedef std::set<ClassName##WPtr> ClassName##WPtrSet; \ + typedef ClassName##WPtrSet::iterator ClassName##WPtrSetIter; \ + typedef ClassName##WPtrSet::const_iterator ClassName##WPtrSetConstIter; \ + typedef ClassName##WPtrSet::reverse_iterator ClassName##WPtrSetRevIter; \ + typedef std::set<ClassName##ConstPtr> ClassName##ConstPtrSet; \ + typedef ClassName##ConstPtrSet::iterator ClassName##ConstPtrSetIter; \ + typedef ClassName##ConstPtrSet::const_iterator ClassName##ConstPtrSetConstIter; \ + typedef ClassName##ConstPtrSet::reverse_iterator ClassName##ConstPtrSetRevIter; \ + typedef std::set<ClassName##ConstWPtr> ClassName##ConstWPtrSet; \ + typedef ClassName##ConstWPtrSet::iterator ClassName##ConstWPtrSetIter; \ + typedef ClassName##ConstWPtrSet::const_iterator ClassName##ConstWPtrSetConstIter; \ + typedef ClassName##ConstWPtrSet::reverse_iterator ClassName##ConstWPtrSetRevIter; // NodeBase WOLF_PTR_TYPEDEFS(NodeBase); @@ -291,7 +292,7 @@ WOLF_PTR_TYPEDEFS(FrameBase); WOLF_LIST_TYPEDEFS(FrameBase); class TimeStamp; -typedef std::map<TimeStamp, FrameBasePtr> FramePtrMap; +typedef std::map<TimeStamp, FrameBasePtr> FramePtrMap; typedef std::map<TimeStamp, FrameBaseConstPtr> FrameConstPtrMap; // - Capture @@ -332,15 +333,16 @@ WOLF_PTR_TYPEDEFS(LocalParametrizationBase); inline bool file_exists(const std::string& _name) { struct stat buffer; - return (stat (_name.c_str(), &buffer) == 0); + return (stat(_name.c_str(), &buffer) == 0); } -inline const Eigen::Vector3d gravity(void) { - return Eigen::Vector3d(0,0,-9.806); +inline const Eigen::Vector3d gravity(void) +{ + return Eigen::Vector3d(0, 0, -9.806); } // =================================================== // Schema folder registry WOLF_REGISTER_FOLDER(_WOLF_SCHEMA_DIR); -} // namespace wolf +} // namespace wolf diff --git a/include/core/composite/matrix_composite.h b/include/core/composite/matrix_composite.h index 3ae0fec997ab17a949e905a0cfaf34fe089b49fa..92ddbca72eaf1145f9a2699da76a89c39f136b3e 100644 --- a/include/core/composite/matrix_composite.h +++ b/include/core/composite/matrix_composite.h @@ -28,191 +28,178 @@ namespace wolf { - -class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > > +class MatrixComposite : public std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> > { - - private: - std::unordered_map < char, unsigned int> size_rows_, size_cols_; - unsigned int rows() const; - unsigned int cols() const; - void sizeAndIndices(const StateKeys & _struct_rows, - const StateKeys & _struct_cols, - std::unordered_map < char, unsigned int>& _indices_rows, - std::unordered_map < char, unsigned int>& _indices_cols, - unsigned int& _rows, - unsigned int& _cols) const; - - public: - MatrixComposite() {}; - MatrixComposite(const StateKeys& _row_structure, - const StateKeys& _col_structure); - MatrixComposite(const StateKeys& _row_structure, - const std::list<int>& _row_sizes, - const StateKeys& _col_structure, - const std::list<int>& _col_sizes); - - /** - * \brief Construct from Eigen::VectorXd and structure - * - * Usage: - * - * VectorXd vec_eigen(1,2,3,4,5); - * - * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !! - * - * result: - * - * vec_comp["a"].transpose(); // = (1,2); - * vec_comp["b"].transpose(); // = (3,4,5); - */ - MatrixComposite(const MatrixXd& _m, - const StateKeys& _row_structure, - const std::list<int>& _row_sizes, - const StateKeys& _col_structure, - const std::list<int>& _col_sizes); - ~MatrixComposite() = default; - - bool check() const; - static MatrixComposite zero(const StateKeys& _row_structure, - const std::list<int>& _row_sizes, - const StateKeys& _col_structure, - const std::list<int>& _col_sizes); - - static MatrixComposite identity(const StateKeys& _structure, - const std::list<int>& _sizes); - - unsigned int count(const char &_row, - const char &_col) const; - - bool emplace(const char &_row, - const char &_col, - const Eigen::MatrixXd &_mat_blk); - - // throw error if queried block is not present - bool at(const char &_row, - const char &_col, - Eigen::MatrixXd &_mat_blk) const; - const MatrixXd& at(const char &_row, - const char &_col) const; - MatrixXd& at(const char &_row, - const char &_col); - - // make some shadowed API available - using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::at; - using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count; - - - MatrixXd matrix(const StateKeys & _struct_rows, - const StateKeys & _struct_cols) const; - - MatrixComposite operator * (double scalar_right) const; - MatrixComposite operator + (const MatrixComposite & _second) const; - MatrixComposite operator - (const MatrixComposite & _second) const; - MatrixComposite operator - (void) const; - - /** - * \brief Matrix product - * - * This function computes the matrix product M * N - * - * It assumes that the second matrix's blocks are compatible with the blocks of this matrix, - * both in their structure and their individual sizes. - * - * For example: Let us call 'this' matrix with the name 'M'. - * - * The matrix 'M' maps the "PO" space into a new space "VW": - * M["V"]["P"] M["V"]["O"] - * M["W"]["P"] M["W"]["O"] - * - * The second matrix N has blocks "PO" in vertical arrangement, - * and "XY" in horizontal arrangement: - * N["P"]["X"] N["P"]["Y"] - * N["O"]["X"] N["O"]["Y"] - * - * Also, assume that the sizes of the blocks "P" and "O" are the same in M and N. - * - * Then, the result is a matrix S = M * N with block arrangements "VW" and "XY" - * S["V"]["X"] S["V"]["Y"] - * S["W"]["X"] S["W"]["Y"] - */ - MatrixComposite operator *(const MatrixComposite & _second) const; - - /** - * \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix - * - * This function computes the product J * Q * J.transpose. - * - * It considers that this is a Jacobian matrix, and that the provided matrix - * is a proper covariance (e.g. symmmetric and semi-positive). - * It also considers that the Jacobian blocks are compatible with the blocks - * of the provided covariance, both in their structure and their individual sizes. - * - * If these conditions are not satisfied, the product will fail and throw, - * or give aberrant results at best. - * - * ----- - * - * For example: Let us call 'this' a Jacobian matrix with the name 'J'. - * - * This Jacobian 'J' maps the "PO" blocks into "VW": - * - * J["V"]["P"] J["V"]["O"] - * J["W"]["P"] J["W"]["O"] - * - * The provided matrix is a covariances matrix Q. - * Q has blocks "P", "O" in both vertical and horizontal arrangements: - * - * Q["P"]["P"] Q["P"]["O"] - * Q["O"]["P"] Q["O"]["O"] - * - * Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J. - * - * Now, upon a call to - * - * MatrixComposite S = J.propagate(Q); - * - * the result is a covariances matrix S with blocks "V" and "W" - * - * S["V"]["V"] S["V"]["W"] - * S["W"]["V"] S["W"]["W"] - */ - MatrixComposite propagate(const MatrixComposite & _Cov); // This performs this * _Cov * this.tr - - /** - * \brief Matrix-vector product - * - * This function computes the matrix product M * N - * - * It assumes that the second matrix's blocks are compatible with the blocks of this matrix, - * both in their structure and their individual sizes. - * - * For example: Let us call 'this' matrix with the name 'M'. - * - * The matrix 'M' maps the "PO" space into a new space "VW": - * M["V"]["P"] M["V"]["O"] - * M["W"]["P"] M["W"]["O"] - * - * The second vector V has blocks "PO" in vertical arrangement, - * V["P"] - * V["O"] - * - * Also, assume that the sizes of the blocks "P" and "O" are the same in M and V. - * - * Then, the result is a vector S = M * V with block arrangement "VW" - * S["V"] - * S["W"] - */ - VectorComposite operator *(const VectorComposite & _second) const; - - friend std::ostream& operator << (std::ostream & _os, const MatrixComposite & _M); - - friend MatrixComposite operator * (double scalar_left, const MatrixComposite& M); + private: + std::unordered_map<char, unsigned int> size_rows_, size_cols_; + unsigned int rows() const; + unsigned int cols() const; + void sizeAndIndices(const StateKeys& _struct_rows, + const StateKeys& _struct_cols, + std::unordered_map<char, unsigned int>& _indices_rows, + std::unordered_map<char, unsigned int>& _indices_cols, + unsigned int& _rows, + unsigned int& _cols) const; + + public: + MatrixComposite(){}; + MatrixComposite(const StateKeys& _row_structure, const StateKeys& _col_structure); + MatrixComposite(const StateKeys& _row_structure, + const std::list<int>& _row_sizes, + const StateKeys& _col_structure, + const std::list<int>& _col_sizes); + + /** + * \brief Construct from Eigen::VectorXd and structure + * + * Usage: + * + * VectorXd vec_eigen(1,2,3,4,5); + * + * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !! + * + * result: + * + * vec_comp["a"].transpose(); // = (1,2); + * vec_comp["b"].transpose(); // = (3,4,5); + */ + MatrixComposite(const MatrixXd& _m, + const StateKeys& _row_structure, + const std::list<int>& _row_sizes, + const StateKeys& _col_structure, + const std::list<int>& _col_sizes); + ~MatrixComposite() = default; + + bool check() const; + static MatrixComposite zero(const StateKeys& _row_structure, + const std::list<int>& _row_sizes, + const StateKeys& _col_structure, + const std::list<int>& _col_sizes); + + static MatrixComposite identity(const StateKeys& _structure, const std::list<int>& _sizes); + + unsigned int count(const char& _row, const char& _col) const; + + bool emplace(const char& _row, const char& _col, const Eigen::MatrixXd& _mat_blk); + + // throw error if queried block is not present + bool at(const char& _row, const char& _col, Eigen::MatrixXd& _mat_blk) const; + const MatrixXd& at(const char& _row, const char& _col) const; + MatrixXd& at(const char& _row, const char& _col); + + // make some shadowed API available + using std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> >::at; + using std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> >::count; + + MatrixXd matrix(const StateKeys& _struct_rows, const StateKeys& _struct_cols) const; + + MatrixComposite operator*(double scalar_right) const; + MatrixComposite operator+(const MatrixComposite& _second) const; + MatrixComposite operator-(const MatrixComposite& _second) const; + MatrixComposite operator-(void) const; + + /** + * \brief Matrix product + * + * This function computes the matrix product M * N + * + * It assumes that the second matrix's blocks are compatible with the blocks of this matrix, + * both in their structure and their individual sizes. + * + * For example: Let us call 'this' matrix with the name 'M'. + * + * The matrix 'M' maps the "PO" space into a new space "VW": + * M["V"]["P"] M["V"]["O"] + * M["W"]["P"] M["W"]["O"] + * + * The second matrix N has blocks "PO" in vertical arrangement, + * and "XY" in horizontal arrangement: + * N["P"]["X"] N["P"]["Y"] + * N["O"]["X"] N["O"]["Y"] + * + * Also, assume that the sizes of the blocks "P" and "O" are the same in M and N. + * + * Then, the result is a matrix S = M * N with block arrangements "VW" and "XY" + * S["V"]["X"] S["V"]["Y"] + * S["W"]["X"] S["W"]["Y"] + */ + MatrixComposite operator*(const MatrixComposite& _second) const; + + /** + * \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix + * + * This function computes the product J * Q * J.transpose. + * + * It considers that this is a Jacobian matrix, and that the provided matrix + * is a proper covariance (e.g. symmmetric and semi-positive). + * It also considers that the Jacobian blocks are compatible with the blocks + * of the provided covariance, both in their structure and their individual sizes. + * + * If these conditions are not satisfied, the product will fail and throw, + * or give aberrant results at best. + * + * ----- + * + * For example: Let us call 'this' a Jacobian matrix with the name 'J'. + * + * This Jacobian 'J' maps the "PO" blocks into "VW": + * + * J["V"]["P"] J["V"]["O"] + * J["W"]["P"] J["W"]["O"] + * + * The provided matrix is a covariances matrix Q. + * Q has blocks "P", "O" in both vertical and horizontal arrangements: + * + * Q["P"]["P"] Q["P"]["O"] + * Q["O"]["P"] Q["O"]["O"] + * + * Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J. + * + * Now, upon a call to + * + * MatrixComposite S = J.propagate(Q); + * + * the result is a covariances matrix S with blocks "V" and "W" + * + * S["V"]["V"] S["V"]["W"] + * S["W"]["V"] S["W"]["W"] + */ + MatrixComposite propagate(const MatrixComposite& _Cov); // This performs this * _Cov * this.tr + + /** + * \brief Matrix-vector product + * + * This function computes the matrix product M * N + * + * It assumes that the second matrix's blocks are compatible with the blocks of this matrix, + * both in their structure and their individual sizes. + * + * For example: Let us call 'this' matrix with the name 'M'. + * + * The matrix 'M' maps the "PO" space into a new space "VW": + * M["V"]["P"] M["V"]["O"] + * M["W"]["P"] M["W"]["O"] + * + * The second vector V has blocks "PO" in vertical arrangement, + * V["P"] + * V["O"] + * + * Also, assume that the sizes of the blocks "P" and "O" are the same in M and V. + * + * Then, the result is a vector S = M * V with block arrangement "VW" + * S["V"] + * S["W"] + */ + VectorComposite operator*(const VectorComposite& _second) const; + + friend std::ostream& operator<<(std::ostream& _os, const MatrixComposite& _M); + + friend MatrixComposite operator*(double scalar_left, const MatrixComposite& M); }; //////////// IMPLEMENTATION //////////// -inline unsigned int MatrixComposite::count(const char &_row, const char &_col) const +inline unsigned int MatrixComposite::count(const char& _row, const char& _col) const { const auto& rit = this->find(_row); @@ -223,4 +210,4 @@ inline unsigned int MatrixComposite::count(const char &_row, const char &_col) c return rit->second.count(_col); } -} +} // namespace wolf diff --git a/include/core/composite/spec_state_composite.h b/include/core/composite/spec_state_composite.h index 3f0e2139cc29bfc755c760974c0cb36511d247df..6ba838167df5d113a889b3e3ab4db030c29a8281 100644 --- a/include/core/composite/spec_state_composite.h +++ b/include/core/composite/spec_state_composite.h @@ -171,7 +171,7 @@ inline bool SpecState::isFactor() const // CONVERSION TO YAML namespace YAML { -template<> +template <> struct convert<wolf::SpecState> { static Node encode(const wolf::SpecState& spec) diff --git a/include/core/composite/spec_state_sensor_composite.h b/include/core/composite/spec_state_sensor_composite.h index 1821e727313378461662bbe7940fe01463539742..351d2999010e94588faab2e87b5c80bf9c1fe70b 100644 --- a/include/core/composite/spec_state_sensor_composite.h +++ b/include/core/composite/spec_state_sensor_composite.h @@ -49,7 +49,7 @@ class SpecStateSensor : public SpecState void check() const override; - std::string print(const std::string& _spaces = "") const override; + std::string print(const std::string& _spaces = "") const override; friend std::ostream& operator<<(std::ostream& _os, const wolf::SpecStateSensor& _x); YAML::Node toYaml() const; diff --git a/include/core/composite/type_composite.h b/include/core/composite/type_composite.h index 848ca52e1239cec96c97619ac82de22dd5cffdaa..f46734ad8248191b2c708fcc8ca4a467b33b518d 100644 --- a/include/core/composite/type_composite.h +++ b/include/core/composite/type_composite.h @@ -25,10 +25,9 @@ namespace wolf { - typedef Composite<std::string> TypeComposite; -void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim = 0); -bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0); +void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim = 0); +bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0); // template <> // inline Composite<std::string>::Composite(const YAML::Node& _n) @@ -51,7 +50,7 @@ bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0); // for (auto&& pair : *this) // { // YAML::Node n_type; -// n_type["type"] = pair.second; +// n_type["type"] = pair.second; // n[pair.first] = n_type; // } @@ -64,7 +63,7 @@ inline bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim) { checkTypeComposite(_types, _dim); } - catch(const std::exception& e) + catch (const std::exception& e) { return false; } @@ -120,7 +119,9 @@ inline void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim) } else { - throw std::runtime_error("checkTypeComposite(_types, _dim) bad value for '_dim', should be 2 for 2D, 3 for 3D, or 0 for not defined"); + throw std::runtime_error( + "checkTypeComposite(_types, _dim) bad value for '_dim', should be 2 for 2D, 3 for 3D, or 0 for not " + "defined"); } } diff --git a/include/core/composite/vector_composite.h b/include/core/composite/vector_composite.h index d632d5bccd0e5e496c091588b58f360a12e02da7..84ae2785801172af3e6471015c65699b131c343c 100644 --- a/include/core/composite/vector_composite.h +++ b/include/core/composite/vector_composite.h @@ -30,54 +30,53 @@ namespace wolf { - using namespace Eigen; class VectorComposite : public Composite<Eigen::VectorXd> { - public: - using Composite<Eigen::VectorXd>::Composite; - - /** - * \brief Construct from Eigen::VectorXd and keys - * - * Usage: - * - * VectorXd vec_eigen(1,2,3,4,5); - * - * VectorComposite vec_comp("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !! - * - * result: - * - * vec_comp["a"].transpose(); // = (1,2); - * vec_comp["b"].transpose(); // = (3,4,5); - */ - VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes); - VectorComposite(const StateKeys& _keys, const std::list<VectorXd>& _vectors); + public: + using Composite<Eigen::VectorXd>::Composite; + + /** + * \brief Construct from Eigen::VectorXd and keys + * + * Usage: + * + * VectorXd vec_eigen(1,2,3,4,5); + * + * VectorComposite vec_comp("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !! + * + * result: + * + * vec_comp["a"].transpose(); // = (1,2); + * vec_comp["b"].transpose(); // = (3,4,5); + */ + VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes); + VectorComposite(const StateKeys& _keys, const std::list<VectorXd>& _vectors); + + Eigen::VectorXd vector(const StateKeys& _keys) const; - Eigen::VectorXd vector(const StateKeys& _keys) const; + /** + * \brief set from Eigen::VectorXd and keys + * + * Usage: + * + * Eigen::VectorXd vec_eigen; + * wolf::VectorComposite vec_comp; + * + * vec_comp.set("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !! + * + * result: + * + * vec_comp["a"].transpose(); // = (1,2); + * vec_comp["b"].transpose(); // = (3,4,5); + */ + void set(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes); + void set(const StateKeys& _keys, const std::list<VectorXd>& _vectors); - /** - * \brief set from Eigen::VectorXd and keys - * - * Usage: - * - * Eigen::VectorXd vec_eigen; - * wolf::VectorComposite vec_comp; - * - * vec_comp.set("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !! - * - * result: - * - * vec_comp["a"].transpose(); // = (1,2); - * vec_comp["b"].transpose(); // = (3,4,5); - */ - void set(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes); - void set(const StateKeys& _keys, const std::list<VectorXd>& _vectors); - - void setZero(); + void setZero(); - friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x); + friend std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x); }; -} +} // namespace wolf diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h index 68fcf9f170bc084bb1f35083e829b7f1e14d5cef..45412f5cc77fe67fd11df1707146968e850ff557 100644 --- a/include/core/factor/factor_autodiff.h +++ b/include/core/factor/factor_autodiff.h @@ -36,15 +36,15 @@ namespace wolf template <class FacT, unsigned int RES, unsigned int B0, - unsigned int B1 = 0, - unsigned int B2 = 0, - unsigned int B3 = 0, - unsigned int B4 = 0, - unsigned int B5 = 0, - unsigned int B6 = 0, - unsigned int B7 = 0, - unsigned int B8 = 0, - unsigned int B9 = 0, + unsigned int B1 = 0, + unsigned int B2 = 0, + unsigned int B3 = 0, + unsigned int B4 = 0, + unsigned int B5 = 0, + unsigned int B6 = 0, + unsigned int B7 = 0, + unsigned int B8 = 0, + unsigned int B9 = 0, unsigned int B10 = 0, unsigned int B11 = 0, unsigned int B12 = 0, @@ -85,7 +85,7 @@ class FactorAutodiff : public FactorBase const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -105,7 +105,8 @@ class FactorAutodiff : public FactorBase for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } // initialize jets @@ -335,7 +336,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11 const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -355,7 +356,8 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11 for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } // initialize jets @@ -561,7 +563,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11 const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -581,9 +583,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11 for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -780,7 +783,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11 const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -800,9 +803,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11 for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -992,7 +996,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0, const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -1012,9 +1016,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0, for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -1197,7 +1202,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0, const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -1217,9 +1222,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0, for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -1395,7 +1401,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0, const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -1415,9 +1421,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0, for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -1586,7 +1593,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0 const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -1606,9 +1613,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0 for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -1762,7 +1770,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0, const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -1782,9 +1790,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0, for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -1932,7 +1941,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0, const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -1952,9 +1961,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0, for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -2096,7 +2106,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -2116,9 +2126,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -2244,7 +2255,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -2264,9 +2275,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -2385,7 +2397,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -2405,9 +2417,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -2521,7 +2534,7 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> : const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -2541,9 +2554,10 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> : for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); @@ -2654,7 +2668,7 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> : const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + FactorStatus _status = FAC_ACTIVE) : FactorBase(_type, _top, JAC_AUTO, @@ -2674,9 +2688,10 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> : for (auto i = 0; i < n_blocks; i++) { assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str()); - assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); + assert(state_block_sizes_.at(i) == sizes_template.at(i) && + ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str()); } - + // initialize jets unsigned int last_jet_idx = 0; for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h index 11ba8567be861415958afe98d95dde8e42487d5e..af51ee8db33170c8b72b943ecd06b4fae523532b 100644 --- a/include/core/factor/factor_base.h +++ b/include/core/factor/factor_base.h @@ -277,7 +277,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa bool _metric, bool _state_blocks, std::ostream& _stream = std::cout, - std::string _tabs = "") const; + std::string _tabs = "") const; virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const; bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const; diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h index aa3af2b3db9bfe6ede5611bba0882da5369921b0..afc9299a97e51845639505a620069a64ddb96a53 100644 --- a/include/core/factor/factor_block_absolute.h +++ b/include/core/factor/factor_block_absolute.h @@ -31,7 +31,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorBlockAbsolute); // class @@ -91,8 +90,7 @@ class FactorBlockAbsolute : public FactorAnalytic _node_state_blocks->getStateBlock(_key)->getSize()); assert(sb_constrained_size_ + sb_constrained_start_ <= sb_size_); assert(sb_constrained_size_ > 0); - assert(_measurement.size() == sb_constrained_size_ and - "FactorBlockAbsolute: _measurement wrong size"); + assert(_measurement.size() == sb_constrained_size_ and "FactorBlockAbsolute: _measurement wrong size"); assert(_measurement_sqrt_information_upper.rows() == sb_constrained_size_ and _measurement_sqrt_information_upper.cols() == sb_constrained_size_ and "FactorBlockAbsolute: _measurement_sqrt_information_upper wrong size"); diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h index 809c99612eb3bfba4c48b072ebef731453ba2f0a..b5087cb17f4f0321e9608a581b5faddd4a4c8eb8 100644 --- a/include/core/factor/factor_block_difference.h +++ b/include/core/factor/factor_block_difference.h @@ -28,7 +28,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorBlockDifference); // class diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h index 4ab23b0a0db6b4c095f1e782ed378df5a037a1e2..b7546fdecd58343de3f826b10baf7d48600d4bbb 100644 --- a/include/core/factor/factor_diff_drive.h +++ b/include/core/factor/factor_diff_drive.h @@ -29,7 +29,6 @@ namespace { - constexpr std::size_t POSITION_SIZE = 2; constexpr std::size_t ORIENTATION_SIZE = 1; constexpr std::size_t INTRINSICS_SIZE = 3; @@ -39,7 +38,6 @@ constexpr std::size_t RESIDUAL_SIZE = 3; namespace wolf { - WOLF_PTR_TYPEDEFS(FactorDiffDrive); class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive, diff --git a/include/core/factor/factor_distance_3d.h b/include/core/factor/factor_distance_3d.h index 67816af683860a0c97ff975c8153c8cb36e2baa4..7ed72d0f6e8049a07a4c20cc66e316fb7e3f3d93 100644 --- a/include/core/factor/factor_distance_3d.h +++ b/include/core/factor/factor_distance_3d.h @@ -24,7 +24,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorDistance3d); class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3> diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h index 2510db856f9d2b7c0027f399ee9e621d5f2d495a..3df6484ec95519cebcd21be8b718324b3ee91833 100644 --- a/include/core/factor/factor_pose_2d.h +++ b/include/core/factor/factor_pose_2d.h @@ -29,7 +29,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorPose2d); // class diff --git a/include/core/factor/factor_pose_2d_with_extrinsics.h b/include/core/factor/factor_pose_2d_with_extrinsics.h index 69901d3c93110ab30a49994ddaa9aa2dce90dff3..6b59bab3fd7a1996b3e932c12d25e016d9eb2b47 100644 --- a/include/core/factor/factor_pose_2d_with_extrinsics.h +++ b/include/core/factor/factor_pose_2d_with_extrinsics.h @@ -29,7 +29,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorPose2dWithExtrinsics); // class @@ -43,16 +42,15 @@ class FactorPose2dWithExtrinsics : public FactorAutodiff<FactorPose2dWithExtrins const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) - : FactorAutodiff( - "FactorPose2dWithExtrinsics", - TOP_ABS, - _measurement, - _measurement_sqrt_information_upper, - {_frame_ptr, _sensor_ptr}, - _processor_ptr, - {_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()}, - _apply_loss_function, - _status) + : FactorAutodiff("FactorPose2dWithExtrinsics", + TOP_ABS, + _measurement, + _measurement_sqrt_information_upper, + {_frame_ptr, _sensor_ptr}, + _processor_ptr, + {_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()}, + _apply_loss_function, + _status) { assert(_frame_ptr->getP() && "FactorPose2dWithExtrinsics: _frame_ptr state P not found!"); assert(_frame_ptr->getO() && "FactorPose2dWithExtrinsics: _frame_ptr state O not found!"); diff --git a/include/core/factor/factor_pose_3d.h b/include/core/factor/factor_pose_3d.h index ca967a87e01e59647024ef8acde5976703d3dc39..a3a18e589ed50651bd15a46d503a1985a95d4aa1 100644 --- a/include/core/factor/factor_pose_3d.h +++ b/include/core/factor/factor_pose_3d.h @@ -27,7 +27,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorPose3d); // class diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h index 1bcae3a421a725452b6135c12a5cd7f439eade20..aa3e57ebe4b15f63a3e7c5c0eb2d84d461e65f39 100644 --- a/include/core/factor/factor_pose_3d_with_extrinsics.h +++ b/include/core/factor/factor_pose_3d_with_extrinsics.h @@ -29,7 +29,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorPose3dWithExtrinsics); // class @@ -43,16 +42,15 @@ class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrins const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) - : FactorAutodiff( - "FactorPose3dWithExtrinsics", - TOP_ABS, - _measurement, - _measurement_sqrt_information_upper, - {_frame_ptr, _sensor_ptr}, - _processor_ptr, - {_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()}, - _apply_loss_function, - _status) + : FactorAutodiff("FactorPose3dWithExtrinsics", + TOP_ABS, + _measurement, + _measurement_sqrt_information_upper, + {_frame_ptr, _sensor_ptr}, + _processor_ptr, + {_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()}, + _apply_loss_function, + _status) { assert(_frame_ptr->getP() && "FactorPose3dWithExtrinsics: _frame_ptr state P not found!"); assert(_frame_ptr->getO() && "FactorPose3dWithExtrinsics: _frame_ptr state O not found!"); diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h index 3f0fdcb1f23b749c710ccdf8edf5eec8c6053a97..116e6eb8e52e6a9f64d3e429d8f9243ab2d5fb0c 100644 --- a/include/core/factor/factor_quaternion_absolute.h +++ b/include/core/factor/factor_quaternion_absolute.h @@ -28,7 +28,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorQuaternionAbsolute); // class diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h index df4e20cfaf2508120af48d08e795f128fa0ec43d..43bc6db56a48724b34c4f8aeb52744cbc97b0e9b 100644 --- a/include/core/factor/factor_relative_pose_3d.h +++ b/include/core/factor/factor_relative_pose_3d.h @@ -25,7 +25,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorRelativePose3d); // class diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h index 67f0794c54ed716bb9fa16489422bf98b036caaa..8c85bca406686a92800bd7015849996b032a0561 100644 --- a/include/core/feature/feature_base.h +++ b/include/core/feature/feature_base.h @@ -35,7 +35,6 @@ class FactorBase; namespace wolf { - // class FeatureBase class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase> { @@ -144,15 +143,15 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature template <typename classType, typename... T> static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all); - virtual void printHeader(int depth, // + virtual void printHeader(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream, std::string _tabs = "") const; - void print(int depth, // + void print(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream = std::cout, std::string _tabs = "") const; @@ -177,7 +176,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature namespace wolf { - template <typename classType, typename... T> std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all) { diff --git a/include/core/feature/feature_diff_drive.h b/include/core/feature/feature_diff_drive.h index 3e8a84ca06f25660a8e8ce1bf2c9796a66b17154..d7314441d34c5c1205679568f9630b581299d706 100644 --- a/include/core/feature/feature_diff_drive.h +++ b/include/core/feature/feature_diff_drive.h @@ -20,24 +20,22 @@ #pragma once -//Wolf includes +// Wolf includes #include "core/feature/feature_motion.h" -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(FeatureDiffDrive) class FeatureDiffDrive : public FeatureMotion { - public: - - FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated, - const Eigen::MatrixXd& _delta_preintegrated_covariance, - const Eigen::VectorXd& _diff_drive_params, - const Eigen::MatrixXd& _jacobian_diff_drive_params); - - virtual ~FeatureDiffDrive() = default; + public: + FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated, + const Eigen::MatrixXd& _delta_preintegrated_covariance, + const Eigen::VectorXd& _diff_drive_params, + const Eigen::MatrixXd& _jacobian_diff_drive_params); + virtual ~FeatureDiffDrive() = default; }; } /* namespace wolf */ diff --git a/include/core/feature/feature_match.h b/include/core/feature/feature_match.h index 034cc3ac508f8f2d84b135d46f46c845697a8783..5cc32a60af5da015b2e87f4f5619383e8cd3ce01 100644 --- a/include/core/feature/feature_match.h +++ b/include/core/feature/feature_match.h @@ -23,10 +23,10 @@ // Wolf includes #include "core/common/wolf.h" -//wolf nampseace -namespace wolf { - -//forward declaration to typedef class pointers +// wolf nampseace +namespace wolf +{ +// forward declaration to typedef class pointers WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatch); /** \brief Map of feature matches @@ -36,8 +36,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatch); * map<FeatureBasePtr actual_feature, FeatureMatchPtr corresponding_feature_match> * */ -typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; //a map is also typedefined - +typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; // a map is also typedefined + /** \brief Match between a feature and a feature * * Match between a feature and a feature (feature-feature correspondence) @@ -45,8 +45,8 @@ typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; //a map is al */ struct FeatureMatch { - FeatureBasePtr feature_ptr_; ///< Corresponding feature - double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good) + FeatureBasePtr feature_ptr_; ///< Corresponding feature + double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good) }; -}//end namespace +} // namespace wolf diff --git a/include/core/feature/feature_motion.h b/include/core/feature/feature_motion.h index 96da75694f82a1c74f0f513c3989f2683cfc9ff7..8675f4c6a332a785312fd8ba384553f9979ca760 100644 --- a/include/core/feature/feature_motion.h +++ b/include/core/feature/feature_motion.h @@ -25,26 +25,25 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FeatureMotion); class FeatureMotion : public FeatureBase { - public: - FeatureMotion(const std::string& _type, - const VectorXd& _delta_preint, - const MatrixXd _delta_preint_cov, - const VectorXd& _calib_preint, - const MatrixXd& _jacobian_calib); - ~FeatureMotion() override; + public: + FeatureMotion(const std::string& _type, + const VectorXd& _delta_preint, + const MatrixXd _delta_preint_cov, + const VectorXd& _calib_preint, + const MatrixXd& _jacobian_calib); + ~FeatureMotion() override; - const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement() - const Eigen::VectorXd& getCalibrationPreint() const; - const Eigen::MatrixXd& getJacobianCalibration() const; + const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement() + const Eigen::VectorXd& getCalibrationPreint() const; + const Eigen::MatrixXd& getJacobianCalibration() const; - private: - Eigen::VectorXd calib_preint_; - Eigen::MatrixXd jacobian_calib_; + private: + Eigen::VectorXd calib_preint_; + Eigen::MatrixXd jacobian_calib_; }; inline const Eigen::VectorXd& FeatureMotion::getDeltaPreint() const diff --git a/include/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h index 075e82bc7c152b83fd481fa766ec953942b906fa..f647522a22ea06d49376b47008126a2f91072d45 100644 --- a/include/core/feature/feature_odom_2d.h +++ b/include/core/feature/feature_odom_2d.h @@ -20,29 +20,28 @@ #pragma once -//Wolf includes +// Wolf includes #include "core/feature/feature_base.h" -//std includes - -namespace wolf { +// std includes +namespace wolf +{ WOLF_PTR_TYPEDEFS(FeatureOdom2d); - -//class FeatureOdom2d + +// class FeatureOdom2d class FeatureOdom2d : public FeatureBase { - public: - - /** \brief Constructor from capture pointer and measure - * - * \param _measurement the measurement - * \param _meas_covariance the noise of the measurement - * - */ - FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance); - - ~FeatureOdom2d() override; + public: + /** \brief Constructor from capture pointer and measure + * + * \param _measurement the measurement + * \param _meas_covariance the noise of the measurement + * + */ + FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance); + + ~FeatureOdom2d() override; }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/feature/feature_pose.h b/include/core/feature/feature_pose.h index 8479a8e146bcc9948ec86172f6b0dba35318f53a..bd9c547ac44c53ebb495fde8c55a48c193e5d5fc 100644 --- a/include/core/feature/feature_pose.h +++ b/include/core/feature/feature_pose.h @@ -20,29 +20,28 @@ #pragma once -//Wolf includes +// Wolf includes #include "core/feature/feature_base.h" -//std includes +// std includes // -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(FeaturePose); -//class FeaturePose +// class FeaturePose class FeaturePose : public FeatureBase { - public: - - /** \brief Constructor from capture pointer and measure - * - * \param _measurement the measurement - * \param _meas_covariance the noise of the measurement - * - */ - FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance); - ~FeaturePose() override; + public: + /** \brief Constructor from capture pointer and measure + * + * \param _measurement the measurement + * \param _meas_covariance the noise of the measurement + * + */ + FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance); + ~FeaturePose() override; }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 3472c2be41451f74a930fa07828f332116effb7e..ae9691f84638d1f48c948c70b14264ffd6756a8b 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -159,16 +159,16 @@ class FrameBase : public NodeStateBlocks CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr); // Debug and info ------------------------------------------------------- - virtual void printHeader(int depth, // + virtual void printHeader(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream, std::string _tabs = "") const; - void print(int depth, // + void print(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream = std::cout, std::string _tabs = "") const; diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h index cc9e5516c4203d83e9dfaf78691d4d5f2765d2dd..3bb567ca8380340fcb2fbe2bcf596946b5478406 100644 --- a/include/core/hardware/hardware_base.h +++ b/include/core/hardware/hardware_base.h @@ -33,7 +33,6 @@ class SensorBase; namespace wolf { - // class HardwareBase class HardwareBase : public NodeBase, public std::enable_shared_from_this<HardwareBase> { @@ -56,15 +55,15 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa SensorBaseConstPtr getSensor(const std::string& _sensor_name) const; SensorBasePtr getSensor(const std::string& _sensor_name); - virtual void printHeader(int depth, // + virtual void printHeader(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream, std::string _tabs = "") const; - void print(int depth, // + void print(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream = std::cout, std::string _tabs = "") const; @@ -82,7 +81,6 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa namespace wolf { - inline SensorBaseConstPtrList HardwareBase::getSensorList() const { SensorBaseConstPtrList list_const; diff --git a/include/core/landmark/factory_landmark.h b/include/core/landmark/factory_landmark.h index b9e14bf698b9a2a5b28c3a11ae2cf8dbf32cba20..cc3c600909aa413c9b03a1cb783be5f1e56fb616 100644 --- a/include/core/landmark/factory_landmark.h +++ b/include/core/landmark/factory_landmark.h @@ -28,7 +28,6 @@ namespace wolf { - /** \brief Landmark factory class * * This factory can create objects of classes deriving from LandmarkBase. @@ -56,7 +55,7 @@ namespace wolf * \code * static LandmarkBasePtr create(const YAML::Node& _node) * { - * return std::make_shared<LandmarkDerived>("LandmarkDerived", _node); + * return std::make_shared<LandmarkDerived>("LandmarkDerived", _node); * } * \endcode * @@ -70,12 +69,13 @@ namespace wolf * auto corner_ptr = FactoryLandmark::create("LandmarkCorner", _node); * \endcode * - * We RECOMMEND using the macro ````WOLF_LANDMARK_CREATE(LandmarkClass)```` to automatically add the landmark creator, provided in ````landmark_base.h````. - * + * We RECOMMEND using the macro ````WOLF_LANDMARK_CREATE(LandmarkClass)```` to automatically add the landmark creator, + * provided in ````landmark_base.h````. + * * To do so, you should implement a constructor with the API: - * + * * \code - * LandmarkDerived(const YAML::Node& _node) : + * LandmarkDerived(const YAML::Node& _node) : * LandmarkBase("LandmarkDerived", _node) * { * < CODE > @@ -109,7 +109,7 @@ namespace wolf * const bool registered_corner = FactoryLandmark::registerCreator("LandmarkCorner", LandmarkCorner::create); * } * \endcode - * + * * Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro * ````WOLF_REGISTER_LANDMARK(LandmarkType)```` in ````landmark_base.h````. * You are free to comment out these lines and place them wherever you consider it more convenient for your design. @@ -131,7 +131,7 @@ namespace wolf * // a YAML node with the info * * LandmarkBasePtr corner_1_ptr = - * FactoryLandmark::create ( "LandmarkCorner", + * FactoryLandmark::create ( "LandmarkCorner", * node ); * * LandmarkBasePtr corner_2_ptr = @@ -143,17 +143,19 @@ namespace wolf * \endcode */ -typedef Factory<LandmarkBasePtr, - const YAML::Node&> FactoryLandmark; +typedef Factory<LandmarkBasePtr, const YAML::Node&> FactoryLandmark; -template<> +template <> inline std::string FactoryLandmark::getClass() const { - return "FactoryLandmark"; + return "FactoryLandmark"; } -#define WOLF_REGISTER_LANDMARK(LandmarkType) \ - namespace{ const bool WOLF_UNUSED LandmarkType##Registered = \ - FactoryLandmark::registerCreator(#LandmarkType, LandmarkType::create);} \ +#define WOLF_REGISTER_LANDMARK(LandmarkType) \ + namespace \ + { \ + const bool WOLF_UNUSED LandmarkType##Registered = \ + FactoryLandmark::registerCreator(#LandmarkType, LandmarkType::create); \ + } } /* namespace wolf */ diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index 03abdad51adea89558a04da0a900bf6834e025a0..b11a2ebcf561d1da730c08a080ebe23f3eedc086 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -54,7 +54,7 @@ namespace wolf static LandmarkBasePtr create(const YAML::Node& _node) \ { \ auto lmk = std::make_shared<LandmarkClass>(_node); \ - lmk->emplacePriors(SpecStateComposite(_node["states"])); \ + lmk->emplacePriors(SpecStateComposite(_node["states"])); \ return lmk; \ } diff --git a/include/core/landmark/landmark_match.h b/include/core/landmark/landmark_match.h index 072494273bd2d7979f4d3b970a65cdcea8689e6c..5132e25bf3327b6811ab3c58155ebfa2f06a48c9 100644 --- a/include/core/landmark/landmark_match.h +++ b/include/core/landmark/landmark_match.h @@ -23,9 +23,9 @@ // Wolf includes #include "core/common/wolf.h" -//wolf nampseace -namespace wolf { - +// wolf nampseace +namespace wolf +{ // Map of Feature - Landmark matches WOLF_STRUCT_PTR_TYPEDEFS(LandmarkMatch); typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap; @@ -37,19 +37,14 @@ typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap; **/ struct LandmarkMatch { - LandmarkBasePtr landmark_ptr_; ///< Pointer to the matched landmark - double normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match - - LandmarkMatch() : - landmark_ptr_(nullptr), normalized_score_(0) - { + LandmarkBasePtr landmark_ptr_; ///< Pointer to the matched landmark + double normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match - } - LandmarkMatch(LandmarkBasePtr _landmark_ptr, double _normalized_score) : - landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score) + LandmarkMatch() : landmark_ptr_(nullptr), normalized_score_(0) {} + LandmarkMatch(LandmarkBasePtr _landmark_ptr, double _normalized_score) + : landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score) { - } }; -}//end namespace +} // namespace wolf diff --git a/include/core/map/factory_map.h b/include/core/map/factory_map.h index 0b4a347c2cadf32191cd83f4957f683a83bbe514..f2a803269964461a23b925ed1aa87a8e89fa0b03 100644 --- a/include/core/map/factory_map.h +++ b/include/core/map/factory_map.h @@ -28,7 +28,6 @@ namespace wolf { - /** \brief Map factory class * * This factory can create objects of classes deriving from MapBase. @@ -108,14 +107,16 @@ namespace wolf */ typedef Factory<MapBasePtr, const YAML::Node&> FactoryMap; -template<> +template <> inline std::string FactoryMap::getClass() const { - return "FactoryMap"; + return "FactoryMap"; } -#define WOLF_REGISTER_MAP(MapType) \ - namespace{ const bool WOLF_UNUSED MapType##Registered = \ - FactoryMap::registerCreator(#MapType, MapType::create); } \ +#define WOLF_REGISTER_MAP(MapType) \ + namespace \ + { \ + const bool WOLF_UNUSED MapType##Registered = FactoryMap::registerCreator(#MapType, MapType::create); \ + } } /* namespace wolf */ diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h index 9d9fd4ed2924a1b6764faf7f1a39d5c76762af26..aeaa5550cc747563a6844c377ec30b635359c860 100644 --- a/include/core/map/map_base.h +++ b/include/core/map/map_base.h @@ -38,7 +38,6 @@ class LandmarkBase; namespace wolf { - /* * Macro for defining Autoconf map creator. * @@ -110,15 +109,15 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase> void load(std::string _map_file_yaml); void save(std::string _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf") const; - virtual void printHeader(int depth, // + virtual void printHeader(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream, std::string _tabs = "") const; - void print(int depth, // + void print(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream = std::cout, std::string _tabs = "") const; diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h index 5e14eeaea5b30ee831ab73e7791a0c1d8a01a433..79b6cf27e05b8a9de97d2ebc24ee6d9f903e3837 100644 --- a/include/core/math/SE2.h +++ b/include/core/math/SE2.h @@ -31,55 +31,55 @@ /* * Some of the functions below are based on: * - * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf + * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, + * https://arxiv.org/pdf/1812.01537.pdf */ namespace wolf { -namespace SO2{ - -template<typename T> +namespace SO2 +{ +template <typename T> inline Eigen::Matrix<T, 2, 2> exp(const T theta) { return Eigen::Rotation2D<T>(theta).matrix(); } -} // namespace SO2 - - -namespace SE2{ +} // namespace SO2 -template<class T> +namespace SE2 +{ +template <class T> inline Eigen::Matrix<T, 2, 2> skew(const T& t) { - return (Eigen::Matrix<T, 2, 2>() << (T) 0, -t, t, (T) 0).finished(); + return (Eigen::Matrix<T, 2, 2>() << (T)0, -t, t, (T)0).finished(); } /** \brief Compute [1]_x * t = (-t.y ; t.x) */ -template<class D> +template <class D> inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const MatrixBase<D>& t) { - return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0)); + return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0)); } -template<typename T> +template <typename T> inline Eigen::Matrix2d V_helper(const T theta) { - T s; // sin(theta) / theta - T c_1; // (1-cos(theta)) / theta + T s; // sin(theta) / theta + T c_1; // (1-cos(theta)) / theta if (fabs(theta) > T(Constants::EPS)) { // [1] eq. 158 - s = sin(theta) / theta; + s = sin(theta) / theta; c_1 = (T(1.0) - cos(theta)) / theta; } else { // approx of [1] eq. 158 - s = T(1.0); // sin(x) ~= x - c_1 = theta / T(2.0); // cos(x) ~= 1 - x^2/2 + s = T(1.0); // sin(x) ~= x + c_1 = theta / T(2.0); // cos(x) ~= 1 - x^2/2 } return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished(); @@ -94,9 +94,8 @@ inline VectorComposite identity() return v; } -template<typename D1, typename D2> -inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o, - MatrixBase<D2>& ip, typename D2::Scalar& io) +template <typename D1, typename D2> +inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o, MatrixBase<D2>& ip, typename D2::Scalar& io) { MatrixSizeCheck<2, 1>::check(p); MatrixSizeCheck<2, 1>::check(ip); @@ -105,9 +104,8 @@ inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o, io = -o; } -template<typename D1, typename D2, typename D3, typename D4> -inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o, - MatrixBase<D3>& ip, MatrixBase<D4>& io) +template <typename D1, typename D2, typename D3, typename D4> +inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o, MatrixBase<D3>& ip, MatrixBase<D4>& io) { MatrixSizeCheck<2, 1>::check(p); MatrixSizeCheck<1, 1>::check(o); @@ -117,20 +115,19 @@ inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o, inverse(p, o(0), ip, io(0)); } -template<typename D1, typename D2> -inline void inverse(const MatrixBase<D1>& d, - MatrixBase<D2>& id) +template <typename D1, typename D2> +inline void inverse(const MatrixBase<D1>& d, MatrixBase<D2>& id) { MatrixSizeCheck<3, 1>::check(d); MatrixSizeCheck<3, 1>::check(id); - Map<const Matrix<typename D1::Scalar, 2, 1> > p ( & d( 0 ) ); - Map<Matrix<typename D2::Scalar, 2, 1> > ip ( & id( 0 ) ); + Map<const Matrix<typename D1::Scalar, 2, 1> > p(&d(0)); + Map<Matrix<typename D2::Scalar, 2, 1> > ip(&id(0)); inverse(p, d(2), ip, id(2)); } -template<typename D> +template <typename D> inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d) { Matrix<typename D::Scalar, 3, 1> id; @@ -150,7 +147,7 @@ inline VectorComposite inverse(const VectorComposite& v) return c; } -template<class D1, class D2> +template <class D1, class D2> inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta) { MatrixSizeCheck<3, 1>::check(_tau); @@ -158,29 +155,29 @@ inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta // [1] eq. 156 _delta.head(2) = V_helper(_tau(2)) * _tau.head(2); - _delta(2) = pi2pi(_tau(2)); + _delta(2) = pi2pi(_tau(2)); } -template<class D, typename T> +template <class D, typename T> inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta) { - MatrixSizeCheck<2, 1>::check (p); + MatrixSizeCheck<2, 1>::check(p); // Jacobian of t = V(theta)*p wrt theta -- developed in Matlab symbolic, and copied here. - T x = p(0); - T y = p(1); + T x = p(0); + T y = p(1); Matrix<T, 2, 1> J_Vp_th; if (fabs(theta) > T(Constants::EPS)) { - T s_th = sin(theta); - T c_th = cos(theta); + T s_th = sin(theta); + T c_th = cos(theta); T theta2 = theta * theta; J_Vp_th << -(y * c_th - y + x * s_th - theta * x * c_th + theta * y * s_th) / theta2, - (x * c_th - x - y * s_th + theta * y * c_th + theta * x * s_th) / theta2; + (x * c_th - x - y * s_th + theta * y * c_th + theta * x * s_th) / theta2; } else - { // sin(x) ~= x + { // sin(x) ~= x // cos(x) ~= 1 - x^2/2 J_Vp_th << -y / 2.0, x / 2.0; } @@ -188,7 +185,7 @@ inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T return J_Vp_th; } -template<class D1, class D2, class D3> +template <class D1, class D2, class D3> inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau) { MatrixSizeCheck<3, 1>::check(_tau); @@ -198,10 +195,10 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D typedef typename D1::Scalar T; // [1] eq. 156 - T theta = pi2pi(_tau(2)); - Eigen::Matrix<T, 2, 2> V = V_helper(theta); - _delta.head(2) = V * _tau.head(2); - _delta(2) = theta; + T theta = pi2pi(_tau(2)); + Eigen::Matrix<T, 2, 2> V = V_helper(theta); + _delta.head(2) = V * _tau.head(2); + _delta(2) = theta; // Jacobian is the composite definition [1] eq. 89, with jacobian blocks: // J_Vp_p = V: see V_helper, eq. 158 @@ -213,9 +210,9 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D inline void exp(const VectorComposite& _tau, VectorComposite& _delta) { // [1] eq. 156 - const auto &p = _tau.at('P'); - const auto &theta = pi2pi(_tau.at('O')(0)); - Matrix2d V = V_helper(theta); + const auto& p = _tau.at('P'); + const auto& theta = pi2pi(_tau.at('O')(0)); + Matrix2d V = V_helper(theta); _delta['P'] = V * p; _delta['O'] = Matrix1d(theta); @@ -233,9 +230,9 @@ inline VectorComposite exp(const VectorComposite& tau) inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau) { // [1] eq. 156 - const auto &p = _tau.at('P'); - const auto &theta = pi2pi(_tau.at('O')(0)); - Matrix2d V = V_helper(theta); + const auto& p = _tau.at('P'); + const auto& theta = pi2pi(_tau.at('O')(0)); + Matrix2d V = V_helper(theta); _delta['P'] = V * p; _delta['O'] = Matrix1d(theta); @@ -251,32 +248,34 @@ inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComp _J_delta_tau.emplace('O', 'O', Matrix1d(1)); } -template<class D1, class D2, class D3> -inline void compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2) +template <class D1, class D2, class D3> +inline void compose(const MatrixBase<D1>& _delta1, + const MatrixBase<D2>& _delta2, + MatrixBase<D3>& _delta1_compose_delta2) { MatrixSizeCheck<3, 1>::check(_delta1); MatrixSizeCheck<3, 1>::check(_delta2); MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2); - _delta1_compose_delta2.template head<2>() = _delta1.template head<2>() - + SO2::exp(_delta1(2)) * _delta2.template head<2>(); + _delta1_compose_delta2.template head<2>() = + _delta1.template head<2>() + SO2::exp(_delta1(2)) * _delta2.template head<2>(); _delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2)); } -template<class D1, class D2> -inline Matrix<typename D1::Scalar,3,1> compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2) +template <class D1, class D2> +inline Matrix<typename D1::Scalar, 3, 1> compose(const MatrixBase<D1>& _delta1, const MatrixBase<D2>& _delta2) { - Matrix<typename D1::Scalar,3,1> delta1_compose_delta2; + Matrix<typename D1::Scalar, 3, 1> delta1_compose_delta2; compose(_delta1, _delta2, delta1_compose_delta2); return delta1_compose_delta2; } -template<class D1, class D2, class D3, class D4, class D5> +template <class D1, class D2, class D3, class D4, class D5> inline void compose(const MatrixBase<D1>& _delta1, const MatrixBase<D2>& _delta2, - MatrixBase<D3>& _delta1_compose_delta2, - MatrixBase<D4>& _J_compose_delta1, - MatrixBase<D5>& _J_compose_delta2) + MatrixBase<D3>& _delta1_compose_delta2, + MatrixBase<D4>& _J_compose_delta1, + MatrixBase<D5>& _J_compose_delta2) { MatrixSizeCheck<3, 1>::check(_delta1); MatrixSizeCheck<3, 1>::check(_delta2); @@ -284,7 +283,7 @@ inline void compose(const MatrixBase<D1>& _delta1, MatrixSizeCheck<3, 3>::check(_J_compose_delta1); MatrixSizeCheck<3, 3>::check(_J_compose_delta2); - Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary + Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary // tc = t1 + R1 t2 _delta1_compose_delta2.template head<2>() = _delta1.template head<2>() + R1 * _delta2.template head<2>(); @@ -326,22 +325,20 @@ inline void compose(const MatrixBase<D1>& _delta1, _J_compose_delta2.template block<2, 2>(0, 0) = R1; } -inline void compose(const VectorComposite& _x1, - const VectorComposite& _x2, - VectorComposite& _c) +inline void compose(const VectorComposite& _x1, const VectorComposite& _x2, VectorComposite& _c) { const auto& p1 = _x1.at('P'); - const auto& a1 = _x1.at('O')(0); // angle + const auto& a1 = _x1.at('O')(0); // angle const auto& R1 = SO2::exp(a1); const auto& p2 = _x2.at('P'); - const auto& a2 = _x2.at('O')(0); // angle + const auto& a2 = _x2.at('O')(0); // angle // tc = t1 + R1 t2 _c['P'] = p1 + R1 * p2; // Rc = R1 R2 --> theta = theta1 + theta2 - _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ; + _c['O'] = Matrix1d(pi2pi(a1 + a2)); } inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2) @@ -353,16 +350,16 @@ inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& inline void compose(const VectorComposite& _x1, const VectorComposite& _x2, - VectorComposite& _c, - MatrixComposite& _J_c_x1, - MatrixComposite& _J_c_x2) + VectorComposite& _c, + MatrixComposite& _J_c_x1, + MatrixComposite& _J_c_x2) { const auto& p1 = _x1.at('P'); - const auto& a1 = _x1.at('O')(0); // angle - Matrix2d R1 = SO2::exp(a1); // need temporary + const auto& a1 = _x1.at('O')(0); // angle + Matrix2d R1 = SO2::exp(a1); // need temporary const auto& p2 = _x2.at('P'); - const auto& a2 = _x2.at('O')(0); // angle + const auto& a2 = _x2.at('O')(0); // angle /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130) * @@ -393,31 +390,33 @@ inline void compose(const VectorComposite& _x1, _J_c_x1.clear(); _J_c_x1.emplace('P', 'P', Matrix2d::Identity()); - _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)) ); - _J_c_x1.emplace('O', 'P', RowVector2d(0,0)); + _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2))); + _J_c_x1.emplace('O', 'P', RowVector2d(0, 0)); _J_c_x1.emplace('O', 'O', Matrix1d(1)); _J_c_x2.clear(); _J_c_x2.emplace('P', 'P', R1); - _J_c_x2.emplace('P', 'O', Vector2d(0,0)); - _J_c_x2.emplace('O', 'P', RowVector2d(0,0)); + _J_c_x2.emplace('P', 'O', Vector2d(0, 0)); + _J_c_x2.emplace('O', 'P', RowVector2d(0, 0)); _J_c_x2.emplace('O', 'O', Matrix1d(1)); - - // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'. + // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or + // 'x2'. // tc = t1 + R1 t2 _c['P'] = p1 + R1 * p2; // Rc = R1 R2 --> theta = theta1 + theta2 - _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ; - + _c['O'] = Matrix1d(pi2pi(a1 + a2)); } -template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> -inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1, - const MatrixBase<D4>& p2, const MatrixBase<D5>& o2, - MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q) +template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void plus(const MatrixBase<D1>& p1, + const MatrixBase<D2>& q1, + const MatrixBase<D4>& p2, + const MatrixBase<D5>& o2, + MatrixBase<D7>& plus_p, + MatrixBase<D8>& plus_q) { MatrixSizeCheck<2, 1>::check(p1); MatrixSizeCheck<1, 1>::check(q1); @@ -430,24 +429,21 @@ inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1, plus_q(0) = pi2pi(q1(0) + o2(0)); } -template<typename D1, typename D2, typename D3> -inline void plus(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, - MatrixBase<D3>& d_plus) +template <typename D1, typename D2, typename D3> +inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d_plus) { - Map<const Matrix<typename D1::Scalar, 2, 1> > p1 ( & d1(0) ); - Map<const Matrix<typename D1::Scalar, 1, 1> > q1 ( & d1(2) ); - Map<const Matrix<typename D2::Scalar, 2, 1> > p2 ( & d2(0) ); - Map<const Matrix<typename D2::Scalar, 1, 1> > o2 ( & d2(2) ); - Map<Matrix<typename D3::Scalar, 2, 1> > p_p ( & d_plus(0) ); - Map<Matrix<typename D1::Scalar, 1, 1> > q_p ( & d_plus(2) ); + Map<const Matrix<typename D1::Scalar, 2, 1> > p1(&d1(0)); + Map<const Matrix<typename D1::Scalar, 1, 1> > q1(&d1(2)); + Map<const Matrix<typename D2::Scalar, 2, 1> > p2(&d2(0)); + Map<const Matrix<typename D2::Scalar, 1, 1> > o2(&d2(2)); + Map<Matrix<typename D3::Scalar, 2, 1> > p_p(&d_plus(0)); + Map<Matrix<typename D1::Scalar, 1, 1> > q_p(&d_plus(2)); plus(p1, q1, p2, o2, p_p, q_p); } -template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2) +template <typename D1, typename D2> +inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2) { Matrix<typename D1::Scalar, 3, 1> d_plus; plus(d1, d2, d_plus); @@ -468,23 +464,24 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau return res; } -template<typename D1, typename D2, typename D3> -inline void between(const MatrixBase<D1>& p1, const typename D1::Scalar& o1, - const MatrixBase<D2>& p2, const typename D2::Scalar& o2, - MatrixBase<D3>& p12, typename D3::Scalar& o12) +template <typename D1, typename D2, typename D3> +inline void between(const MatrixBase<D1>& p1, + const typename D1::Scalar& o1, + const MatrixBase<D2>& p2, + const typename D2::Scalar& o2, + MatrixBase<D3>& p12, + typename D3::Scalar& o12) { - MatrixSizeCheck<2, 1>::check(p1); - MatrixSizeCheck<2, 1>::check(p2); - MatrixSizeCheck<2, 1>::check(p12); + MatrixSizeCheck<2, 1>::check(p1); + MatrixSizeCheck<2, 1>::check(p2); + MatrixSizeCheck<2, 1>::check(p12); - p12 = SO2::exp(-o1) * ( p2 - p1 ); - o12 = o2 - o1; + p12 = SO2::exp(-o1) * (p2 - p1); + o12 = o2 - o1; } -template<typename D1, typename D2, typename D3> -inline void between(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, - MatrixBase<D3>& d12) +template <typename D1, typename D2, typename D3> +inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d12) { MatrixSizeCheck<3, 1>::check(d1); MatrixSizeCheck<3, 1>::check(d2); @@ -492,16 +489,15 @@ inline void between(const MatrixBase<D1>& d1, typedef typename D1::Scalar T; - Map<const Matrix<T, 2, 1> > p1 ( & d1(0) ); - Map<const Matrix<T, 2, 1> > p2 ( & d2(0) ); - Map<Matrix<T, 2, 1> > p12 ( & d12(0) ); + Map<const Matrix<T, 2, 1> > p1(&d1(0)); + Map<const Matrix<T, 2, 1> > p2(&d2(0)); + Map<Matrix<T, 2, 1> > p12(&d12(0)); between(p1, d1(2), p2, d2(2), p12, d12(2)); } -template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2 ) +template <typename D1, typename D2> +inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2) { MatrixSizeCheck<3, 1>::check(d1); MatrixSizeCheck<3, 1>::check(d2); @@ -510,5 +506,5 @@ inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1, return d12; } -} // namespace SE2 -} // namespacs wolf +} // namespace SE2 +} // namespace wolf diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h index fe456a9103b420eacdea3461e671b8e68c34ac67..be94f1b8009738597895ac1da98bea6e69c9e543 100644 --- a/include/core/math/SE3.h +++ b/include/core/math/SE3.h @@ -50,23 +50,25 @@ * * Some of the functions below are based on: * - * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf + * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, + * https://arxiv.org/pdf/1812.01537.pdf */ namespace wolf { -namespace SE3 { +namespace SE3 +{ using namespace Eigen; -template<typename D1, typename D2> +template <typename D1, typename D2> inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q) { MatrixSizeCheck<3, 1>::check(p); - p = MatrixBase<D1>::Zero(3,1); + p = MatrixBase<D1>::Zero(3, 1); q = QuaternionBase<D2>::Identity(); } -template<typename D1, typename D2> +template <typename D1, typename D2> inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q) { MatrixSizeCheck<3, 1>::check(p); @@ -84,42 +86,39 @@ inline void identity(VectorComposite& v) v['O'] = Quaterniond::Identity().coeffs(); } -template<typename T = double> +template <typename T = double> inline Matrix<T, 7, 1> identity() { Matrix<T, 7, 1> ret; - ret<< T(0), T(0), T(0), - T(0), T(0), T(0), T(1); + ret << T(0), T(0), T(0), T(0), T(0), T(0), T(1); return ret; } -template<typename D1, typename D2, typename D4, typename D5> -inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, - MatrixBase<D4>& ip, QuaternionBase<D5>& iq) +template <typename D1, typename D2, typename D4, typename D5> +inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, MatrixBase<D4>& ip, QuaternionBase<D5>& iq) { MatrixSizeCheck<3, 1>::check(p); MatrixSizeCheck<3, 1>::check(ip); - ip = -(q.conjugate() * p) ; - iq = q.conjugate() ; + ip = -(q.conjugate() * p); + iq = q.conjugate(); } -template<typename D1, typename D2> -inline void inverse(const MatrixBase<D1>& d, - MatrixBase<D2>& id) +template <typename D1, typename D2> +inline void inverse(const MatrixBase<D1>& d, MatrixBase<D2>& id) { MatrixSizeCheck<7, 1>::check(d); MatrixSizeCheck<7, 1>::check(id); - Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & d( 0 ) ); - Map<const Quaternion<typename D1::Scalar> > q ( & d( 3 ) ); - Map<Matrix<typename D2::Scalar, 3, 1> > ip ( & id( 0 ) ); - Map<Quaternion<typename D2::Scalar> > iq ( & id( 3 ) ); + Map<const Matrix<typename D1::Scalar, 3, 1>> p(&d(0)); + Map<const Quaternion<typename D1::Scalar>> q(&d(3)); + Map<Matrix<typename D2::Scalar, 3, 1>> ip(&id(0)); + Map<Quaternion<typename D2::Scalar>> iq(&id(3)); inverse(p, q, ip, iq); } -template<typename D> +template <typename D> inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d) { Matrix<typename D::Scalar, 7, 1> id; @@ -129,107 +128,110 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d) inline void inverse(const VectorComposite& v, VectorComposite& c) { - Map<const Quaternion<double> > qv( & v.at('O')(0) ); - Map<Quaternion<double> > qc( & c['O'](0) ); + Map<const Quaternion<double>> qv(&v.at('O')(0)); + Map<Quaternion<double>> qc(&c['O'](0)); inverse(v.at('P'), qv, c['P'], qc); } inline VectorComposite inverse(const VectorComposite& v) { - VectorComposite c("PO", Vector7d::Zero(), {3,4}); + VectorComposite c("PO", Vector7d::Zero(), {3, 4}); inverse(v, c); return c; } - -template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> -inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, - const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, - MatrixBase<D7>& pc, QuaternionBase<D8>& qc ) +template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void compose(const MatrixBase<D1>& p1, + const QuaternionBase<D2>& q1, + const MatrixBase<D4>& p2, + const QuaternionBase<D5>& q2, + MatrixBase<D7>& pc, + QuaternionBase<D8>& qc) { - MatrixSizeCheck<3, 1>::check(p1); - MatrixSizeCheck<3, 1>::check(p2); - MatrixSizeCheck<3, 1>::check(pc); + MatrixSizeCheck<3, 1>::check(p1); + MatrixSizeCheck<3, 1>::check(p2); + MatrixSizeCheck<3, 1>::check(pc); - pc = p1 + q1 * p2; - qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum + pc = p1 + q1 * p2; + qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum } -template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> -inline void compose(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1, - const MatrixBase<D4>& p2, const MatrixBase<D5>& q2, - MatrixBase<D7>& pc, MatrixBase<D8>& qc ) +template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void compose(const MatrixBase<D1>& p1, + const MatrixBase<D2>& q1, + const MatrixBase<D4>& p2, + const MatrixBase<D5>& q2, + MatrixBase<D7>& pc, + MatrixBase<D8>& qc) { - MatrixSizeCheck<3, 1>::check(p1); - MatrixSizeCheck<4, 1>::check(q1); - MatrixSizeCheck<3, 1>::check(p2); - MatrixSizeCheck<4, 1>::check(q2); - MatrixSizeCheck<3, 1>::check(pc); - MatrixSizeCheck<4, 1>::check(qc); + MatrixSizeCheck<3, 1>::check(p1); + MatrixSizeCheck<4, 1>::check(q1); + MatrixSizeCheck<3, 1>::check(p2); + MatrixSizeCheck<4, 1>::check(q2); + MatrixSizeCheck<3, 1>::check(pc); + MatrixSizeCheck<4, 1>::check(qc); - Map<const Quaternion<typename D2::Scalar> > mq1 ( & q1( 0 ) ); - Map<const Quaternion<typename D5::Scalar> > mq2 ( & q2( 0 ) ); - Map< Quaternion<typename D8::Scalar> > mqc ( & qc( 0 ) ); + Map<const Quaternion<typename D2::Scalar>> mq1(&q1(0)); + Map<const Quaternion<typename D5::Scalar>> mq2(&q2(0)); + Map<Quaternion<typename D8::Scalar>> mqc(&qc(0)); - compose(p1, mq1, p2, mq2, pc, mqc); + compose(p1, mq1, p2, mq2, pc, mqc); } -template<typename D1, typename D2, typename D3> -inline void compose(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, - MatrixBase<D3>& sum) +template <typename D1, typename D2, typename D3> +inline void compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& sum) { MatrixSizeCheck<7, 1>::check(d1); MatrixSizeCheck<7, 1>::check(d2); MatrixSizeCheck<7, 1>::check(sum); - Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1( 0 ) ); - Map<const Quaternion<typename D1::Scalar> > q1 ( & d1( 3 ) ); - Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2( 0 ) ); - Map<const Quaternion<typename D2::Scalar> > q2 ( & d2( 3 ) ); - Map<Matrix<typename D3::Scalar, 3, 1> > pc ( & sum( 0 ) ); - Map<Quaternion<typename D3::Scalar> > qc ( & sum( 3 ) ); + Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0)); + Map<const Quaternion<typename D1::Scalar>> q1(&d1(3)); + Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0)); + Map<const Quaternion<typename D2::Scalar>> q2(&d2(3)); + Map<Matrix<typename D3::Scalar, 3, 1>> pc(&sum(0)); + Map<Quaternion<typename D3::Scalar>> qc(&sum(3)); compose(p1, q1, p2, q2, pc, qc); } -template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2 ) +template <typename D1, typename D2> +inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2) { - Matrix<typename D1::Scalar, 7, 1> ret; + Matrix<typename D1::Scalar, 7, 1> ret; compose(d1, d2, ret); return ret; } -template<typename D1, typename D2, typename D3, typename D4, typename D5> +template <typename D1, typename D2, typename D3, typename D4, typename D5> inline void compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, - MatrixBase<D3>& sum, - MatrixBase<D4>& J_sum_d1, - MatrixBase<D5>& J_sum_d2) + MatrixBase<D3>& sum, + MatrixBase<D4>& J_sum_d1, + MatrixBase<D5>& J_sum_d2) { MatrixSizeCheck<7, 1>::check(d1); MatrixSizeCheck<7, 1>::check(d2); MatrixSizeCheck<7, 1>::check(sum); - MatrixSizeCheck< 6, 6>::check(J_sum_d1); - MatrixSizeCheck< 6, 6>::check(J_sum_d2); + MatrixSizeCheck<6, 6>::check(J_sum_d1); + MatrixSizeCheck<6, 6>::check(J_sum_d2); // Some useful temporaries - Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //q1.matrix(); // First Delta, DR - Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //q2.matrix(); // Second delta, dR + Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3, 4)); // q1.matrix(); // First Delta, DR + Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3, 4)); // q2.matrix(); // Second delta, dR // Jac wrt first delta - J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I - J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo - J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo + J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I + J_sum_d1.block(0, 3, 3, 3).noalias() = -dR1 * skew(d2.head(3)); // dDp'/dDo + J_sum_d1.block(3, 3, 3, 3) = dR2.transpose(); // dDo'/dDo // Jac wrt second delta - J_sum_d2.setIdentity(); // - J_sum_d2.block(0,0,3,3) = dR1; // dDp'/p + J_sum_d2.setIdentity(); // + J_sum_d2.block(0, 0, 3, 3) = dR1; // dDp'/p // J_sum_d2.block(3,3,3,3) = Matrix3d::Identity(); // dDo'/ddo = I - // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable + // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same + // variable compose(d1, d2, sum); } @@ -240,73 +242,73 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2) { - VectorComposite c("PO", Vector7d::Zero(), {3,4}); + VectorComposite c("PO", Vector7d::Zero(), {3, 4}); compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']); return c; } inline void compose(const VectorComposite& x1, const VectorComposite& x2, - VectorComposite& c, - MatrixComposite& J_c_x1, - MatrixComposite& J_c_x2) + VectorComposite& c, + MatrixComposite& J_c_x1, + MatrixComposite& J_c_x2) { - // Some useful temporaries - const auto R1 = q2R(x1.at('O')); //q1.matrix(); // make temporary - const auto& R2 = q2R(x2.at('O')); //q2.matrix(); // do not make temporary, only reference + const auto R1 = q2R(x1.at('O')); // q1.matrix(); // make temporary + const auto& R2 = q2R(x2.at('O')); // q2.matrix(); // do not make temporary, only reference // Jac wrt first delta - J_c_x1.emplace('P', 'P', Matrix3d::Identity() ) ; - J_c_x1.emplace('P', 'O', - R1 * skew(x2.at('P')) ) ; - J_c_x1.emplace('O', 'P', Matrix3d::Zero() ) ; - J_c_x1.emplace('O', 'O', R2.transpose() ) ; + J_c_x1.emplace('P', 'P', Matrix3d::Identity()); + J_c_x1.emplace('P', 'O', -R1 * skew(x2.at('P'))); + J_c_x1.emplace('O', 'P', Matrix3d::Zero()); + J_c_x1.emplace('O', 'O', R2.transpose()); // Jac wrt second delta - J_c_x2.emplace('P', 'P', R1 ); - J_c_x2.emplace('P', 'O', Matrix3d::Zero() ); - J_c_x2.emplace('O', 'P', Matrix3d::Zero() ); - J_c_x2.emplace('O', 'O', Matrix3d::Identity() ); + J_c_x2.emplace('P', 'P', R1); + J_c_x2.emplace('P', 'O', Matrix3d::Zero()); + J_c_x2.emplace('O', 'P', Matrix3d::Zero()); + J_c_x2.emplace('O', 'O', Matrix3d::Identity()); - // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable + // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same + // variable compose(x1, x2, c); } -template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> -inline void between(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, - const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, - MatrixBase<D7>& p12, QuaternionBase<D8>& q12) +template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void between(const MatrixBase<D1>& p1, + const QuaternionBase<D2>& q1, + const MatrixBase<D4>& p2, + const QuaternionBase<D5>& q2, + MatrixBase<D7>& p12, + QuaternionBase<D8>& q12) { - MatrixSizeCheck<3, 1>::check(p1); - MatrixSizeCheck<3, 1>::check(p2); - MatrixSizeCheck<3, 1>::check(p12); + MatrixSizeCheck<3, 1>::check(p1); + MatrixSizeCheck<3, 1>::check(p2); + MatrixSizeCheck<3, 1>::check(p12); - p12 = q1.conjugate() * ( p2 - p1 ); - q12 = q1.conjugate() * q2; + p12 = q1.conjugate() * (p2 - p1); + q12 = q1.conjugate() * q2; } -template<typename D1, typename D2, typename D3> -inline void between(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, - MatrixBase<D3>& d12) +template <typename D1, typename D2, typename D3> +inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d12) { MatrixSizeCheck<7, 1>::check(d1); MatrixSizeCheck<7, 1>::check(d2); MatrixSizeCheck<7, 1>::check(d12); - Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) ); - Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) ); - Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) ); - Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) ); - Map<Matrix<typename D3::Scalar, 3, 1> > p12 ( & d12(0) ); - Map<Quaternion<typename D3::Scalar> > q12 ( & d12(3) ); + Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0)); + Map<const Quaternion<typename D1::Scalar>> q1(&d1(3)); + Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0)); + Map<const Quaternion<typename D2::Scalar>> q2(&d2(3)); + Map<Matrix<typename D3::Scalar, 3, 1>> p12(&d12(0)); + Map<Quaternion<typename D3::Scalar>> q12(&d12(3)); between(p1, q1, p2, q2, p12, q12); } -template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2 ) +template <typename D1, typename D2> +inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2) { MatrixSizeCheck<7, 1>::check(d1); MatrixSizeCheck<7, 1>::check(d2); @@ -315,28 +317,28 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, return d12; } -template<typename Derived> +template <typename Derived> inline Matrix<typename Derived::Scalar, 6, 1> log(const MatrixBase<Derived>& delta_in) { MatrixSizeCheck<7, 1>::check(delta_in); Matrix<typename Derived::Scalar, 6, 1> ret; - Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & delta_in(0) ); - Map<const Quaternion<typename Derived::Scalar> > q_in ( & delta_in(3) ); - Map<Matrix<typename Derived::Scalar, 3, 1> > p_ret ( & ret(0) ); - Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) ); + Map<const Matrix<typename Derived::Scalar, 3, 1>> p_in(&delta_in(0)); + Map<const Quaternion<typename Derived::Scalar>> q_in(&delta_in(3)); + Map<Matrix<typename Derived::Scalar, 3, 1>> p_ret(&ret(0)); + Map<Matrix<typename Derived::Scalar, 3, 1>> do_ret(&ret(3)); Matrix<typename Derived::Scalar, 3, 3> V_inv; - do_ret = log_q(q_in); - V_inv = jac_SO3_left_inv(do_ret); + do_ret = log_q(q_in); + V_inv = jac_SO3_left_inv(do_ret); p_ret = V_inv * p_in; return ret; } -template<typename Derived> +template <typename Derived> inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_in) { MatrixSizeCheck<6, 1>::check(d_in); @@ -347,10 +349,10 @@ inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_i V = jac_SO3_left(d_in.template tail<3>()); - Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & d_in(0) ); - Map<const Matrix<typename Derived::Scalar, 3, 1> > o_in ( & d_in(3) ); - Map<Matrix<typename Derived::Scalar, 3, 1> > p ( & ret(0) ); - Map<Quaternion<typename Derived::Scalar> > q ( & ret(3) ); + Map<const Matrix<typename Derived::Scalar, 3, 1>> p_in(&d_in(0)); + Map<const Matrix<typename Derived::Scalar, 3, 1>> o_in(&d_in(3)); + Map<Matrix<typename Derived::Scalar, 3, 1>> p(&ret(0)); + Map<Quaternion<typename Derived::Scalar>> q(&ret(3)); p = V * p_in; q = exp_q(o_in); @@ -363,20 +365,23 @@ inline VectorComposite exp(const VectorComposite& tau) const auto& p = tau.at('P'); const auto& theta = tau.at('O'); - Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145) + Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145) - VectorComposite res; + VectorComposite res; - res['P'] = V * p ; - res['O'] = exp_q(theta).coeffs() ; + res['P'] = V * p; + res['O'] = exp_q(theta).coeffs(); return res; } -template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> -inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, - const MatrixBase<D4>& p2, const MatrixBase<D5>& o2, - MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q) +template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void plus(const MatrixBase<D1>& p1, + const QuaternionBase<D2>& q1, + const MatrixBase<D4>& p2, + const MatrixBase<D5>& o2, + MatrixBase<D7>& plus_p, + QuaternionBase<D8>& plus_q) { MatrixSizeCheck<3, 1>::check(p1); MatrixSizeCheck<3, 1>::check(p2); @@ -387,38 +392,38 @@ inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, plus_q = q1 * exp_q(o2); } -template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> -inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1, - const MatrixBase<D4>& p2, const MatrixBase<D5>& o2, - MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q) +template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void plus(const MatrixBase<D1>& p1, + const MatrixBase<D2>& q1, + const MatrixBase<D4>& p2, + const MatrixBase<D5>& o2, + MatrixBase<D7>& plus_p, + MatrixBase<D8>& plus_q) { MatrixSizeCheck<4, 1>::check(q1); MatrixSizeCheck<4, 1>::check(plus_q); - Map<const Quaterniond> q1m ( & q1 (0) ); - Map<Quaterniond> plus_qm ( & plus_q(0) ); + Map<const Quaterniond> q1m(&q1(0)); + Map<Quaterniond> plus_qm(&plus_q(0)); plus(p1, q1m, p2, o2, plus_p, plus_qm); } -template<typename D1, typename D2, typename D3> -inline void plus(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, - MatrixBase<D3>& d_plus) +template <typename D1, typename D2, typename D3> +inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d_plus) { - Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) ); - Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) ); - Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) ); - Map<const Matrix<typename D2::Scalar, 3, 1> > o2 ( & d2(3) ); - Map<Matrix<typename D3::Scalar, 3, 1> > p_p ( & d_plus(0) ); - Map<Quaternion<typename D3::Scalar> > q_p ( & d_plus(3) ); + Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0)); + Map<const Quaternion<typename D1::Scalar>> q1(&d1(3)); + Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0)); + Map<const Matrix<typename D2::Scalar, 3, 1>> o2(&d2(3)); + Map<Matrix<typename D3::Scalar, 3, 1>> p_p(&d_plus(0)); + Map<Quaternion<typename D3::Scalar>> q_p(&d_plus(3)); plus(p1, q1, p2, o2, p_p, q_p); } -template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2) +template <typename D1, typename D2> +inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2) { Matrix<typename D1::Scalar, 7, 1> d_plus; plus(d1, d2, d_plus); @@ -442,55 +447,60 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau return res; } -template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> -inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, - const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, - MatrixBase<D7>& pd, MatrixBase<D8>& od ) +template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void minus(const MatrixBase<D1>& p1, + const QuaternionBase<D2>& q1, + const MatrixBase<D4>& p2, + const QuaternionBase<D5>& q2, + MatrixBase<D7>& pd, + MatrixBase<D8>& od) { pd = p2 - p1; od = log_q(q1.conjugate() * q2); } -template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9> -inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, - const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, - MatrixBase<D6>& pd, MatrixBase<D7>& od, - MatrixBase<D8>& J_do_q1, MatrixBase<D9>& J_do_q2) +template <typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9> +inline void minus(const MatrixBase<D1>& p1, + const QuaternionBase<D2>& q1, + const MatrixBase<D4>& p2, + const QuaternionBase<D5>& q2, + MatrixBase<D6>& pd, + MatrixBase<D7>& od, + MatrixBase<D8>& J_do_q1, + MatrixBase<D9>& J_do_q2) { minus(p1, q1, p2, q2, pd, od); - J_do_q1 = - jac_SO3_left_inv(od); - J_do_q2 = jac_SO3_right_inv(od); + J_do_q1 = -jac_SO3_left_inv(od); + J_do_q2 = jac_SO3_right_inv(od); } -template<typename D1, typename D2, typename D3> -inline void minus(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, - MatrixBase<D3>& err) +template <typename D1, typename D2, typename D3> +inline void minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& err) { - Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) ); - Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) ); - Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) ); - Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) ); - Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & err(0) ); - Map<Matrix<typename D3::Scalar, 3, 1> > od ( & err(3) ); + Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0)); + Map<const Quaternion<typename D1::Scalar>> q1(&d1(3)); + Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0)); + Map<const Quaternion<typename D2::Scalar>> q2(&d2(3)); + Map<Matrix<typename D3::Scalar, 3, 1>> pd(&err(0)); + Map<Matrix<typename D3::Scalar, 3, 1>> od(&err(3)); minus(p1, q1, p2, q2, pd, od); } -template<typename D1, typename D2, typename D3, typename D4, typename D5> +template <typename D1, typename D2, typename D3, typename D4, typename D5> inline void minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, - MatrixBase<D3>& dif, - MatrixBase<D4>& J_diff_d1, - MatrixBase<D5>& J_diff_d2) + MatrixBase<D3>& dif, + MatrixBase<D4>& J_diff_d1, + MatrixBase<D5>& J_diff_d2) { - Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) ); - Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) ); - Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) ); - Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) ); - Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & dif(0) ); - Map<Matrix<typename D3::Scalar, 3, 1> > od ( & dif(3) ); + Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0)); + Map<const Quaternion<typename D1::Scalar>> q1(&d1(3)); + Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0)); + Map<const Quaternion<typename D2::Scalar>> q2(&d2(3)); + Map<Matrix<typename D3::Scalar, 3, 1>> pd(&dif(0)); + Map<Matrix<typename D3::Scalar, 3, 1>> od(&dif(3)); Matrix<typename D4::Scalar, 3, 3> J_do_q1, J_do_q2; @@ -504,27 +514,26 @@ inline void minus(const MatrixBase<D1>& d1, * J_do_q1 = - J_l_inv(theta) * J_do_q2 = J_r_inv(theta) */ - J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity - J_diff_d1.block(3,3,3,3) = J_do_q1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta) + J_diff_d1 = -Matrix<typename D4::Scalar, 6, 6>::Identity(); // d(p2 - p1) / d(p1) = - Identity + J_diff_d1.block(3, 3, 3, 3) = J_do_q1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta) - J_diff_d2.setIdentity(6,6); // d(R1.tr*R2) / d(R2) = Identity - J_diff_d2.block(3,3,3,3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta) + J_diff_d2.setIdentity(6, 6); // d(R1.tr*R2) / d(R2) = Identity + J_diff_d2.block(3, 3, 3, 3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta) } -template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2) +template <typename D1, typename D2> +inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2) { Matrix<typename D1::Scalar, 6, 1> ret; minus(d1, d2, ret); return ret; } -template<typename D1, typename D2, typename D3> -inline void interpolate(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, +template <typename D1, typename D2, typename D3> +inline void interpolate(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, const typename D1::Scalar& t, - MatrixBase<D3>& ret) + MatrixBase<D3>& ret) { Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2); @@ -533,87 +542,87 @@ inline void interpolate(const MatrixBase<D1>& d1, ret = compose(d1, exp(tau)); } -template<typename D1, typename D2> -inline void toSE3(const MatrixBase<D1>& pose, - MatrixBase<D2>& SE3) +template <typename D1, typename D2> +inline void toSE3(const MatrixBase<D1>& pose, MatrixBase<D2>& SE3) { - MatrixSizeCheck<4,4>::check(SE3); + MatrixSizeCheck<4, 4>::check(SE3); typedef typename D1::Scalar T; - SE3.template block<3,1>(0,3) = pose.template head<3>(); - SE3.template block<3,3>(0,0) = q2R(pose.template tail<4>()); - SE3.template block<1,3>(3,0).setZero(); - SE3(3,3) = (T)1.0; + SE3.template block<3, 1>(0, 3) = pose.template head<3>(); + SE3.template block<3, 3>(0, 0) = q2R(pose.template tail<4>()); + SE3.template block<1, 3>(3, 0).setZero(); + SE3(3, 3) = (T)1.0; } -template<typename D1, typename D2> +template <typename D1, typename D2> inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D1>& w) { typedef typename D1::Scalar T; - Matrix<T, 3, 3> V = skew(v); - Matrix<T, 3, 3> W = skew(w); - Matrix<T, 3, 3> VW = V * W; - Matrix<T, 3, 3> WV = VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!! + Matrix<T, 3, 3> V = skew(v); + Matrix<T, 3, 3> W = skew(w); + Matrix<T, 3, 3> VW = V * W; + Matrix<T, 3, 3> WV = + VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!! Matrix<T, 3, 3> WVW = WV * W; Matrix<T, 3, 3> VWW = VW * W; - T th_2 = w.squaredNorm(); + T th_2 = w.squaredNorm(); T A(T(0.5)), B, C, D; // Small angle approximation if (th_2 <= T(1e-8)) { - B = double(1./6.) + double(1./120.) * th_2; - C = -double(1./24.) + double(1./720.) * th_2; - D = -double(1./60.); + B = double(1. / 6.) + double(1. / 120.) * th_2; + C = -double(1. / 24.) + double(1. / 720.) * th_2; + D = -double(1. / 60.); } else { - T th = sqrt(th_2); - T th_3 = th_2*th; - T th_4 = th_2*th_2; - T th_5 = th_3*th_2; - T sin_th = sin(th); - T cos_th = cos(th); - B = (th-sin_th)/th_3; - C = (T(1.0) - th_2/T(2.0) - cos_th)/th_4; - D = (th - sin_th - th_3/T(6.0))/th_5; + T th = sqrt(th_2); + T th_3 = th_2 * th; + T th_4 = th_2 * th_2; + T th_5 = th_3 * th_2; + T sin_th = sin(th); + T cos_th = cos(th); + B = (th - sin_th) / th_3; + C = (T(1.0) - th_2 / T(2.0) - cos_th) / th_4; + D = (th - sin_th - th_3 / T(6.0)) / th_5; } Matrix<T, 3, 3> Q; Q.noalias() = - + A * V - + B * (WV + VW + WVW) - - C * (VWW - VWW.transpose() - double(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!! - - D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!! + +A * V + B * (WV + VW + WVW) - + C * (VWW - VWW.transpose() - + double(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!! + - D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!! return Q; } -template<typename D1> +template <typename D1> inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D1>& tangent) { typedef typename D1::Scalar T; - Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear - Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular + Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear + Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular Matrix<T, 3, 3> Jl(jac_SO3_left(w)); - Matrix<T, 3, 3> Q = Q_helper(v,w); + Matrix<T, 3, 3> Q = Q_helper(v, w); Matrix<T, 6, 6> Jl_SE3; - Jl_SE3.topLeftCorner(3,3) = Jl; - Jl_SE3.bottomRightCorner(3,3) = Jl; - Jl_SE3.topRightCorner(3,3) = Q; - Jl_SE3.bottomLeftCorner(3,3) .setZero(); + Jl_SE3.topLeftCorner(3, 3) = Jl; + Jl_SE3.bottomRightCorner(3, 3) = Jl; + Jl_SE3.topRightCorner(3, 3) = Q; + Jl_SE3.bottomLeftCorner(3, 3).setZero(); } -template<typename D1> +template <typename D1> inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D1>& tangent) { return jac_SE3_left(-tangent); } -} // namespace three_d -} // namespace wolf +} // namespace SE3 +} // namespace wolf diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h index e083cb4c75d2f367a9892668790b3c61dc9b0ac1..729549c276f1ab8f151bba8fcae8c1c03e551610 100644 --- a/include/core/math/covariance.h +++ b/include/core/math/covariance.h @@ -22,49 +22,45 @@ #include <Eigen/Dense> -namespace wolf{ - +namespace wolf +{ template <typename T, int N, int RC> -inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M, - const T eps = wolf::Constants::EPS) +inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M, const T eps = wolf::Constants::EPS) { - if (M.cols() != M.rows()) - return false; - return M.isApprox(M.transpose(), eps); + if (M.cols() != M.rows()) return false; + return M.isApprox(M.transpose(), eps); } template <typename T, int N, int RC> inline bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS) { - Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly); + Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly); - if (eigensolver.info() == Eigen::Success) - { - // All eigenvalues must be >= 0: - return (eigensolver.eigenvalues().array() >= eps).all(); - } - else - std::cout << "eigen decomposition failed" << std::endl; + if (eigensolver.info() == Eigen::Success) + { + // All eigenvalues must be >= 0: + return (eigensolver.eigenvalues().array() >= eps).all(); + } + else + std::cout << "eigen decomposition failed" << std::endl; - return false; + return false; } template <typename T, int N, int RC> inline bool isCovariance(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS) { - return isSymmetric(M) && isPositiveSemiDefinite(M, eps); + return isSymmetric(M) && isPositiveSemiDefinite(M, eps); } -#define WOLF_ASSERT_COVARIANCE_MATRIX(x) \ - assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance"); +#define WOLF_ASSERT_COVARIANCE_MATRIX(x) assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance"); -#define WOLF_ASSERT_INFORMATION_MATRIX(x) \ - assert(isCovariance(x, double(0.0)) && "Not an information matrix"); +#define WOLF_ASSERT_INFORMATION_MATRIX(x) assert(isCovariance(x, double(0.0)) && "Not an information matrix"); template <typename T, int N, int RC> -inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS) +inline bool makePosDef(Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS) { - Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T,N,N,RC> > eigensolver(M); + Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M); if (eigensolver.info() == Eigen::Success) { @@ -72,17 +68,16 @@ inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS double epsilon = eps; while ((eigensolver.eigenvalues().array() < eps).any()) { - //std::cout << "----- any negative eigenvalue or too close to zero\n"; - //std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl; - //std::cout << "previous determinant: " << M.determinant() << std::endl; - M = eigensolver.eigenvectors() * - eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() * + // std::cout << "----- any negative eigenvalue or too close to zero\n"; + // std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl; + // std::cout << "previous determinant: " << M.determinant() << std::endl; + M = eigensolver.eigenvectors() * eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() * eigensolver.eigenvectors().transpose(); eigensolver.compute(M); - //std::cout << "epsilon used: " << epsilon << std::endl; - //std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl; - //std::cout << "posterior determinant: " << M.determinant() << std::endl; - epsilon *=10; + // std::cout << "epsilon used: " << epsilon << std::endl; + // std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl; + // std::cout << "posterior determinant: " << M.determinant() << std::endl; + epsilon *= 10; } WOLF_ASSERT_COVARIANCE_MATRIX(M); @@ -94,26 +89,25 @@ inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS return false; } -inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd & _info) +inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd& _info) { // impose symmetry Eigen::MatrixXd info = _info.selfadjointView<Eigen::Upper>(); // Normal Cholesky factorization Eigen::LLT<Eigen::MatrixXd> llt_of_info(info); - Eigen::MatrixXd R = llt_of_info.matrixU(); + Eigen::MatrixXd R = llt_of_info.matrixU(); // Good factorization - if (info.isApprox(R.transpose() * R, Constants::EPS)) - return R; + if (info.isApprox(R.transpose() * R, Constants::EPS)) return R; // Not good factorization: SelfAdjointEigenSolver Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(info); - Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS); + Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS); R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose(); return R; } -} +} // namespace wolf diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h index 651bd5900b4bb85138bf85c272d31f63ea8cc93f..2823f74dfa3b36675c840c25063835c022e83170 100644 --- a/include/core/math/rotations.h +++ b/include/core/math/rotations.h @@ -24,18 +24,18 @@ namespace wolf { - ////////////////////////////////////////////////////////////// /** \brief Return angle between -pi and pi - * This implementation uses std::remainder which makes it incompatible for ceres::jet (this implementation was included in ceres_wrapper/wolf_jet.h) + * This implementation uses std::remainder which makes it incompatible for ceres::jet (this implementation was included + * in ceres_wrapper/wolf_jet.h) * @param angle * @return formatted angle */ -template<typename T> +template <typename T> inline T pi2pi(const T& _angle) { - return remainder(_angle,2*M_PI); + return remainder(_angle, 2 * M_PI); } /** \brief Conversion to radians @@ -43,9 +43,8 @@ inline T pi2pi(const T& _angle) * @param deg angle in degrees * @return angle in radians */ -template<typename T> -inline T -toRad(const T deg) +template <typename T> +inline T toRad(const T deg) { return (T)M_TORAD * deg; } @@ -55,9 +54,8 @@ toRad(const T deg) * @param rad angle in radians * @return angle in degrees */ -template<typename T> -inline T -toDeg(const T rad) +template <typename T> +inline T toDeg(const T rad) { return (T)M_TODEG * rad; } @@ -70,19 +68,16 @@ toDeg(const T rad) * @param _v a 3d vector * @return the skew-symmetric matrix V so that V*u = _v.cross(u), for u in R^3 */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -skew(const Eigen::MatrixBase<Derived>& _v) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skew(const Eigen::MatrixBase<Derived>& _v) { - MatrixSizeCheck<3,1>::check(_v); + MatrixSizeCheck<3, 1>::check(_v); typedef typename Derived::Scalar T; Eigen::Matrix<T, 3, 3> sk; - sk << (T)0.0 , -_v(2), +_v(1), - +_v(2), (T)0.0, -_v(0), - -_v(1), +_v(0), (T)0.0; + sk << (T)0.0, -_v(2), +_v(1), +_v(2), (T)0.0, -_v(0), -_v(1), +_v(0), (T)0.0; return sk; } @@ -92,11 +87,10 @@ skew(const Eigen::MatrixBase<Derived>& _v) * @param _m A skew-symmetric matrix * @return a 3-vector v such that skew(v) = _m */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 1> -vee(const Eigen::MatrixBase<Derived>& _m) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 1> vee(const Eigen::MatrixBase<Derived>& _m) { - MatrixSizeCheck<3,3>::check(_m); + MatrixSizeCheck<3, 3>::check(_m); typedef typename Derived::Scalar T; @@ -113,9 +107,8 @@ vee(const Eigen::MatrixBase<Derived>& _m) * @param _q a right-handed unit quaternion * @return the equivalent rotation matrix */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -q2R(const Eigen::QuaternionBase<Derived>& _q) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::QuaternionBase<Derived>& _q) { return _q.matrix(); } @@ -125,13 +118,12 @@ q2R(const Eigen::QuaternionBase<Derived>& _q) * @param _q a right-handed unit quaternion * @return the equivalent rotation matrix */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -q2R(const Eigen::MatrixBase<Derived>& _q) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::MatrixBase<Derived>& _q) { - MatrixSizeCheck<4,1>::check(_q); - Eigen::Quaternion<typename Derived::Scalar> q(_q(3),_q(0),_q(1),_q(2)); - return q2R( q ); + MatrixSizeCheck<4, 1>::check(_q); + Eigen::Quaternion<typename Derived::Scalar> q(_q(3), _q(0), _q(1), _q(2)); + return q2R(q); } /** \brief rotation matrix to quaternion conversion @@ -139,11 +131,10 @@ q2R(const Eigen::MatrixBase<Derived>& _q) * @param _R a rotation matrix * @return the equivalent right-handed unit quaternion */ -template<typename Derived> -inline Eigen::Quaternion<typename Derived::Scalar> -R2q(const Eigen::MatrixBase<Derived>& _R) +template <typename Derived> +inline Eigen::Quaternion<typename Derived::Scalar> R2q(const Eigen::MatrixBase<Derived>& _R) { - MatrixSizeCheck<3,3>::check(_R); + MatrixSizeCheck<3, 3>::check(_R); return Eigen::Quaternion<typename Derived::Scalar>(_R); } @@ -152,11 +143,10 @@ R2q(const Eigen::MatrixBase<Derived>& _R) * \param _euler = (roll pitch yaw) in ZYX convention * \return equivalent quaternion */ -template<typename D> -inline Eigen::Quaternion<typename D::Scalar> -e2q(const Eigen::MatrixBase<D>& _euler) +template <typename D> +inline Eigen::Quaternion<typename D::Scalar> e2q(const Eigen::MatrixBase<D>& _euler) { - MatrixSizeCheck<3,1>::check(_euler); + MatrixSizeCheck<3, 1>::check(_euler); typedef typename D::Scalar T; @@ -171,9 +161,8 @@ e2q(const Eigen::MatrixBase<D>& _euler) * \param _e = (roll pitch yaw) in ZYX convention * \return equivalent rotation matrix */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -e2R(const Eigen::MatrixBase<Derived>& _e) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> e2R(const Eigen::MatrixBase<Derived>& _e) { MatrixSizeCheck<3, 1>::check(_e); @@ -181,66 +170,62 @@ e2R(const Eigen::MatrixBase<Derived>& _e) } template <typename Derived> -inline typename Eigen::MatrixBase<Derived>::Scalar -getYaw(const Eigen::MatrixBase<Derived>& R) +inline typename Eigen::MatrixBase<Derived>::Scalar getYaw(const Eigen::MatrixBase<Derived>& R) { MatrixSizeCheck<3, 3>::check(R); using std::atan2; - return atan2( R(1, 0), R(0, 0) ); + return atan2(R(1, 0), R(0, 0)); } -template<typename Derived> -inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> -R2e(const Eigen::MatrixBase<Derived>& _R) +template <typename Derived> +inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> R2e(const Eigen::MatrixBase<Derived>& _R) { Eigen::Matrix<typename Derived::Scalar, 3, 1> e; - e(0) = atan2( _R(2,1), _R(2,2)); - e(1) = atan2(-_R(2,0), sqrt(_R(0,0)*_R(0,0) + _R(1,0)*_R(1,0))); - e(2) = atan2( _R(1,0), _R(0,0)); + e(0) = atan2(_R(2, 1), _R(2, 2)); + e(1) = atan2(-_R(2, 0), sqrt(_R(0, 0) * _R(0, 0) + _R(1, 0) * _R(1, 0))); + e(2) = atan2(_R(1, 0), _R(0, 0)); return e; } -template<typename Derived> -inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> -q2e(const Eigen::MatrixBase<Derived>& _q) +template <typename Derived> +inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> q2e(const Eigen::MatrixBase<Derived>& _q) { typedef typename Derived::Scalar T; - Eigen::Matrix<T, 3, 1> e; - - double w = _q(3); - double x = _q(0); - double y = _q(1); - double z = _q(2); - - double xx = x*x; - double xy = x*y; - double xz = x*z; - double yy = y*y; - double yz = y*z; - double zz = z*z; - double wx = w*x; - double wy = w*y; - double wz = w*z; - - double r00 = T(1) - T(2) * ( yy + zz ); - double r10 = T(2) * ( xy + wz ); - double r20 = T(2) * ( xz - wy ); - double r21 = T(2) * ( yz + wx ); - double r22 = T(1) - T(2) * ( xx + yy ); - - e(0) = atan2( r21, r22); - e(1) = atan2(-r20, sqrt(r00*r00 + r10*r10)); - e(2) = atan2( r10, r00); + Eigen::Matrix<T, 3, 1> e; + + double w = _q(3); + double x = _q(0); + double y = _q(1); + double z = _q(2); + + double xx = x * x; + double xy = x * y; + double xz = x * z; + double yy = y * y; + double yz = y * z; + double zz = z * z; + double wx = w * x; + double wy = w * y; + double wz = w * z; + + double r00 = T(1) - T(2) * (yy + zz); + double r10 = T(2) * (xy + wz); + double r20 = T(2) * (xz - wy); + double r21 = T(2) * (yz + wx); + double r22 = T(1) - T(2) * (xx + yy); + + e(0) = atan2(r21, r22); + e(1) = atan2(-r20, sqrt(r00 * r00 + r10 * r10)); + e(2) = atan2(r10, r00); return e; } -template<typename Derived> -inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> -q2e(const Eigen::QuaternionBase<Derived>& _q) +template <typename Derived> +inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> q2e(const Eigen::QuaternionBase<Derived>& _q) { return q2e(_q.coeffs()); } @@ -253,28 +238,27 @@ q2e(const Eigen::QuaternionBase<Derived>& _q) * @param _v a rotation vector with _v.norm() the rotated angle and _v.normalized() the rotation axis. * @return the right-handed unit quaternion performing the rotation encoded by _v */ -template<typename Derived> -inline Eigen::Quaternion<typename Derived::Scalar> -exp_q(const Eigen::MatrixBase<Derived>& _v) +template <typename Derived> +inline Eigen::Quaternion<typename Derived::Scalar> exp_q(const Eigen::MatrixBase<Derived>& _v) { - using std::sqrt; using std::cos; using std::sin; + using std::sqrt; - MatrixSizeCheck<3,1>::check(_v); + MatrixSizeCheck<3, 1>::check(_v); typedef typename Derived::Scalar T; T angle_squared = _v.squaredNorm(); - T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version. + T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version. if (angle > (T)(wolf::Constants::EPS_SMALL)) { - return Eigen::Quaternion<T> ( Eigen::AngleAxis<T>(angle, _v.normalized()) ); + return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, _v.normalized())); } else { - return Eigen::Quaternion<T> ( (T)1.0 , _v(0,0)/(T)2 , _v(1,0)/(T)2 , _v(2,0)/(T)2 ); + return Eigen::Quaternion<T>((T)1.0, _v(0, 0) / (T)2, _v(1, 0) / (T)2, _v(2, 0) / (T)2); } } @@ -283,11 +267,9 @@ exp_q(const Eigen::MatrixBase<Derived>& _v) * @param _q a unit right-handed quaternion * @return a rotation vector v such that _q = exp_q(v) */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 1> -log_q(const Eigen::QuaternionBase<Derived>& _q) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_q(const Eigen::QuaternionBase<Derived>& _q) { - // Will try this implementation once Eigen accepts it! // see https://forum.kde.org/viewtopic.php?f=74&t=143269&p=385299#p385265 // typedef typename Derived::Scalar T; @@ -298,11 +280,11 @@ log_q(const Eigen::QuaternionBase<Derived>& _q) typedef typename Derived::Scalar T; - Eigen::Matrix<T, 3, 1> vec = _q.vec(); - const T sin_angle_squared = vec.squaredNorm(); + Eigen::Matrix<T, 3, 1> vec = _q.vec(); + const T sin_angle_squared = vec.squaredNorm(); if (sin_angle_squared > (T)wolf::Constants::EPS_SMALL) { - const T sin_angle = sqrt(sin_angle_squared); // Allow ceres::Jet to use its own sqrt() version. + const T sin_angle = sqrt(sin_angle_squared); // Allow ceres::Jet to use its own sqrt() version. const T& cos_angle = _q.w(); /* If (cos_angle < 0) then angle >= pi/2 , means : angle for angle_axis vector >= pi (== 2*angle) @@ -314,7 +296,8 @@ log_q(const Eigen::QuaternionBase<Derived>& _q) angle - pi = atan(sin(angle - pi), cos(angle - pi)) = atan(-sin(angle), -cos(angle)) */ - const T two_angle = T(2.0) * ((cos_angle < T(0.0)) ? atan2(-sin_angle, -cos_angle) : atan2(sin_angle, cos_angle)); + const T two_angle = + T(2.0) * ((cos_angle < T(0.0)) ? atan2(-sin_angle, -cos_angle) : atan2(sin_angle, cos_angle)); const T k = two_angle / sin_angle; return vec * k; } @@ -330,9 +313,8 @@ log_q(const Eigen::QuaternionBase<Derived>& _q) * @param _v a rotation vector * @return the rotation matrix performing the rotation encoded by _v */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -exp_R(const Eigen::MatrixBase<Derived>& _v) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBase<Derived>& _v) { using std::sqrt; @@ -341,7 +323,7 @@ exp_R(const Eigen::MatrixBase<Derived>& _v) typedef typename Derived::Scalar T; T angle_squared = _v.squaredNorm(); - T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version. + T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version. if (angle > wolf::Constants::EPS_SMALL) return Eigen::AngleAxis<T>(angle, _v.normalized()).toRotationMatrix(); @@ -354,9 +336,8 @@ exp_R(const Eigen::MatrixBase<Derived>& _v) * @param _R a 3d rotation matrix * @return the rotation vector v such that _R = exp_R(v) */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 1> -log_R(const Eigen::MatrixBase<Derived>& _R) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_R(const Eigen::MatrixBase<Derived>& _R) { return log_q(R2q(_R)); } @@ -366,9 +347,8 @@ log_R(const Eigen::MatrixBase<Derived>& _R) * @param _v a rotation vector * @return the equivalent right-handed unit quaternion */ -template<typename Derived> -inline Eigen::Quaternion<typename Derived::Scalar> -v2q(const Eigen::MatrixBase<Derived>& _v) +template <typename Derived> +inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<Derived>& _v) { return exp_q(_v); } @@ -378,9 +358,8 @@ v2q(const Eigen::MatrixBase<Derived>& _v) * @param _q a right-handed unit quaternion * @return the equivalent rotation vector */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 1> -q2v(const Eigen::QuaternionBase<Derived>& _q) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 1> q2v(const Eigen::QuaternionBase<Derived>& _q) { return log_q(_q); } @@ -390,9 +369,8 @@ q2v(const Eigen::QuaternionBase<Derived>& _q) * @param _v a rotation vector * @return the equivalent rotation matrix */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -v2R(const Eigen::MatrixBase<Derived>& _v) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> v2R(const Eigen::MatrixBase<Derived>& _v) { return exp_R(_v); } @@ -402,9 +380,8 @@ v2R(const Eigen::MatrixBase<Derived>& _v) * @param _R a rotation matrix * @return the equivalent rotation vector */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 1> -R2v(const Eigen::MatrixBase<Derived>& _R) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 1> R2v(const Eigen::MatrixBase<Derived>& _R) { return log_R(_R); } @@ -426,23 +403,22 @@ R2v(const Eigen::MatrixBase<Derived>& _R) * exp(theta+d_theta) = exp(theta)*exp(Jr(theta)*d_theta) */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta) { - using std::sqrt; using std::cos; using std::sin; + using std::sqrt; MatrixSizeCheck<3, 1>::check(_theta); typedef typename Derived::Scalar T; - T theta2 = _theta.squaredNorm(); + T theta2 = _theta.squaredNorm(); Eigen::Matrix<T, 3, 3> W(skew(_theta)); if (theta2 <= Constants::EPS_SMALL) - return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation - T theta = sqrt(theta2); // rotation angle + return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation + T theta = sqrt(theta2); // rotation angle Eigen::Matrix<T, 3, 3> M1, M2; M1.noalias() = ((T)1 - cos(theta)) / theta2 * W; M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W); @@ -458,7 +434,8 @@ jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta) * * where Jrinv = jac_SO3_right_inv(theta); * - * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jrinv * theta) so that + * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jrinv * theta) so + * that * * log( exp(theta) * exp(d_theta) ) = theta + Jrinv(theta) * d_theta * @@ -466,26 +443,25 @@ jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta) * * log( R * exp(d_theta) ) = log(R) + Jrinv(theta) * d_theta */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta) { - using std::sqrt; using std::cos; using std::sin; + using std::sqrt; MatrixSizeCheck<3, 1>::check(_theta); typedef typename Derived::Scalar T; - T theta2 = _theta.squaredNorm(); + T theta2 = _theta.squaredNorm(); Eigen::Matrix<T, 3, 3> W(skew(_theta)); if (theta2 <= Constants::EPS_SMALL) - return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation - T theta = sqrt(theta2); // rotation angle + return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation + T theta = sqrt(theta2); // rotation angle Eigen::Matrix<T, 3, 3> M; M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W); - return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; //is this really more optimized? + return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; // is this really more optimized? } /** \brief Compute Jl (Left Jacobian) @@ -501,23 +477,22 @@ jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta) * * exp(theta+d_theta) = exp(Jl(theta)*d_theta)*exp(theta) */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta) { - using std::sqrt; using std::cos; using std::sin; + using std::sqrt; MatrixSizeCheck<3, 1>::check(_theta); typedef typename Derived::Scalar T; - T theta2 = _theta.squaredNorm(); + T theta2 = _theta.squaredNorm(); Eigen::Matrix<T, 3, 3> W(skew(_theta)); if (theta2 <= Constants::EPS_SMALL) - return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation - T theta = sqrt(theta2); // rotation angle + return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation + T theta = sqrt(theta2); // rotation angle Eigen::Matrix<T, 3, 3> M1, M2; M1.noalias() = ((T)1 - cos(theta)) / theta2 * W; M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W); @@ -532,7 +507,8 @@ jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta) * * where Jlinv = jac_SO3_left_inv(theta); * - * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jlinv * theta) so that + * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jlinv * theta) so + * that * * log( exp(d_theta) * exp(theta) ) = theta + Jlinv(theta) * d_theta * @@ -540,111 +516,107 @@ jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta) * * log( exp(d_theta) * R ) = log(R) + Jlinv(theta) * d_theta */ -template<typename Derived> -inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta) +template <typename Derived> +inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta) { - using std::sqrt; using std::cos; using std::sin; + using std::sqrt; MatrixSizeCheck<3, 1>::check(_theta); typedef typename Derived::Scalar T; - T theta2 = _theta.squaredNorm(); + T theta2 = _theta.squaredNorm(); Eigen::Matrix<T, 3, 3> W(skew(_theta)); if (theta2 <= Constants::EPS_SMALL) - return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation - T theta = sqrt(theta2); // rotation angle + return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation + T theta = sqrt(theta2); // rotation angle Eigen::Matrix<T, 3, 3> M; M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W); - return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; //is this really more optimized? + return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; // is this really more optimized? } -template<typename D1, typename D2, typename D3, typename D4, typename D5> -inline void -compose(const Eigen::QuaternionBase<D1>& _q1, - const Eigen::QuaternionBase<D2>& _q2, - Eigen::QuaternionBase<D3>& _q_comp, - Eigen::MatrixBase<D4>& _J_comp_q1, - Eigen::MatrixBase<D5>& _J_comp_q2) +template <typename D1, typename D2, typename D3, typename D4, typename D5> +inline void compose(const Eigen::QuaternionBase<D1>& _q1, + const Eigen::QuaternionBase<D2>& _q2, + Eigen::QuaternionBase<D3>& _q_comp, + Eigen::MatrixBase<D4>& _J_comp_q1, + Eigen::MatrixBase<D5>& _J_comp_q2) { MatrixSizeCheck<3, 3>::check(_J_comp_q1); MatrixSizeCheck<3, 3>::check(_J_comp_q2); _q_comp = _q1 * _q2; - _J_comp_q1 = q2R(_q2.conjugate()); // R2.tr - _J_comp_q2 . setIdentity(); + _J_comp_q1 = q2R(_q2.conjugate()); // R2.tr + _J_comp_q2.setIdentity(); } -template<typename D1, typename D2, typename D3, typename D4, typename D5> -inline void -between(const Eigen::QuaternionBase<D1>& _q1, - const Eigen::QuaternionBase<D2>& _q2, - Eigen::QuaternionBase<D3>& _q_between, - Eigen::MatrixBase<D4>& _J_between_q1, - Eigen::MatrixBase<D5>& _J_between_q2) +template <typename D1, typename D2, typename D3, typename D4, typename D5> +inline void between(const Eigen::QuaternionBase<D1>& _q1, + const Eigen::QuaternionBase<D2>& _q2, + Eigen::QuaternionBase<D3>& _q_between, + Eigen::MatrixBase<D4>& _J_between_q1, + Eigen::MatrixBase<D5>& _J_between_q2) { MatrixSizeCheck<3, 3>::check(_J_between_q1); MatrixSizeCheck<3, 3>::check(_J_between_q2); _q_between = _q1.conjugate() * _q2; - _J_between_q1 = -q2R(_q2.conjugate()*_q1); // - R2.tr * R1 - _J_between_q2 . setIdentity(); + _J_between_q1 = -q2R(_q2.conjugate() * _q1); // - R2.tr * R1 + _J_between_q2.setIdentity(); } -template<typename D1, typename D2> -inline Eigen::Quaternion<typename D1::Scalar> -plus_right(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v) +template <typename D1, typename D2> +inline Eigen::Quaternion<typename D1::Scalar> plus_right(const Eigen::QuaternionBase<D1>& q, + const Eigen::MatrixBase<D2>& v) { - MatrixSizeCheck<3,1>::check(v); + MatrixSizeCheck<3, 1>::check(v); return q * exp_q(v); } -template<typename D1, typename D2> -inline Eigen::Matrix<typename D2::Scalar, 3, 1> -minus_right(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) +template <typename D1, typename D2> +inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus_right(const Eigen::QuaternionBase<D1>& q1, + const Eigen::QuaternionBase<D2>& q2) { return log_q(q1.conjugate() * q2); } -template<typename D1, typename D2> -inline Eigen::Quaternion<typename D1::Scalar> -plus_left(const Eigen::MatrixBase<D2>& v, const Eigen::QuaternionBase<D1>& q) +template <typename D1, typename D2> +inline Eigen::Quaternion<typename D1::Scalar> plus_left(const Eigen::MatrixBase<D2>& v, + const Eigen::QuaternionBase<D1>& q) { - MatrixSizeCheck<3,1>::check(v); + MatrixSizeCheck<3, 1>::check(v); return exp_q(v) * q; } -template<typename D1, typename D2> -inline Eigen::Matrix<typename D2::Scalar, 3, 1> -minus_left(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) +template <typename D1, typename D2> +inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus_left(const Eigen::QuaternionBase<D1>& q1, + const Eigen::QuaternionBase<D2>& q2) { return log_q(q2 * q1.conjugate()); } -template<typename D1, typename D2> -inline Eigen::Quaternion<typename D1::Scalar> -plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v) +template <typename D1, typename D2> +inline Eigen::Quaternion<typename D1::Scalar> plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v) { return plus_right(q, v); } -template<typename D1, typename D2> -inline Eigen::Matrix<typename D2::Scalar, 3, 1> -minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) +template <typename D1, typename D2> +inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus(const Eigen::QuaternionBase<D1>& q1, + const Eigen::QuaternionBase<D2>& q2) { return minus_right(q1, q2); } -template<typename D1, typename D2> -inline Eigen::Matrix<typename D2::Scalar, 3, 1> -diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) +template <typename D1, typename D2> +inline Eigen::Matrix<typename D2::Scalar, 3, 1> diff(const Eigen::QuaternionBase<D1>& q1, + const Eigen::QuaternionBase<D2>& q2) { return minus(q1, q2); } -} // namespace wolf +} // namespace wolf diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 6d02629929f93efc5ab756411aa5bbd37acda7e4..07391d09f41ab7845fecadb38c30a86ac5b21581 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -180,7 +180,7 @@ class Problem : public std::enable_shared_from_this<Problem> * \param _folders_schema a vector of paths where the schema files (to validate the YAML file) are placed */ ProcessorBasePtr installProcessor(const std::string& _prc_type, - SensorBasePtr _corresponding_sensor, + SensorBasePtr _corresponding_sensor, const std::string& _params_yaml_filename, const std::vector<std::string>& _folders_schema); diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h index 3422c8361d0ae17ab1d9c6f42b4c600cbc0ea7d9..985152c4a3e9239ecfb31031ddb8274a7b87f076 100644 --- a/include/core/processor/factory_processor.h +++ b/include/core/processor/factory_processor.h @@ -55,8 +55,8 @@ namespace wolf * * \code * static ProcessorBasePtr create(const YAML::Node& _server); - * static ProcessorBasePtr create(const std::string& _schema, - * const std::string& _yaml_filepath, + * static ProcessorBasePtr create(const std::string& _schema, + * const std::string& _yaml_filepath, * std::vector<std::string>& _folders_schema); * \endcode * @@ -67,26 +67,26 @@ namespace wolf * { * // Do create the Processor Parameters --- example: ParamsProcessorOdom3d * auto params = std::make_shared<ParamsProcessorOdom3d>(_server); - * + * * // Do create the Processor object --- example: ProcessorOdom3d * return std::make_shared<ProcessorOdom3d>(params); * } - * static ProcessorBasePtr create(const std::string& _schema, - * const std::string& _yaml_filepath, + * static ProcessorBasePtr create(const std::string& _schema, + * const std::string& _yaml_filepath, * std::vector<std::string>& _folders_schema) * { * // parse the yaml file * auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); - * + * * // Check that the yaml has all necessary inforamtion * if (not server.applySchema(_schema)) * { * throw std::runtime_error(server.getLog()); * } - * + * * // Do create the Processor Parameters --- example: ParamsProcessorOdom3d * auto params = std::make_shared<ParamsProcessorOdom3d>(server.getNode()); - * + * * // Do create the Processor object --- example: ProcessorOdom3d * return std::make_shared<ProcessorOdom3d>(params); * } @@ -105,35 +105,38 @@ namespace wolf * or: * * \code - * auto camera_ptr = FactoryProcessorYaml::create("ProcessorOdom2d", "path_of_params_file.yaml", schema_folders_vector); - * \endcode - * - * We RECOMMEND using the macro ````WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass)```` to automatically + * auto camera_ptr = FactoryProcessorYaml::create("ProcessorOdom2d", "path_of_params_file.yaml", + * schema_folders_vector); \endcode + * + * We RECOMMEND using the macro ````WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass)```` to automatically * generate the processor creators, provided in 'processor_base.h'. * */ -typedef Factory<ProcessorBasePtr, - const YAML::Node&, - const std::vector<std::string>&> FactoryProcessorNode; -template<> +typedef Factory<ProcessorBasePtr, const YAML::Node&, const std::vector<std::string>&> FactoryProcessorNode; +template <> inline std::string FactoryProcessorNode::getClass() const { return "FactoryProcessor"; } -typedef Factory<std::shared_ptr<ProcessorBase>, - const std::string&, - const std::vector<std::string>&> FactoryProcessorYaml; -template<> +typedef Factory<std::shared_ptr<ProcessorBase>, const std::string&, const std::vector<std::string>&> + FactoryProcessorYaml; +template <> inline std::string FactoryProcessorYaml::getClass() const { return "FactoryProcessorYaml"; } -#define WOLF_REGISTER_PROCESSOR(ProcessorType) \ - namespace{ const bool WOLF_UNUSED ProcessorType##NodeRegistered = \ - wolf::FactoryProcessorNode::registerCreator(#ProcessorType, ProcessorType::create); } \ - namespace{ const bool WOLF_UNUSED ProcessorType##YamlRegistered = \ - wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); } \ +#define WOLF_REGISTER_PROCESSOR(ProcessorType) \ + namespace \ + { \ + const bool WOLF_UNUSED ProcessorType##NodeRegistered = \ + wolf::FactoryProcessorNode::registerCreator(#ProcessorType, ProcessorType::create); \ + } \ + namespace \ + { \ + const bool WOLF_UNUSED ProcessorType##YamlRegistered = \ + wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); \ + } } /* namespace wolf */ diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h index e192871d18f5158622c546a4d269afc960599b5c..c2109c9b6bc163dccf4f7fe340f1d7ec47434460 100644 --- a/include/core/processor/motion_buffer.h +++ b/include/core/processor/motion_buffer.h @@ -26,42 +26,47 @@ #include <list> #include <algorithm> -namespace wolf { - +namespace wolf +{ using namespace Eigen; struct Motion { - public: - SizeEigen data_size_, delta_size_, cov_size_, calib_size_; - TimeStamp ts_; ///< Time stamp - Eigen::VectorXd data_; ///< instantaneous motion data - Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data - Eigen::VectorXd delta_; ///< instantaneous motion delta - Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta - Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral - Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta - Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_ - Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_ - Eigen::MatrixXd jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors) - public: - Motion() = delete; // completely delete unpredictable stuff like this - Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size); - Motion(const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _delta, - const MatrixXd& _delta_cov, - const VectorXd& _delta_int, - const MatrixXd& _delta_integr_cov, - const MatrixXd& _jac_delta, - const MatrixXd& _jac_delta_int, - const MatrixXd& _jacobian_calib); - ~Motion(); - private: - void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s); + public: + SizeEigen data_size_, delta_size_, cov_size_, calib_size_; + TimeStamp ts_; ///< Time stamp + Eigen::VectorXd data_; ///< instantaneous motion data + Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data + Eigen::VectorXd delta_; ///< instantaneous motion delta + Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta + Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral + Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta + Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_ + Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_ + Eigen::MatrixXd jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors) + public: + Motion() = delete; // completely delete unpredictable stuff like this + Motion(const TimeStamp& _ts, + SizeEigen _data_size, + SizeEigen _delta_size, + SizeEigen _cov_size, + SizeEigen _calib_size); + Motion(const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _delta, + const MatrixXd& _delta_cov, + const VectorXd& _delta_int, + const MatrixXd& _delta_integr_cov, + const MatrixXd& _jac_delta, + const MatrixXd& _jac_delta_int, + const MatrixXd& _jacobian_calib); + ~Motion(); + + private: + void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s); -}; ///< One instance of the buffered data, corresponding to a particular time stamp. +}; ///< One instance of the buffered data, corresponding to a particular time stamp. /** \brief class for motion buffers. * @@ -86,12 +91,16 @@ struct Motion */ class MotionBuffer : public std::list<Motion> { - public: - MotionBuffer() ; - const Motion& getMotion(const TimeStamp& _ts) const; - void getMotion(const TimeStamp& _ts, Motion& _motion) const; - void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer); - void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0, bool show_covs = 0); + public: + MotionBuffer(); + const Motion& getMotion(const TimeStamp& _ts) const; + void getMotion(const TimeStamp& _ts, Motion& _motion) const; + void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer); + void print(bool show_data = 0, + bool show_delta = 0, + bool show_delta_int = 0, + bool show_jacs = 0, + bool show_covs = 0); }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h index 71c064d00153c30f75e608836ca744eea766d707..49d1651b3578155b90d0fe60f7fd50ad6d4d0468 100644 --- a/include/core/processor/motion_provider.h +++ b/include/core/processor/motion_provider.h @@ -33,54 +33,58 @@ WOLF_PTR_TYPEDEFS(MotionProvider); class MotionProvider { - public: - - MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params); - virtual ~MotionProvider(); - - // Queries to the processor: - virtual TimeStamp getTimeStamp() const = 0; - virtual VectorComposite getState(const StateKeys& _structure = "") const = 0; - virtual VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const = 0; - - VectorComposite getOdometry ( ) const; - void setOdometry(const VectorComposite&); - - bool isStateGetter() const; - int getOrder() const; - void setOrder(int); - - public: - TypeComposite getStateTypes ( ) const { return state_types_; }; - StateKeys getStateKeys ( ) const { return state_types_.getKeys(); }; - void setStateTypes(const TypeComposite& _state_structure) { state_types_ = _state_structure; }; - void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr); - - protected: - TypeComposite state_types_; - YAML::Node params_motion_provider_; - - private: - VectorComposite odometry_; - mutable std::mutex mut_odometry_; - + public: + MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params); + virtual ~MotionProvider(); + + // Queries to the processor: + virtual TimeStamp getTimeStamp() const = 0; + virtual VectorComposite getState(const StateKeys& _structure = "") const = 0; + virtual VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const = 0; + + VectorComposite getOdometry() const; + void setOdometry(const VectorComposite&); + + bool isStateGetter() const; + int getOrder() const; + void setOrder(int); + + public: + TypeComposite getStateTypes() const + { + return state_types_; + }; + StateKeys getStateKeys() const + { + return state_types_.getKeys(); + }; + void setStateTypes(const TypeComposite& _state_structure) + { + state_types_ = _state_structure; + }; + void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr); + + protected: + TypeComposite state_types_; + YAML::Node params_motion_provider_; + + private: + VectorComposite odometry_; + mutable std::mutex mut_odometry_; }; -inline MotionProvider::MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params) : - state_types_(_state_types), - params_motion_provider_(Clone(_params)) +inline MotionProvider::MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params) + : state_types_(_state_types), params_motion_provider_(Clone(_params)) { checkTypeComposite(_state_types); } -} +} // namespace wolf ///// IMPLEMENTATION /////// -namespace wolf{ - -inline MotionProvider::~MotionProvider() +namespace wolf { -} +inline MotionProvider::~MotionProvider() {} inline bool MotionProvider::isStateGetter() const { diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 1ac7c1cd1134d610cc17e147c9350dcfea5b7ebe..20b548aa44426ef7e7323d3638b9f733ee70c673 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -46,7 +46,6 @@ class SensorBase; namespace wolf { - /* * Macro for defining Autoconf processor creator for WOLF's high level API. * @@ -114,7 +113,7 @@ namespace wolf } \ return create(server.getNode(), {}); \ } - + /** \brief Buffer for arbitrary type objects * * Object and functions to manage a buffer of objects. diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h index bf6d0e402ec239bf3503a0d18ca8194e1eeca8f1..e7fd6fd01eab8bf8d07bc87557d5de0adc4a6bc7 100644 --- a/include/core/processor/processor_diff_drive.h +++ b/include/core/processor/processor_diff_drive.h @@ -24,7 +24,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorDiffDrive); class ProcessorDiffDrive : public ProcessorOdom2d diff --git a/include/core/processor/processor_fixed_wing_model.h b/include/core/processor/processor_fixed_wing_model.h index 27a1c8f0c6db671be83cfab82ff736269fd946db..2fa506a34adc673cba0575b4418d968cbd695857 100644 --- a/include/core/processor/processor_fixed_wing_model.h +++ b/include/core/processor/processor_fixed_wing_model.h @@ -24,54 +24,67 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorFixedWingModel); class ProcessorFixedWingModel : public ProcessorBase { - public: - ProcessorFixedWingModel(const YAML::Node& _params); - - // Factory method for high level API - WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel); - - virtual ~ProcessorFixedWingModel() override; - void configure(SensorBasePtr _sensor) override; - - protected: - - /** \brief process an incoming capture NEVER CALLED - */ - virtual void processCapture(CaptureBasePtr) override {}; - - /** \brief process an incoming key-frame: applies the motion model between consecutive keyframes - */ - virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) override; - - /** \brief trigger in capture - */ - virtual bool triggerInCapture(CaptureBasePtr) const override {return false;}; - - /** \brief trigger in key-frame - */ - virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return true;}; - - /** \brief store key frame - */ - virtual bool storeKeyFrame(FrameBasePtr) override {return false;}; - - /** \brief store capture - */ - virtual bool storeCapture(CaptureBasePtr) override {return false;}; - - /** \brief Vote for KeyFrame generation - */ - virtual bool voteForKeyFrame() const override {return false;}; - - // Params - Eigen::Vector3d velocity_local_; - double angle_stdev_; - double min_vel_norm_; + public: + ProcessorFixedWingModel(const YAML::Node& _params); + + // Factory method for high level API + WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel); + + virtual ~ProcessorFixedWingModel() override; + void configure(SensorBasePtr _sensor) override; + + protected: + /** \brief process an incoming capture NEVER CALLED + */ + virtual void processCapture(CaptureBasePtr) override{}; + + /** \brief process an incoming key-frame: applies the motion model between consecutive keyframes + */ + virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) override; + + /** \brief trigger in capture + */ + virtual bool triggerInCapture(CaptureBasePtr) const override + { + return false; + }; + + /** \brief trigger in key-frame + */ + virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override + { + return true; + }; + + /** \brief store key frame + */ + virtual bool storeKeyFrame(FrameBasePtr) override + { + return false; + }; + + /** \brief store capture + */ + virtual bool storeCapture(CaptureBasePtr) override + { + return false; + }; + + /** \brief Vote for KeyFrame generation + */ + virtual bool voteForKeyFrame() const override + { + return false; + }; + + // Params + Eigen::Vector3d velocity_local_; + double angle_stdev_; + double min_vel_norm_; }; } /* namespace wolf */ diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h index 1dd8cdb1cf9d077a96af4ab7a0b2d4eb922b290a..1702ec0a300ef16f91494be12e42187142026e20 100644 --- a/include/core/processor/processor_landmark_external.h +++ b/include/core/processor/processor_landmark_external.h @@ -26,7 +26,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal); // Class diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h index 7cfaa92da5879ff210e9e36d07f061db3c0773f1..335161845b0e6ae53b237f4919644a78ea18bdf0 100644 --- a/include/core/processor/processor_loop_closure.h +++ b/include/core/processor/processor_loop_closure.h @@ -23,8 +23,8 @@ // Wolf related headers #include "core/processor/processor_base.h" -namespace wolf{ - +namespace wolf +{ WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure); /** \brief Match between a capture and a capture @@ -34,9 +34,9 @@ WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure); */ struct MatchLoopClosure { - CaptureBasePtr capture_reference; ///< Capture reference - CaptureBasePtr capture_target; ///< Capture target - double normalized_score; ///< normalized similarity score (0 is bad, 1 is good) + CaptureBasePtr capture_reference; ///< Capture reference + CaptureBasePtr capture_target; ///< Capture target + double normalized_score; ///< normalized similarity score (0 is bad, 1 is good) }; /** \brief General loop closure processor @@ -53,57 +53,69 @@ struct MatchLoopClosure */ class ProcessorLoopClosure : public ProcessorBase { - public: - - ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params); - - ~ProcessorLoopClosure() override = default; - void configure(SensorBasePtr _sensor) override { }; - - int getMaxLoops() const; - void setMaxLoops(int _max_loops); - - protected: - - /** \brief Process a capture (linked to a frame) - * If voteFindLoopClosures() returns true, findLoopClosures() is called. - * emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures() - */ - virtual void process(CaptureBasePtr); - - /** \brief Returns if findLoopClosures() has to be called for the given capture - */ - virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0; - - /** \brief detects and emplaces all features of the given capture - */ - virtual void emplaceFeatures(CaptureBasePtr cap) = 0; - - /** \brief Find captures that correspond to loop closures with the given capture - */ - virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0; - - /** \brief validates a loop closure - */ - virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0; - - /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures - */ - virtual void emplaceFactors(MatchLoopClosurePtr) = 0; - - void processCapture(CaptureBasePtr) override; - void processKeyFrame(FrameBasePtr _frm) override; - - bool triggerInCapture(CaptureBasePtr _cap) const override { return true;}; - bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;}; - - bool storeKeyFrame(FrameBasePtr _frm) override { return false;}; - bool storeCapture(CaptureBasePtr _cap) override { return false;}; - - bool voteForKeyFrame() const override { return false;}; + public: + ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params); + + ~ProcessorLoopClosure() override = default; + void configure(SensorBasePtr _sensor) override{}; + + int getMaxLoops() const; + void setMaxLoops(int _max_loops); + + protected: + /** \brief Process a capture (linked to a frame) + * If voteFindLoopClosures() returns true, findLoopClosures() is called. + * emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures() + */ + virtual void process(CaptureBasePtr); + + /** \brief Returns if findLoopClosures() has to be called for the given capture + */ + virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0; + + /** \brief detects and emplaces all features of the given capture + */ + virtual void emplaceFeatures(CaptureBasePtr cap) = 0; + + /** \brief Find captures that correspond to loop closures with the given capture + */ + virtual std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0; + + /** \brief validates a loop closure + */ + virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0; + + /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures + */ + virtual void emplaceFactors(MatchLoopClosurePtr) = 0; + + void processCapture(CaptureBasePtr) override; + void processKeyFrame(FrameBasePtr _frm) override; + + bool triggerInCapture(CaptureBasePtr _cap) const override + { + return true; + }; + bool triggerInKeyFrame(FrameBasePtr _frm) const override + { + return true; + }; + + bool storeKeyFrame(FrameBasePtr _frm) override + { + return false; + }; + bool storeCapture(CaptureBasePtr _cap) override + { + return false; + }; + + bool voteForKeyFrame() const override + { + return false; + }; }; - inline int ProcessorLoopClosure::getMaxLoops() const { return params_["max_loops"].as<int>(); @@ -114,5 +126,4 @@ inline void ProcessorLoopClosure::setMaxLoops(int _max_loops) params_["max_loops"] = _max_loops; } - -} // namespace wolf +} // namespace wolf diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 7ff8ea3034b32f88d1bdc70c09983277c5975bfb..503a9357a14007afc17a438516c6b2612828800a 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -31,7 +31,6 @@ namespace wolf { - /** \brief class for Motion processors * * This processor integrates motion data into vehicle states. @@ -59,7 +58,7 @@ namespace wolf * * which are called at the beginning and at the end of process(). See the doc of these functions for more info. */ - /* // TODO: JS: review these instructions from here onwards: +/* // TODO: JS: review these instructions from here onwards: * * while the integrated motion refers to the robot frame. * @@ -111,415 +110,427 @@ namespace wolf */ class ProcessorMotion : public ProcessorBase, public MotionProvider { - public: - typedef enum { - FIRST_TIME_WITHOUT_KF, - FIRST_TIME_WITH_KF_BEFORE_INCOMING, - FIRST_TIME_WITH_KF_ON_INCOMING, - FIRST_TIME_WITH_KF_AFTER_INCOMING, - RUNNING_WITHOUT_KF, - RUNNING_WITH_KF_BEFORE_ORIGIN, - RUNNING_WITH_KF_ON_ORIGIN, - RUNNING_WITH_KF_AFTER_ORIGIN - } ProcessingStep ; - - protected: - ProcessingStep processing_step_; ///< State machine controlling the processing step - bool bootstrapping_; ///< processor is bootstrapping - - // This is the main public interface - public: - ProcessorMotion(const std::string& _type, - TypeComposite _state_types, - int _dim, - SizeEigen _state_size, - SizeEigen _delta_size, - SizeEigen _delta_cov_size, - SizeEigen _data_size, - SizeEigen _calib_size, - const YAML::Node& _params); - ~ProcessorMotion() override; - - // Instructions to the processor: - virtual void resetDerived(); - - // Queries to the processor: - TimeStamp getTimeStamp() const override; - VectorComposite getState(const StateKeys& _structure = "") const override; - VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override; - - /** \brief Gets the delta preintegrated covariance from all integrations so far - * \return the delta preintegrated covariance matrix - */ - const Eigen::MatrixXd getCurrentDeltaPreintCov() const; - - /** \brief Provide the motion integrated so far - * \return the integrated motion - */ - Motion getMotion() const; - - /** \brief Provide the motion integrated until a given timestamp - * \return the integrated motion - */ - Motion getMotion(const TimeStamp& _ts) const; - - /** \brief Finds the capture that contains the closest previous motion of _ts - * \return a pointer to the capture (if it exists) or a nullptr (otherwise) - */ - CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const; - CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts); - - /** Set the origin of all motion for this processor - * \param _origin_frame the keyframe to be the origin - */ - void setOrigin(FrameBasePtr _origin_frame); - - /** Set the origin of all motion for this processor - * \param _x_origin the state at the origin - * \param _ts_origin origin timestamp. - */ - FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin); - - // Helper functions: - MotionBuffer& getBuffer(); - const MotionBuffer& getBuffer() const; - - protected: - - /** \brief process an incoming capture - * - * Each derived processor should implement this function. It will be called if: - * - A new capture arrived and triggerInCapture() returned true. - */ - void processCapture(CaptureBasePtr) override; - - /** \brief process an incoming key-frame - * - * The ProcessorMotion only processes incoming captures (it is not called). - */ - void processKeyFrame(FrameBasePtr _keyframe_ptr) override {}; - - /** \brief trigger in capture - * - * Returns true if processCapture() should be called after the provided capture arrived. - */ - bool triggerInCapture(CaptureBasePtr) const override {return true;} - - /** \brief trigger in key-frame - * - * The ProcessorMotion only processes incoming captures, then it returns false. - */ - bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;} - - /** \brief store key frame - * - * Returns true if the key frame should be stored - */ - bool storeKeyFrame(FrameBasePtr) override { return true;} - - /** \brief store capture - * - * Returns true if the capture should be stored - */ - bool storeCapture(CaptureBasePtr) override { return false;} - - bool voteForKeyFrame() const override; - - double updateDt(); - /** \brief Make one step of motion integration - * - * Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_ - */ - void integrateOneStep(); - void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const; - void splitBuffer(const wolf::CaptureMotionPtr& capture_source, - const TimeStamp ts_split, - const wolf::CaptureMotionPtr& capture_target) const; - - /** Pre-process incoming Capture - * - * This is called by process() just after assigning incoming_ptr_ to a valid Capture. - * - * Overload this function to prepare stuff on derived classes. - * - * Typical uses of prePrecess() are: - * - casting base types to derived types - * - initializing counters, flags, or any derived variables - * - initializing algorithms needed for processing the derived data - */ - virtual void preProcess(){ }; - - /** - * @brief Bootstrap the processor with some initialization steps - */ - virtual void bootstrap(){}; - - /** Post-process - * - * This is called by process() after finishing the processing algorithm. - * - * Overload this function to post-process stuff on derived classes. - * - * Typical uses of postPrecess() are: - * - resetting and/or clearing variables and/or algorithms at the end of processing - * - drawing / printing / logging the results of the processing - */ - virtual void postProcess(){ }; - - FrameBasePtr computeProcessingStep(); - - void assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const; - - // These are the pure virtual functions doing the mathematics - public: - - /** \brief convert raw CaptureMotion data to the delta-state format - * - * This function accepts raw data and time step dt, - * and computes the value of the delta-state and its covariance. Note that these values are - * held by the members delta_ and delta_cov_. - * - * @param _data measured motion data - * @param _data_cov covariance - * @param _dt time step - * @param _delta computed delta - * @param _delta_cov covariance - * @param _calib current state of the calibrated parameters - * @param _jacobian_calib Jacobian of the delta wrt calib - * - * Rationale: - * - * The delta-state format must be compatible for integration using - * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta(). - * See the class documentation for some Eigen::VectorXd suggestions. - * - * The data format is generally not the same as the delta format, - * because it is the format of the raw data provided by the Capture, - * which is unaware of the needs of this processor. - * - * Additionally, sometimes the data format is in the form of a - * velocity, or a higher derivative, while the delta is in the form of an increment. - * In such cases, converting from data to delta-state needs integrating - * the data over the period dt. - * - * Two trivial implementations would establish: - * - If `_data` is an increment: - * - * delta_ = _data; - * delta_cov_ = _data_cov - * - If `_data` is a velocity: - * - * delta_ = _data * _dt - * delta_cov_ = _data_cov * _dt. - * - * However, other more complicated relations are possible. - * In general, we'll have a nonlinear - * function relating `delta_` to `_data` and `_dt`, as follows: - * - * delta_ = f ( _data, _dt) - * - * The delta covariance is obtained by classical uncertainty propagation of the data covariance, - * that is, through the Jacobians of `f()`, - * - * delta_cov_ = F_data * _data_cov * F_data.transpose - * - * where `F_data = d_f/d_data` is the Jacobian of `f()`. - */ - virtual void computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jacobian_calib) const = 0; - - /** \brief composes a delta-state on top of another delta-state - * \param _delta1 the first delta-state - * \param _delta2 the second delta-state - * \param _dt2 the second delta-state's time delta - * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state. - * - * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2. - */ - virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _dt2, - Eigen::VectorXd& _delta1_plus_delta2) const = 0; - - /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians - * \param _delta1 the first delta-state - * \param _delta2 the second delta-state - * \param _dt2 the second delta-state's time delta - * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state. - * \param _jacobian1 the jacobian of the composition w.r.t. _delta1. - * \param _jacobian2 the jacobian of the composition w.r.t. _delta2. - * - * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its jacobians. - */ - virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _dt2, - Eigen::VectorXd& _delta1_plus_delta2, - Eigen::MatrixXd& _jacobian1, - Eigen::MatrixXd& _jacobian2) const = 0; - - /** \brief composes a delta-state on top of a state - * \param _x the initial state - * \param _delta the delta-state - * \param _x_plus_delta the updated state. It has the same format as the initial state. - * \param _dt the time interval spanned by the Delta - * - * This function implements the composition (+) so that _x2 = _x1 (+) _delta. - */ - virtual void statePlusDelta(const VectorComposite& _x, - const Eigen::VectorXd& _delta, - const double _dt, - VectorComposite& _x_plus_delta) const = 0; - - /** \brief Delta zero - * \return a delta state equivalent to the null motion. - * - * Examples (see documentation of the the class for info on Eigen::VectorXd): - * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3) - * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1] - * - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !! - */ - virtual Eigen::VectorXd deltaZero() const = 0; - - - /** \brief correct the preintegrated delta - * This produces a delta correction according to the appropriate manifold. - * @param delta_preint : the preintegrated delta to correct - * @param delta_step : an increment in the tangent space of the manifold - * - * We want to do - * - * delta_corr = delta_preint (+) d_step - * - * where the operator (+) is the right-plus operator on the delta's manifold. - * - * Note: usually in motion pre-integration we have - * - * delta_step = Jac_delta_calib * (calib - calib_pre) - * - */ - virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, - const Eigen::VectorXd& delta_step) const = 0; - - /** \brief merge two consecutive capture motions into the second one - * Merge two consecutive capture motions into the second one. - * The first capture motion is not removed. - * If the second capture has feature and factor emplaced, they are replaced by a new ones. - * @param cap_prev : the first capture motion to be merged (input) - * @param cap_target : the second capture motion (modified) - */ - void mergeCaptures(CaptureMotionPtr cap_prev, - CaptureMotionPtr cap_target); - - protected: - /** \brief emplace a CaptureMotion - * \param _frame_own frame owning the Capture to create - * \param _sensor Sensor that produced the Capture - * \param _ts time stamp - * \param _data a vector of motion data - * \param _data_cov covariances matrix of the motion data data - * \param _calib calibration vector - * \param _calib_preint calibration vector used during pre-integration - * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture - */ - virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin_ptr) = 0; - - - /** \brief emplace the features and factors corresponding to given capture and link them to the capture - * \param _capture_own: the parent capture - * \param _capture_origin: the capture constrained by this motion factor - * - * Typical factors to add for a ProcessorMotionDerived can be: - * - A preintegrated motion factor -- this is the main factor - * - A calibration drift factor -- only for dynamic sensor calibration parameters - * - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot models - */ - virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0; - - virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0; - - Motion motionZero(const TimeStamp& _ts) const; - - public: - - virtual VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const = 0; - bool hasCalibration() const {return calib_size_ > 0;} - - //getters - CaptureMotionConstPtr getOrigin() const; - CaptureMotionConstPtr getLast() const; - CaptureMotionConstPtr getIncoming() const; - CaptureMotionPtr getOrigin(); - CaptureMotionPtr getLast(); - CaptureMotionPtr getIncoming(); - - Eigen::MatrixXd getUnmeasuredPerturbationCov() const; - - double getMaxTimeSpan() const; - double getMaxBuffLength() const; - double getMaxDistTraveled() const; - double getMaxAngleTurned() const; - - void setMaxTimeSpan(const double& _max_time_span); - void setMaxBuffLength(const double& _max_buff_length); - void setMaxDistTraveled(const double& _max_dist_traveled); - void setMaxAngleTurned(const double& _max_angle_turned); - - void printHeader(int depth, // - bool factored_by, // - bool metric, // - bool state_blocks, - std::ostream& stream , - std::string _tabs = "") const override; - - protected: - // Attributes - SizeEigen x_size_; ///< The size of the state vector - SizeEigen data_size_; ///< the size of the incoming data - SizeEigen delta_size_; ///< the size of the deltas - SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix - SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes) - CaptureMotionPtr origin_ptr_; - CaptureMotionPtr last_ptr_; - CaptureMotionPtr incoming_ptr_; - - FrameBasePtr last_frame_ptr_; - - protected: - // helpers to avoid allocation - mutable double dt_; ///< Time step - mutable Eigen::VectorXd x_; ///< current state - mutable Eigen::VectorXd delta_; ///< current delta - mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance - mutable Eigen::VectorXd delta_integrated_; ///< integrated delta - mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance - mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration - mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated - mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta - mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params - mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params - - Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity + public: + typedef enum + { + FIRST_TIME_WITHOUT_KF, + FIRST_TIME_WITH_KF_BEFORE_INCOMING, + FIRST_TIME_WITH_KF_ON_INCOMING, + FIRST_TIME_WITH_KF_AFTER_INCOMING, + RUNNING_WITHOUT_KF, + RUNNING_WITH_KF_BEFORE_ORIGIN, + RUNNING_WITH_KF_ON_ORIGIN, + RUNNING_WITH_KF_AFTER_ORIGIN + } ProcessingStep; + + protected: + ProcessingStep processing_step_; ///< State machine controlling the processing step + bool bootstrapping_; ///< processor is bootstrapping + + // This is the main public interface + public: + ProcessorMotion(const std::string& _type, + TypeComposite _state_types, + int _dim, + SizeEigen _state_size, + SizeEigen _delta_size, + SizeEigen _delta_cov_size, + SizeEigen _data_size, + SizeEigen _calib_size, + const YAML::Node& _params); + ~ProcessorMotion() override; + + // Instructions to the processor: + virtual void resetDerived(); + + // Queries to the processor: + TimeStamp getTimeStamp() const override; + VectorComposite getState(const StateKeys& _structure = "") const override; + VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override; + + /** \brief Gets the delta preintegrated covariance from all integrations so far + * \return the delta preintegrated covariance matrix + */ + const Eigen::MatrixXd getCurrentDeltaPreintCov() const; + + /** \brief Provide the motion integrated so far + * \return the integrated motion + */ + Motion getMotion() const; + + /** \brief Provide the motion integrated until a given timestamp + * \return the integrated motion + */ + Motion getMotion(const TimeStamp& _ts) const; + + /** \brief Finds the capture that contains the closest previous motion of _ts + * \return a pointer to the capture (if it exists) or a nullptr (otherwise) + */ + CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const; + CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts); + + /** Set the origin of all motion for this processor + * \param _origin_frame the keyframe to be the origin + */ + void setOrigin(FrameBasePtr _origin_frame); + + /** Set the origin of all motion for this processor + * \param _x_origin the state at the origin + * \param _ts_origin origin timestamp. + */ + FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin); + + // Helper functions: + MotionBuffer& getBuffer(); + const MotionBuffer& getBuffer() const; + + protected: + /** \brief process an incoming capture + * + * Each derived processor should implement this function. It will be called if: + * - A new capture arrived and triggerInCapture() returned true. + */ + void processCapture(CaptureBasePtr) override; + + /** \brief process an incoming key-frame + * + * The ProcessorMotion only processes incoming captures (it is not called). + */ + void processKeyFrame(FrameBasePtr _keyframe_ptr) override{}; + + /** \brief trigger in capture + * + * Returns true if processCapture() should be called after the provided capture arrived. + */ + bool triggerInCapture(CaptureBasePtr) const override + { + return true; + } + + /** \brief trigger in key-frame + * + * The ProcessorMotion only processes incoming captures, then it returns false. + */ + bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override + { + return false; + } + + /** \brief store key frame + * + * Returns true if the key frame should be stored + */ + bool storeKeyFrame(FrameBasePtr) override + { + return true; + } + + /** \brief store capture + * + * Returns true if the capture should be stored + */ + bool storeCapture(CaptureBasePtr) override + { + return false; + } + + bool voteForKeyFrame() const override; + + double updateDt(); + /** \brief Make one step of motion integration + * + * Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_ + */ + void integrateOneStep(); + void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const; + void splitBuffer(const wolf::CaptureMotionPtr& capture_source, + const TimeStamp ts_split, + const wolf::CaptureMotionPtr& capture_target) const; + + /** Pre-process incoming Capture + * + * This is called by process() just after assigning incoming_ptr_ to a valid Capture. + * + * Overload this function to prepare stuff on derived classes. + * + * Typical uses of prePrecess() are: + * - casting base types to derived types + * - initializing counters, flags, or any derived variables + * - initializing algorithms needed for processing the derived data + */ + virtual void preProcess(){}; + + /** + * @brief Bootstrap the processor with some initialization steps + */ + virtual void bootstrap(){}; + + /** Post-process + * + * This is called by process() after finishing the processing algorithm. + * + * Overload this function to post-process stuff on derived classes. + * + * Typical uses of postPrecess() are: + * - resetting and/or clearing variables and/or algorithms at the end of processing + * - drawing / printing / logging the results of the processing + */ + virtual void postProcess(){}; + + FrameBasePtr computeProcessingStep(); + + void assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const; + + // These are the pure virtual functions doing the mathematics + public: + /** \brief convert raw CaptureMotion data to the delta-state format + * + * This function accepts raw data and time step dt, + * and computes the value of the delta-state and its covariance. Note that these values are + * held by the members delta_ and delta_cov_. + * + * @param _data measured motion data + * @param _data_cov covariance + * @param _dt time step + * @param _delta computed delta + * @param _delta_cov covariance + * @param _calib current state of the calibrated parameters + * @param _jacobian_calib Jacobian of the delta wrt calib + * + * Rationale: + * + * The delta-state format must be compatible for integration using + * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta(). + * See the class documentation for some Eigen::VectorXd suggestions. + * + * The data format is generally not the same as the delta format, + * because it is the format of the raw data provided by the Capture, + * which is unaware of the needs of this processor. + * + * Additionally, sometimes the data format is in the form of a + * velocity, or a higher derivative, while the delta is in the form of an increment. + * In such cases, converting from data to delta-state needs integrating + * the data over the period dt. + * + * Two trivial implementations would establish: + * - If `_data` is an increment: + * + * delta_ = _data; + * delta_cov_ = _data_cov + * - If `_data` is a velocity: + * + * delta_ = _data * _dt + * delta_cov_ = _data_cov * _dt. + * + * However, other more complicated relations are possible. + * In general, we'll have a nonlinear + * function relating `delta_` to `_data` and `_dt`, as follows: + * + * delta_ = f ( _data, _dt) + * + * The delta covariance is obtained by classical uncertainty propagation of the data covariance, + * that is, through the Jacobians of `f()`, + * + * delta_cov_ = F_data * _data_cov * F_data.transpose + * + * where `F_data = d_f/d_data` is the Jacobian of `f()`. + */ + virtual void computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jacobian_calib) const = 0; + + /** \brief composes a delta-state on top of another delta-state + * \param _delta1 the first delta-state + * \param _delta2 the second delta-state + * \param _dt2 the second delta-state's time delta + * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state. + * + * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2. + */ + virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _dt2, + Eigen::VectorXd& _delta1_plus_delta2) const = 0; + + /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians + * \param _delta1 the first delta-state + * \param _delta2 the second delta-state + * \param _dt2 the second delta-state's time delta + * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state. + * \param _jacobian1 the jacobian of the composition w.r.t. _delta1. + * \param _jacobian2 the jacobian of the composition w.r.t. _delta2. + * + * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its + * jacobians. + */ + virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _dt2, + Eigen::VectorXd& _delta1_plus_delta2, + Eigen::MatrixXd& _jacobian1, + Eigen::MatrixXd& _jacobian2) const = 0; + + /** \brief composes a delta-state on top of a state + * \param _x the initial state + * \param _delta the delta-state + * \param _x_plus_delta the updated state. It has the same format as the initial state. + * \param _dt the time interval spanned by the Delta + * + * This function implements the composition (+) so that _x2 = _x1 (+) _delta. + */ + virtual void statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _dt, + VectorComposite& _x_plus_delta) const = 0; + + /** \brief Delta zero + * \return a delta state equivalent to the null motion. + * + * Examples (see documentation of the the class for info on Eigen::VectorXd): + * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3) + * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1] + * - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !! + */ + virtual Eigen::VectorXd deltaZero() const = 0; + + /** \brief correct the preintegrated delta + * This produces a delta correction according to the appropriate manifold. + * @param delta_preint : the preintegrated delta to correct + * @param delta_step : an increment in the tangent space of the manifold + * + * We want to do + * + * delta_corr = delta_preint (+) d_step + * + * where the operator (+) is the right-plus operator on the delta's manifold. + * + * Note: usually in motion pre-integration we have + * + * delta_step = Jac_delta_calib * (calib - calib_pre) + * + */ + virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, + const Eigen::VectorXd& delta_step) const = 0; + + /** \brief merge two consecutive capture motions into the second one + * Merge two consecutive capture motions into the second one. + * The first capture motion is not removed. + * If the second capture has feature and factor emplaced, they are replaced by a new ones. + * @param cap_prev : the first capture motion to be merged (input) + * @param cap_target : the second capture motion (modified) + */ + void mergeCaptures(CaptureMotionPtr cap_prev, CaptureMotionPtr cap_target); + + protected: + /** \brief emplace a CaptureMotion + * \param _frame_own frame owning the Capture to create + * \param _sensor Sensor that produced the Capture + * \param _ts time stamp + * \param _data a vector of motion data + * \param _data_cov covariances matrix of the motion data data + * \param _calib calibration vector + * \param _calib_preint calibration vector used during pre-integration + * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture + */ + virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin_ptr) = 0; + + /** \brief emplace the features and factors corresponding to given capture and link them to the capture + * \param _capture_own: the parent capture + * \param _capture_origin: the capture constrained by this motion factor + * + * Typical factors to add for a ProcessorMotionDerived can be: + * - A preintegrated motion factor -- this is the main factor + * - A calibration drift factor -- only for dynamic sensor calibration parameters + * - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot + * models + */ + virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0; + + virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0; + + Motion motionZero(const TimeStamp& _ts) const; + + public: + virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const = 0; + bool hasCalibration() const + { + return calib_size_ > 0; + } + + // getters + CaptureMotionConstPtr getOrigin() const; + CaptureMotionConstPtr getLast() const; + CaptureMotionConstPtr getIncoming() const; + CaptureMotionPtr getOrigin(); + CaptureMotionPtr getLast(); + CaptureMotionPtr getIncoming(); + + Eigen::MatrixXd getUnmeasuredPerturbationCov() const; + + double getMaxTimeSpan() const; + double getMaxBuffLength() const; + double getMaxDistTraveled() const; + double getMaxAngleTurned() const; + + void setMaxTimeSpan(const double& _max_time_span); + void setMaxBuffLength(const double& _max_buff_length); + void setMaxDistTraveled(const double& _max_dist_traveled); + void setMaxAngleTurned(const double& _max_angle_turned); + + void printHeader(int depth, // + bool factored_by, // + bool metric, // + bool state_blocks, + std::ostream& stream, + std::string _tabs = "") const override; + + protected: + // Attributes + SizeEigen x_size_; ///< The size of the state vector + SizeEigen data_size_; ///< the size of the incoming data + SizeEigen delta_size_; ///< the size of the deltas + SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix + SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes) + CaptureMotionPtr origin_ptr_; + CaptureMotionPtr last_ptr_; + CaptureMotionPtr incoming_ptr_; + + FrameBasePtr last_frame_ptr_; + + protected: + // helpers to avoid allocation + mutable double dt_; ///< Time step + mutable Eigen::VectorXd x_; ///< current state + mutable Eigen::VectorXd delta_; ///< current delta + mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance + mutable Eigen::VectorXd delta_integrated_; ///< integrated delta + mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance + mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration + mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated + mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta + mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params + mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params + + Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity }; -} +} // namespace wolf #include "core/frame/frame_base.h" -namespace wolf{ - +namespace wolf +{ inline void ProcessorMotion::resetDerived() { // Blank function, to be implemented in derived classes @@ -568,15 +579,15 @@ inline MotionBuffer& ProcessorMotion::getBuffer() inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const { return Motion(_ts, - VectorXd::Zero(data_size_), // data - Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data + VectorXd::Zero(data_size_), // data + Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data deltaZero(), - Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta + Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta deltaZero(), - Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr - Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta - Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr - Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib + Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr + Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta + Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr + Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib ); } @@ -655,5 +666,4 @@ inline void ProcessorMotion::setMaxAngleTurned(const double& _max_angle_turned) params_["keyframe_vote"]["max_angle_turned"] = _max_angle_turned; } - -} // namespace wolf +} // namespace wolf diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h index ee573516acbaf5746f0917edc0e3bf399cbc0626..0c3268f447a9bf126ee007df99672a860c0def3c 100644 --- a/include/core/processor/processor_odom_2d.h +++ b/include/core/processor/processor_odom_2d.h @@ -27,7 +27,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorOdom2d); class ProcessorOdom2d : public ProcessorMotion @@ -41,7 +40,7 @@ class ProcessorOdom2d : public ProcessorMotion WOLF_PROCESSOR_CREATE(ProcessorOdom2d); double getCovDet() const; - bool voteForKeyFrame() const override; + bool voteForKeyFrame() const override; protected: void computeCurrentDelta(const Eigen::VectorXd& _data, diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h index f6d97b527435da766ce25b73c38cb3e1545e67f3..ea9855622162affaf61cc616b4936bde8276d208 100644 --- a/include/core/processor/processor_odom_3d.h +++ b/include/core/processor/processor_odom_3d.h @@ -26,8 +26,8 @@ #include "core/factor/factor_relative_pose_3d.h" #include <cmath> -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(ProcessorOdom3d); /** \brief Processor for 3d odometry integration. @@ -37,7 +37,8 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3d); * The odometry data is extracted from Captures of the type CaptureOdometry3d. * This data comes in the form of a 6-vector, or a 7-vector, containing the following components: * - a 3d position increment in the local frame of the robot (dx, dy, dz). - * - a 3d orientation increment in the local frame of the robot (droll, dpitch, dyaw), or quaternion (dqx, dqy, dqz, dqw). + * - a 3d orientation increment in the local frame of the robot (droll, dpitch, dyaw), or quaternion (dqx, dqy, dqz, + * dqw). * * The produced integrated deltas are in the form of 7-vectors with the following components: * - a 3d position increment in the local frame of the robot (Dx, Dy, Dz) @@ -54,60 +55,60 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3d); */ class ProcessorOdom3d : public ProcessorMotion { - public: - ProcessorOdom3d(const YAML::Node& _params); - ~ProcessorOdom3d() override; - void configure(SensorBasePtr _sensor) override; + public: + ProcessorOdom3d(const YAML::Node& _params); + ~ProcessorOdom3d() override; + void configure(SensorBasePtr _sensor) override; - // Factory method for high level API - WOLF_PROCESSOR_CREATE(ProcessorOdom3d); + // Factory method for high level API + WOLF_PROCESSOR_CREATE(ProcessorOdom3d); - public: - // Motion integration - void computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jacobian_calib) const override; - void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _Dt2, - Eigen::VectorXd& _delta1_plus_delta2) const override; - void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _Dt2, - Eigen::VectorXd& _delta1_plus_delta2, - Eigen::MatrixXd& _jacobian1, - Eigen::MatrixXd& _jacobian2) const override; - void statePlusDelta(const VectorComposite& _x, - const Eigen::VectorXd& _delta, - const double _Dt, - VectorComposite& _x_plus_delta) const override; - Eigen::VectorXd deltaZero() const override; - Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, - const Eigen::VectorXd& delta_step) const override; + public: + // Motion integration + void computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jacobian_calib) const override; + void deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _Dt2, + Eigen::VectorXd& _delta1_plus_delta2) const override; + void deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _Dt2, + Eigen::VectorXd& _delta1_plus_delta2, + Eigen::MatrixXd& _jacobian1, + Eigen::MatrixXd& _jacobian2) const override; + void statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _Dt, + VectorComposite& _x_plus_delta) const override; + Eigen::VectorXd deltaZero() const override; + Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, + const Eigen::VectorXd& delta_step) const override; - bool voteForKeyFrame() const override; + bool voteForKeyFrame() const override; - CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin) override; - void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override; + CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin) override; + void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override; - VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override; - void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; + VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override; + void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; }; inline Eigen::VectorXd ProcessorOdom3d::deltaZero() const { - return (Eigen::VectorXd(7) << 0,0,0, 0,0,0,1).finished(); // p, q + return (Eigen::VectorXd(7) << 0, 0, 0, 0, 0, 0, 1).finished(); // p, q } -} // namespace wolf +} // namespace wolf diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h index 7edc72618ea14216c53bbf06caa2d972e02fee52..03aae6627321ebf2e233bcd5f9f449ea989654ac 100644 --- a/include/core/processor/processor_pose.h +++ b/include/core/processor/processor_pose.h @@ -25,7 +25,6 @@ namespace wolf { - // class template <unsigned int DIM> class ProcessorPose : public ProcessorBase @@ -80,10 +79,8 @@ WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose3d); namespace wolf { - template <unsigned int DIM> -inline ProcessorPose<DIM>::ProcessorPose(const YAML::Node& _params) - : ProcessorBase("ProcessorPose", DIM, _params) +inline ProcessorPose<DIM>::ProcessorPose(const YAML::Node& _params) : ProcessorBase("ProcessorPose", DIM, _params) { static_assert(DIM == 2 or DIM == 3); } diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index c6e99e5c0291bfded5be11f2f3aa79f42b47e71b..9653707c768f4cddb2837e5c8037d15efd46d3ab 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -25,7 +25,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorTracker); /** \brief General tracker processor @@ -98,7 +97,7 @@ class ProcessorTracker : public ProcessorBase FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming FeatureBasePtrList - known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last + known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming StateKeys state_keys_; ///< Keys of frames created or used by this processor diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h index 2d682e6ac560f0a309891006cce84ba7d6dd505c..522d60f020016a873473a97efc399e378bd6b1dd 100644 --- a/include/core/processor/processor_tracker_feature.h +++ b/include/core/processor/processor_tracker_feature.h @@ -20,7 +20,7 @@ #pragma once -//wolf includes +// wolf includes #include "core/processor/processor_tracker.h" #include "core/capture/capture_base.h" #include "core/feature/feature_match.h" @@ -29,7 +29,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature); /** \brief Feature tracker processor @@ -52,7 +51,8 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature); * * This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions. * As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function. - * We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by derived classes with '<=== IMPLEMENT' + * We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by + * derived classes with '<=== IMPLEMENT' * * - On each incoming Capture, * - Track known features in the \b incoming Capture: processKnown() <--- IMPLEMENTED @@ -85,117 +85,118 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature); */ class ProcessorTrackerFeature : public ProcessorTracker { - public: - - /** \brief Constructor with type - */ - ProcessorTrackerFeature(const std::string& _type, - const StateKeys& _structure, - int _dim, - const YAML::Node& _params); - ~ProcessorTrackerFeature() override; - - protected: - TrackMatrix track_matrix_; - - FeatureMatchMap matches_last_from_incoming_; - - /** \brief Process known Features - * \return The number of successful matches. - * - * This function operates on the \b incoming capture pointed by incoming_ptr_. - * - * This function does: - * - Track Features against other Features in the \b origin Capture. Tips: - * - An intermediary step of matching against Features in the \b last Capture makes tracking easier. - * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last. - * - If required, correct the drift by re-comparing against the Features in origin. - * - The Factors in \b last need to be transferred to \b incoming (moved, not copied). - * - Create the necessary Features in the \b incoming Capture, - * of the correct type, derived from FeatureBase. - * - Create the factors, of the correct type, derived from FactorBase - * (through FactorAnalytic or FactorSparse). - */ - unsigned int processKnown() override; - - /** \brief Track provided features in \b _capture - * \param _features_in input list of features in \b last to track - * \param _capture the capture in which the _features_in should be searched - * \param _features_out returned list of features found in \b _capture - * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr - * - * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead. - * Then, they will be already linked to the _capture. - * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the - * features that are returned in _features_out (`FeatureBase::link(_capture)`). - * - * \return the number of features tracked - */ - virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in, + public: + /** \brief Constructor with type + */ + ProcessorTrackerFeature(const std::string& _type, + const StateKeys& _structure, + int _dim, + const YAML::Node& _params); + ~ProcessorTrackerFeature() override; + + protected: + TrackMatrix track_matrix_; + + FeatureMatchMap matches_last_from_incoming_; + + /** \brief Process known Features + * \return The number of successful matches. + * + * This function operates on the \b incoming capture pointed by incoming_ptr_. + * + * This function does: + * - Track Features against other Features in the \b origin Capture. Tips: + * - An intermediary step of matching against Features in the \b last Capture makes tracking easier. + * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in + * \b last. + * - If required, correct the drift by re-comparing against the Features in origin. + * - The Factors in \b last need to be transferred to \b incoming (moved, not copied). + * - Create the necessary Features in the \b incoming Capture, + * of the correct type, derived from FeatureBase. + * - Create the factors, of the correct type, derived from FactorBase + * (through FactorAnalytic or FactorSparse). + */ + unsigned int processKnown() override; + + /** \brief Track provided features in \b _capture + * \param _features_in input list of features in \b last to track + * \param _capture the capture in which the _features_in should be searched + * \param _features_out returned list of features found in \b _capture + * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr + * + * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` + * instead. Then, they will be already linked to the _capture. If you detect all the features at once in + * preprocess(), you should create them (`make_shared()`) and only link the features that are returned in + * _features_out (`FeatureBase::link(_capture)`). + * + * \return the number of features tracked + */ + virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in, + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out, + FeatureMatchMap& _feature_correspondences) = 0; + + /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. + * \param _origin_feature input feature in origin capture tracked + * \param _incoming_feature input/output feature in incoming capture to be corrected + * \return false if the the process discards the correspondence with origin's feature + */ + virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, + const FeatureBasePtr _last_feature, + FeatureBasePtr _incoming_feature) = 0; + + /** \brief Vote for KeyFrame generation + * + * If a KeyFrame criterion is validated, this function returns true, + * meaning that it wants to create a KeyFrame at the \b last Capture. + * + * WARNING! This function only votes! It does not create KeyFrames! + */ + bool voteForKeyFrame() const override = 0; + + // We overload the advance and reset functions to update the lists of matches + void advanceDerived() override; + void resetDerived() override; + + /**\brief Process new Features + * + */ + unsigned int processNew(const int& _max_features) override; + + /** \brief Detect new Features + * \param _max_features maximum number of features detected (-1: unlimited. 0: none) + * \param _capture The capture in which the new features should be detected. + * \param _features_out The list of detected Features in _capture. + * \return The number of detected Features. + * + * This function detects Features that do not correspond to known Features/Landmarks in the system. + * + * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` + * instead. Then, they will be already linked to the _capture. If you detect all the features at once in + * preprocess(), you should create them (`make_shared()`) and only link the features that are returned in + * _features_out (`FeatureBase::link(_capture)`). + * + * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_, + * the list of newly detected features of the capture last_ptr_. + */ + virtual unsigned int detectNewFeatures(const int& _max_new_features, const CaptureBasePtr& _capture, - FeatureBasePtrList& _features_out, - FeatureMatchMap& _feature_correspondences) = 0; - - /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. - * \param _origin_feature input feature in origin capture tracked - * \param _incoming_feature input/output feature in incoming capture to be corrected - * \return false if the the process discards the correspondence with origin's feature - */ - virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, - const FeatureBasePtr _last_feature, - FeatureBasePtr _incoming_feature) = 0; - - /** \brief Vote for KeyFrame generation - * - * If a KeyFrame criterion is validated, this function returns true, - * meaning that it wants to create a KeyFrame at the \b last Capture. - * - * WARNING! This function only votes! It does not create KeyFrames! - */ - bool voteForKeyFrame() const override = 0; - - // We overload the advance and reset functions to update the lists of matches - void advanceDerived() override; - void resetDerived() override; - - /**\brief Process new Features - * - */ - unsigned int processNew(const int& _max_features) override; - - /** \brief Detect new Features - * \param _max_features maximum number of features detected (-1: unlimited. 0: none) - * \param _capture The capture in which the new features should be detected. - * \param _features_out The list of detected Features in _capture. - * \return The number of detected Features. - * - * This function detects Features that do not correspond to known Features/Landmarks in the system. - * - * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead. - * Then, they will be already linked to the _capture. - * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the - * features that are returned in _features_out (`FeatureBase::link(_capture)`). - * - * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_, - * the list of newly detected features of the capture last_ptr_. - */ - virtual unsigned int detectNewFeatures(const int& _max_new_features, - const CaptureBasePtr& _capture, - FeatureBasePtrList& _features_out) = 0; - - /** \brief Emplaces a new factor - * \param _feature_ptr pointer to the parent Feature - * \param _feature_other_ptr pointer to the other feature constrained. - * - * Implement this method in derived classes. - * - * This function emplaces a factor of the appropriate type for the derived processor. - */ - virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0; - - /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in Capture \b origin - */ - void establishFactors() override; + FeatureBasePtrList& _features_out) = 0; + + /** \brief Emplaces a new factor + * \param _feature_ptr pointer to the parent Feature + * \param _feature_other_ptr pointer to the other feature constrained. + * + * Implement this method in derived classes. + * + * This function emplaces a factor of the appropriate type for the derived processor. + */ + virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0; + + /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in + * Capture \b origin + */ + void establishFactors() override; }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h index 062e05712d9e43766b691b741c12aa4d9999a34a..d024f4132daf257806ac8a42e64f5a572a6093eb 100644 --- a/include/core/processor/processor_tracker_landmark.h +++ b/include/core/processor/processor_tracker_landmark.h @@ -27,7 +27,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark); /** \brief Landmark tracker processor diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h index 6daa3880da9fc97782be6bdd86fc91616b9ce458..3e13668e041ae12579a347f5610b89a2d1156c71 100644 --- a/include/core/processor/track_matrix.h +++ b/include/core/processor/track_matrix.h @@ -30,18 +30,19 @@ namespace wolf { -using std::map; -using std::vector; using std::list; +using std::map; using std::pair; using std::shared_ptr; +using std::vector; -typedef map<TimeStamp, FeatureBasePtr> Track; -typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst; -typedef map<SizeStd, FeatureBasePtr > Snapshot; -typedef map<SizeStd, FeatureBaseConstPtr > SnapshotConst; -typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id -typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatchesConst; // matched feature pairs indexed by track_id +typedef map<TimeStamp, FeatureBasePtr> Track; +typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst; +typedef map<SizeStd, FeatureBasePtr> Snapshot; +typedef map<SizeStd, FeatureBaseConstPtr> SnapshotConst; +typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id +typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > + TrackMatchesConst; // matched feature pairs indexed by track_id /** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps) * This class implements the following data structure: @@ -71,7 +72,8 @@ typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatc * * The class makes sure that both accesses are consistent each time some addition or removal of features is performed. * - * The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in logarithmic time: + * The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in + * logarithmic time: * * Tracks are identified with the track ID in f->trackId() * Snapshots are identified with the Capture pointer in f->getCapture() @@ -89,54 +91,54 @@ typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatc class TrackMatrix { - public: - TrackMatrix(); - virtual ~TrackMatrix(); - - void newTrack (FeatureBasePtr _ftr); - void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr); - void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new); - void remove (FeatureBasePtr _ftr); - void remove (const SizeStd& _track_id); - void remove (CaptureBasePtr _cap); - - SizeStd numTracks () const; - SizeStd trackSize (const SizeStd& _track_id) const; - TrackConst track (const SizeStd& _track_id) const; - Track track (const SizeStd& _track_id); - SnapshotConst snapshot (CaptureBaseConstPtr _capture) const; - Snapshot snapshot (CaptureBasePtr _capture); - vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const; - vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id); - FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const; - FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap); - TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const; - TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2); - - FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const; - FeatureBasePtr firstFeature(const SizeStd& _track_id); - FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const; - FeatureBasePtr lastFeature (const SizeStd& _track_id); - FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const; - FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap); - CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const; - CaptureBasePtr firstCapture(const SizeStd& _track_id); - - list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const; - - // tracks across captures that belong to keyframe - TrackConst trackAtKeyframes(const SizeStd& _track_id) const; - Track trackAtKeyframes(const SizeStd& _track_id); - - private: - - static SizeStd track_id_count_; - - // tracks across all Captures - map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) ) - - // Across track: maps of Feature pointers indexed by track_Id. - map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) ) + public: + TrackMatrix(); + virtual ~TrackMatrix(); + + void newTrack(FeatureBasePtr _ftr); + void add(const SizeStd& _track_id, const FeatureBasePtr& _ftr); + void add(const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new); + void remove(FeatureBasePtr _ftr); + void remove(const SizeStd& _track_id); + void remove(CaptureBasePtr _cap); + + SizeStd numTracks() const; + SizeStd trackSize(const SizeStd& _track_id) const; + TrackConst track(const SizeStd& _track_id) const; + Track track(const SizeStd& _track_id); + SnapshotConst snapshot(CaptureBaseConstPtr _capture) const; + Snapshot snapshot(CaptureBasePtr _capture); + vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const; + vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id); + FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const; + FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap); + TrackMatchesConst matches(CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const; + TrackMatches matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2); + + FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const; + FeatureBasePtr firstFeature(const SizeStd& _track_id); + FeatureBaseConstPtr lastFeature(const SizeStd& _track_id) const; + FeatureBasePtr lastFeature(const SizeStd& _track_id); + FeatureBaseConstPtr feature(const SizeStd& _track_id, CaptureBaseConstPtr _cap) const; + FeatureBasePtr feature(const SizeStd& _track_id, CaptureBasePtr _cap); + CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const; + CaptureBasePtr firstCapture(const SizeStd& _track_id); + + list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const; + + // tracks across captures that belong to keyframe + TrackConst trackAtKeyframes(const SizeStd& _track_id) const; + Track trackAtKeyframes(const SizeStd& _track_id); + + private: + static SizeStd track_id_count_; + + // tracks across all Captures + map<SizeStd, Track> tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) ) + + // Across track: maps of Feature pointers indexed by track_Id. + map<CaptureBasePtr, Snapshot> + snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) ) }; } /* namespace wolf */ diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h index daf4cdf25d6f8facb16a363a2719edecdc463f9d..073e6c68a58c5ae2f8c139eaca42a1ce0ceb2e3c 100644 --- a/include/core/sensor/factory_sensor.h +++ b/include/core/sensor/factory_sensor.h @@ -39,7 +39,6 @@ class Composite; namespace wolf { - /** \brief Sensor factory class * * This factory can create objects of classes deriving from SensorBase. @@ -88,13 +87,13 @@ namespace wolf * throw std::runtime_error(server.getLog()); * } * } - * + * * // Do create the Sensor object --- example: SensorCamera * auto sensor = std::make_shared<SensorCamera>(params, priors); * * return sensor; * } - * + * * static SensorBasePtr create(const std::string& _schema, * const std::string& _yaml_filepath, * std::vector<std::string>& _folders_schema) diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index c305e75f7a7fe227701ed7a2076d724e068bc676..ad7ce431e044a348e78022104487bd29d9c9fead 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -119,20 +119,20 @@ class SensorBase : public NodeStateBlocks HardwareBaseWPtr hardware_ptr_; ProcessorBasePtrList processor_list_; - static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory) + static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory) - unsigned int sensor_id_; // sensor ID + unsigned int sensor_id_; // sensor ID std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks - CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree) + CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree) protected: std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s] - YAML::Node params_; ///< Params yaml node + YAML::Node params_; ///< Params yaml node - int dim_compatible_; ///< Dimension compatibility of the sensor: 2: 2D, 3: 3D, -1: both + int dim_compatible_; ///< Dimension compatibility of the sensor: 2: 2D, 3: 3D, -1: both void setProblem(ProblemPtr _problem) override final; diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 3ac570830a5959ad3094ad19c30544078d0086b0..087ea3b8237351bf309da17eec8723933bfb351f 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -24,13 +24,12 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(SensorDiffDrive); class SensorDiffDrive : public SensorBase { SensorDiffDrive(const YAML::Node& _params); - + public: WOLF_SENSOR_CREATE(SensorDiffDrive); diff --git a/include/core/sensor/sensor_motion_model.h b/include/core/sensor/sensor_motion_model.h index 792f32714c65064eac962a1207a1124d8d2590fe..c4ed862cc81f0b8b34e9d7b01625ccce198aae1c 100644 --- a/include/core/sensor/sensor_motion_model.h +++ b/include/core/sensor/sensor_motion_model.h @@ -25,7 +25,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(SensorMotionModel); class SensorMotionModel : public SensorBase diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h index fcab5b0a9c35fdb4524a88bae0badfee39170345..89eb373a1021254585d6c9c5111e184c25eb3ad3 100644 --- a/include/core/sensor/sensor_odom.h +++ b/include/core/sensor/sensor_odom.h @@ -26,14 +26,11 @@ namespace wolf { - template <unsigned int DIM> class SensorOdom : public SensorBase { protected: - - SensorOdom(const YAML::Node& _params) - : SensorBase("SensorOdom" + toString(DIM) + "d", DIM, _params) + SensorOdom(const YAML::Node& _params) : SensorBase("SensorOdom" + toString(DIM) + "d", DIM, _params) { static_assert(DIM == 2 or DIM == 3); } diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h index cdd591e4d375b8813258cbd0d4c1d561e804d533..57fd3c571e64920ab0241268058496032285e44f 100644 --- a/include/core/sensor/sensor_pose.h +++ b/include/core/sensor/sensor_pose.h @@ -26,14 +26,11 @@ namespace wolf { - template <unsigned int DIM> class SensorPose : public SensorBase { protected: - - SensorPose(const YAML::Node& _params) - : SensorBase("SensorPose" + toString(DIM) + "d", DIM, _params) + SensorPose(const YAML::Node& _params) : SensorBase("SensorPose" + toString(DIM) + "d", DIM, _params) { static_assert(DIM == 2 or DIM == 3); } diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h index f31fa027350998efe133f5828adb398c046bdb31..e320d8437367c27bae1650e8502027593e389ab7 100644 --- a/include/core/solver/factory_solver.h +++ b/include/core/solver/factory_solver.h @@ -24,7 +24,7 @@ namespace wolf { class SensorBase; struct ParamsSensorBase; -} +} // namespace wolf // wolf #include "core/common/factory.h" @@ -34,36 +34,37 @@ struct ParamsSensorBase; namespace wolf { - /** \brief Solver factory class * */ -typedef Factory<std::shared_ptr<SolverManager>, - const ProblemPtr&, - const YAML::Node&> FactorySolver; +typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const YAML::Node&> FactorySolver; -template<> +template <> inline std::string FactorySolver::getClass() const { - return "FactorySolver"; + return "FactorySolver"; } -typedef Factory<std::shared_ptr<SolverManager>, - const ProblemPtr&, - const std::string&, - const std::vector<std::string>&> FactorySolverYaml; +typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const std::string&, const std::vector<std::string>&> + FactorySolverYaml; -template<> +template <> inline std::string FactorySolverYaml::getClass() const { - return "FactorySolverYaml"; + return "FactorySolverYaml"; } -#define WOLF_REGISTER_SOLVER(SolverType) \ - namespace{ const bool WOLF_UNUSED SolverType##Registered = \ - wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \ - namespace{ const bool WOLF_UNUSED SolverType##RegisteredYaml = \ - wolf::FactorySolverYaml::registerCreator(#SolverType, SolverType::create); } \ +#define WOLF_REGISTER_SOLVER(SolverType) \ + namespace \ + { \ + const bool WOLF_UNUSED SolverType##Registered = \ + wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); \ + } \ + namespace \ + { \ + const bool WOLF_UNUSED SolverType##RegisteredYaml = \ + wolf::FactorySolverYaml::registerCreator(#SolverType, SolverType::create); \ + } } /* namespace wolf */ diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index c4e02ede2faa274ddb2a4a3fa4d8314ccd381fa1..6e190280a6021be2ff18dd20576944d017d60f95 100644 --- a/include/core/solver/solver_manager.h +++ b/include/core/solver/solver_manager.h @@ -20,15 +20,15 @@ #pragma once -//wolf includes +// wolf includes #include "core/common/wolf.h" #include "core/state_block/state_block.h" #include "core/factor/factor_base.h" #include "core/common/params_base.h" #include "core/solver/factory_solver.h" -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(SolverManager) WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver) @@ -44,26 +44,25 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver) * SolverManagerClass(const ProblemPtr& wolf_problem, * const ParamsSolverClassPtr _params); */ -#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \ -static SolverManagerPtr create(const ProblemPtr& _problem, \ - const YAML::Node& _input_node) \ -{ \ - auto params = std::make_shared<ParamsSolverClass>(_input_node); \ - return std::make_shared<SolverClass>(_problem, params); \ -} \ -static SolverManagerPtr create(const ProblemPtr& _problem, \ - const std::string& _yaml_filepath, \ - const std::vector<std::string>& _folders_schema) \ -{ \ - auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ - if (not server.applySchema(#SolverClass)) \ - { \ - WOLF_ERROR(server.getLog()); \ - return nullptr; \ - } \ - auto params = std::make_shared<ParamsSolverClass>(server.getNode()); \ - return std::make_shared<SolverClass>(_problem, params); \ -} \ +#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \ + static SolverManagerPtr create(const ProblemPtr& _problem, const YAML::Node& _input_node) \ + { \ + auto params = std::make_shared<ParamsSolverClass>(_input_node); \ + return std::make_shared<SolverClass>(_problem, params); \ + } \ + static SolverManagerPtr create(const ProblemPtr& _problem, \ + const std::string& _yaml_filepath, \ + const std::vector<std::string>& _folders_schema) \ + { \ + auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ + if (not server.applySchema(#SolverClass)) \ + { \ + WOLF_ERROR(server.getLog()); \ + return nullptr; \ + } \ + auto params = std::make_shared<ParamsSolverClass>(server.getNode()); \ + return std::make_shared<SolverClass>(_problem, params); \ + } struct ParamsSolver; @@ -72,206 +71,199 @@ struct ParamsSolver; */ class SolverManager { - public: - - /** \brief Enumeration of covariance blocks to be computed - * - * Enumeration of covariance blocks to be computed - * - */ - enum class CovarianceBlocksToBeComputed : std::size_t - { - ALL = 0, ///< All blocks and all cross-covariances - ALL_MARGINALS = 1, ///< All marginals - ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks - GAUSS = 3, ///< GAUSS: last frame P and V - GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T - GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T - }; - - /** - * \brief Enumeration for the verbosity of the solver report. - */ - enum class ReportVerbosity : std::size_t - { - QUIET = 0, - BRIEF = 1, - FULL = 2 - }; - - // PROFILING - unsigned int n_solve_, n_cov_; - std::chrono::microseconds acc_duration_total_; - std::chrono::microseconds acc_duration_solver_; - std::chrono::microseconds acc_duration_update_; - std::chrono::microseconds acc_duration_state_; - std::chrono::microseconds acc_duration_cov_; - - std::chrono::microseconds max_duration_total_; - std::chrono::microseconds max_duration_solver_; - std::chrono::microseconds max_duration_update_; - std::chrono::microseconds max_duration_state_; - std::chrono::microseconds max_duration_cov_; - - void printProfiling(std::ostream& stream = std::cout) const; - - protected: - - ProblemPtr wolf_problem_; - ParamsSolverPtr params_; - - public: - /** - * \brief Constructor with default params_ - */ - SolverManager(const ProblemPtr& _problem); - /** - * \brief Constructor with given params_ - */ - SolverManager(const ProblemPtr& _problem, - const ParamsSolverPtr& _params); - - virtual ~SolverManager(); - - /** - * \brief Solves with the verbosity defined in params_ - */ - std::string solve(); - /** - * \brief Solves with a given verbosity - */ - std::string solve(const ReportVerbosity report_level); - - virtual bool computeCovariances() final; - - virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final; - - virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final; - - - /** - * \brief Updates solver's problem according to the wolf_problem - */ - virtual void update(); - - ProblemPtr getProblem(); + public: + /** \brief Enumeration of covariance blocks to be computed + * + * Enumeration of covariance blocks to be computed + * + */ + enum class CovarianceBlocksToBeComputed : std::size_t + { + ALL = 0, ///< All blocks and all cross-covariances + ALL_MARGINALS = 1, ///< All marginals + ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current + ///< robot and all landmarks + GAUSS = 3, ///< GAUSS: last frame P and V + GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T + GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T + }; + + /** + * \brief Enumeration for the verbosity of the solver report. + */ + enum class ReportVerbosity : std::size_t + { + QUIET = 0, + BRIEF = 1, + FULL = 2 + }; + + // PROFILING + unsigned int n_solve_, n_cov_; + std::chrono::microseconds acc_duration_total_; + std::chrono::microseconds acc_duration_solver_; + std::chrono::microseconds acc_duration_update_; + std::chrono::microseconds acc_duration_state_; + std::chrono::microseconds acc_duration_cov_; + + std::chrono::microseconds max_duration_total_; + std::chrono::microseconds max_duration_solver_; + std::chrono::microseconds max_duration_update_; + std::chrono::microseconds max_duration_state_; + std::chrono::microseconds max_duration_cov_; + + void printProfiling(std::ostream& stream = std::cout) const; + + protected: + ProblemPtr wolf_problem_; + ParamsSolverPtr params_; + + public: + /** + * \brief Constructor with default params_ + */ + SolverManager(const ProblemPtr& _problem); + /** + * \brief Constructor with given params_ + */ + SolverManager(const ProblemPtr& _problem, const ParamsSolverPtr& _params); + + virtual ~SolverManager(); + + /** + * \brief Solves with the verbosity defined in params_ + */ + std::string solve(); + /** + * \brief Solves with a given verbosity + */ + std::string solve(const ReportVerbosity report_level); + + virtual bool computeCovariances() final; + + virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final; + + virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final; + + /** + * \brief Updates solver's problem according to the wolf_problem + */ + virtual void update(); + + ProblemPtr getProblem(); + + virtual ParamsSolverPtr getParams() const; + + double getPeriod() const; + + double getCovPeriod() const; + + ReportVerbosity getVerbosity() const; + + virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final; + + virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final; + + virtual bool isStateBlockFixed(const StateBlockPtr& st) final; + + virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final; + + virtual bool hasThisLocalParametrization(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) final; + + virtual bool hasLocalParametrization(const StateBlockPtr& st) const final; + + virtual int numFactors() const final; + virtual int numStateBlocks() const final; + virtual int numStateBlocksFloating() const final; + + virtual int numFactorsDerived() const = 0; + virtual int numStateBlocksDerived() const = 0; - virtual ParamsSolverPtr getParams() const; + virtual bool check(std::string prefix = "") const final; - double getPeriod() const; + protected: + std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_; + std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_; + std::set<FactorBasePtr> factors_; + std::set<StateBlockPtr> floating_state_blocks_; - double getCovPeriod() const; + virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr); + const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const; + double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr); - ReportVerbosity getVerbosity() const; - - virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final; - - virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final; - - virtual bool isStateBlockFixed(const StateBlockPtr& st) final; - - virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final; - - virtual bool hasThisLocalParametrization(const StateBlockPtr& st, - const LocalParametrizationBasePtr& local_param) final; - - virtual bool hasLocalParametrization(const StateBlockPtr& st) const final; - - virtual int numFactors() const final; - virtual int numStateBlocks() const final; - virtual int numStateBlocksFloating() const final; - - virtual int numFactorsDerived() const = 0; - virtual int numStateBlocksDerived() const = 0; - - virtual bool check(std::string prefix="") const final; - - protected: - - std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_; - std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_; - std::set<FactorBasePtr> factors_; - std::set<StateBlockPtr> floating_state_blocks_; - - virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr); - const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const; - double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr); - - private: - // SolverManager functions - virtual void addFactor(const FactorBasePtr& fac_ptr) final; - virtual void removeFactor(const FactorBasePtr& fac_ptr) final; - virtual void addStateBlock(const StateBlockPtr& state_ptr) final; - virtual void removeStateBlock(const StateBlockPtr& state_ptr) final; - virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final; - virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final; - virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final; + private: + // SolverManager functions + virtual void addFactor(const FactorBasePtr& fac_ptr) final; + virtual void removeFactor(const FactorBasePtr& fac_ptr) final; + virtual void addStateBlock(const StateBlockPtr& state_ptr) final; + virtual void removeStateBlock(const StateBlockPtr& state_ptr) final; + virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final; + virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final; + virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final; // Derived virtual functions - protected: - virtual std::string solveDerived(const ReportVerbosity report_level) = 0; - virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0; - virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0; - - virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0; - virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0; - virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0; - virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0; - virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0; - virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0; - - virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0; - virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0; - virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0; - virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0; - virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, - const LocalParametrizationBasePtr& local_param) = 0; - - virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0; - virtual bool checkDerived(std::string prefix="") const = 0; - - public: - virtual bool hasConverged() = 0; - virtual bool wasStopped() = 0; - virtual unsigned int iterations() = 0; - virtual double initialCost() = 0; - virtual double finalCost() = 0; - virtual double totalTime() = 0; - + protected: + virtual std::string solveDerived(const ReportVerbosity report_level) = 0; + virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0; + virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0; + + virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0; + virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0; + virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0; + virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0; + virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0; + virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0; + + virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0; + virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0; + virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0; + virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0; + virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) = 0; + + virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0; + virtual bool checkDerived(std::string prefix = "") const = 0; + + public: + virtual bool hasConverged() = 0; + virtual bool wasStopped() = 0; + virtual unsigned int iterations() = 0; + virtual double initialCost() = 0; + virtual double finalCost() = 0; + virtual double totalTime() = 0; }; // Params (here becaure it needs of declaration of SolverManager::ReportVerbosity) -struct ParamsSolver: public ParamsBase +struct ParamsSolver : public ParamsBase { - double period = 0.0; - SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET; - bool compute_cov = false; - SolverManager::CovarianceBlocksToBeComputed cov_enum = SolverManager::CovarianceBlocksToBeComputed::ROBOT_LANDMARKS; - double cov_period = 1.0; - - ParamsSolver() = default; - ParamsSolver(const YAML::Node& _input_node): - ParamsBase(_input_node) - { - period = _input_node["period"].as<double>(); - verbose = (SolverManager::ReportVerbosity)_input_node["verbose"].as<int>(); - compute_cov = _input_node["compute_cov"].as<bool>(); - if (compute_cov) - { - cov_enum = (SolverManager::CovarianceBlocksToBeComputed)_input_node["cov_enum"].as<int>(); - cov_period = _input_node["cov_period"].as<double>(); - } - } - std::string print() const override + double period = 0.0; + SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET; + bool compute_cov = false; + SolverManager::CovarianceBlocksToBeComputed cov_enum = + SolverManager::CovarianceBlocksToBeComputed::ROBOT_LANDMARKS; + double cov_period = 1.0; + + ParamsSolver() = default; + ParamsSolver(const YAML::Node& _input_node) : ParamsBase(_input_node) + { + period = _input_node["period"].as<double>(); + verbose = (SolverManager::ReportVerbosity)_input_node["verbose"].as<int>(); + compute_cov = _input_node["compute_cov"].as<bool>(); + if (compute_cov) { - return "period: " + toString(period) + "\n" + - "verbose: " + toString((int)verbose) + "\n" + - "compute_cov: " + toString(compute_cov) + "\n" + - "cov_enum: " + toString((int)cov_enum) + "\n" + - "cov_period: " + toString(cov_period) + "\n"; + cov_enum = (SolverManager::CovarianceBlocksToBeComputed)_input_node["cov_enum"].as<int>(); + cov_period = _input_node["cov_period"].as<double>(); } - - ~ParamsSolver() override = default; + } + std::string print() const override + { + return "period: " + toString(period) + "\n" + "verbose: " + toString((int)verbose) + "\n" + + "compute_cov: " + toString(compute_cov) + "\n" + "cov_enum: " + toString((int)cov_enum) + "\n" + + "cov_period: " + toString(cov_period) + "\n"; + } + + ~ParamsSolver() override = default; }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/solver_suitesparse/ccolamd_ordering.h b/include/core/solver_suitesparse/ccolamd_ordering.h index 12bb7e698d5dfef5ee11c934344e1cb613d86f5c..7272b3a1b898afe2eeb02661a8c81ac308ff8383 100644 --- a/include/core/solver_suitesparse/ccolamd_ordering.h +++ b/include/core/solver_suitesparse/ccolamd_ordering.h @@ -20,7 +20,7 @@ #pragma once -//std includes +// std includes #include <iostream> // Eigen includes @@ -31,60 +31,63 @@ // ccolamd #include "ccolamd.h" -namespace Eigen{ - -template<typename Index> +namespace Eigen +{ +template <typename Index> class CCOLAMDOrdering { - public: - typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType; - typedef Matrix<Index, Dynamic, 1> IndexVector; + public: + typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType; + typedef Matrix<Index, Dynamic, 1> IndexVector; - template<typename MatrixType> - void operator()(const MatrixType& mat, PermutationType& perm, Index* cmember = nullptr) - { - Index m = mat.rows(); - Index n = mat.cols(); - Index nnz = mat.nonZeros(); + template <typename MatrixType> + void operator()(const MatrixType& mat, PermutationType& perm, Index* cmember = nullptr) + { + Index m = mat.rows(); + Index n = mat.cols(); + Index nnz = mat.nonZeros(); - // Get the recommended value of Alen to be used by colamd - Index Alen = ccolamd_recommended(nnz, m, n); - // Set the default parameters - double knobs[CCOLAMD_KNOBS]; - Index stats[CCOLAMD_STATS]; - ccolamd_set_defaults(knobs); + // Get the recommended value of Alen to be used by colamd + Index Alen = ccolamd_recommended(nnz, m, n); + // Set the default parameters + double knobs[CCOLAMD_KNOBS]; + Index stats[CCOLAMD_STATS]; + ccolamd_set_defaults(knobs); - IndexVector p(n + 1), A(Alen); - for (Index i = 0; i <= n; i++) - p(i) = mat.outerIndex()[i]; - for (Index i = 0; i < nnz; i++) - A(i) = mat.innerIndex()[i]; -// std::cout << "p = " << std::endl << p.transpose() << std::endl; -// std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl; + IndexVector p(n + 1), A(Alen); + for (Index i = 0; i <= n; i++) p(i) = mat.outerIndex()[i]; + for (Index i = 0; i < nnz; i++) A(i) = mat.innerIndex()[i]; + // std::cout << "p = " << std::endl << p.transpose() << std::endl; + // std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl; - // Call CColamd routine to compute the ordering - Index info = compute_ccolamd(m, n, Alen, A.data(), p.data(), knobs, stats, cmember); - if (!info) - assert(info && "CCOLAMD failed "); + // Call CColamd routine to compute the ordering + Index info = compute_ccolamd(m, n, Alen, A.data(), p.data(), knobs, stats, cmember); + if (!info) assert(info && "CCOLAMD failed "); - perm.resize(n); - for (Index i = 0; i < n; i++) - perm.indices()(p(i)) = i; - } + perm.resize(n); + for (Index i = 0; i < n; i++) perm.indices()(p(i)) = i; + } - private: - int compute_ccolamd(int &m, int &n, int &Alen, int* A, int* p, double* knobs, int* stats, int* cmember) - { - int info = ccolamd(m, n, Alen, A, p, knobs, stats, cmember); - //ccolamd_report (stats) ; - return info; - } + private: + int compute_ccolamd(int& m, int& n, int& Alen, int* A, int* p, double* knobs, int* stats, int* cmember) + { + int info = ccolamd(m, n, Alen, A, p, knobs, stats, cmember); + // ccolamd_report (stats) ; + return info; + } - long int compute_ccolamd(long int &m, long int &n, long int &Alen, long int* A, long int* p, double* knobs, long int* stats, long int* cmember) - { - long int info = ccolamd_l(m, n, Alen, A, p, knobs, stats, cmember); - //ccolamd_l_report (stats) ; - return info; - } + long int compute_ccolamd(long int& m, + long int& n, + long int& Alen, + long int* A, + long int* p, + double* knobs, + long int* stats, + long int* cmember) + { + long int info = ccolamd_l(m, n, Alen, A, p, knobs, stats, cmember); + // ccolamd_l_report (stats) ; + return info; + } }; -} +} // namespace Eigen diff --git a/include/core/solver_suitesparse/cost_function_base.h b/include/core/solver_suitesparse/cost_function_base.h index 3668a5bf738311545fd14cc27fed85d8ec341524..75e62aba983ec2384183dd8d09fb3bc2e9458ccd 100644 --- a/include/core/solver_suitesparse/cost_function_base.h +++ b/include/core/solver_suitesparse/cost_function_base.h @@ -25,83 +25,88 @@ class CostFunctionBase { - protected: - unsigned int n_blocks_; - Eigen::MatrixXd J_0_; - Eigen::MatrixXd J_1_; - Eigen::MatrixXd J_2_; - Eigen::MatrixXd J_3_; - Eigen::MatrixXd J_4_; - Eigen::MatrixXd J_5_; - Eigen::MatrixXd J_6_; - Eigen::MatrixXd J_7_; - Eigen::MatrixXd J_8_; - Eigen::MatrixXd J_9_; - Eigen::VectorXd residual_; - std::vector<Eigen::MatrixXd*> jacobians_; - std::vector<unsigned int> block_sizes_; + protected: + unsigned int n_blocks_; + Eigen::MatrixXd J_0_; + Eigen::MatrixXd J_1_; + Eigen::MatrixXd J_2_; + Eigen::MatrixXd J_3_; + Eigen::MatrixXd J_4_; + Eigen::MatrixXd J_5_; + Eigen::MatrixXd J_6_; + Eigen::MatrixXd J_7_; + Eigen::MatrixXd J_8_; + Eigen::MatrixXd J_9_; + Eigen::VectorXd residual_; + std::vector<Eigen::MatrixXd *> jacobians_; + std::vector<unsigned int> block_sizes_; - public: - CostFunctionBase(const unsigned int &_measurement_size, - const unsigned int &_block_0_size, - const unsigned int &_block_1_size, - const unsigned int &_block_2_size, - const unsigned int &_block_3_size, - const unsigned int &_block_4_size, - const unsigned int &_block_5_size, - const unsigned int &_block_6_size, - const unsigned int &_block_7_size, - const unsigned int &_block_8_size, - const unsigned int &_block_9_size) : - n_blocks_(10), - J_0_(_measurement_size, _block_0_size), - J_1_(_measurement_size, _block_1_size), - J_2_(_measurement_size, _block_2_size), - J_3_(_measurement_size, _block_3_size), - J_4_(_measurement_size, _block_4_size), - J_5_(_measurement_size, _block_5_size), - J_6_(_measurement_size, _block_6_size), - J_7_(_measurement_size, _block_7_size), - J_8_(_measurement_size, _block_8_size), - J_9_(_measurement_size, _block_9_size), - residual_(_measurement_size), - jacobians_({&J_0_,&J_1_,&J_2_,&J_3_,&J_4_,&J_5_,&J_6_,&J_7_,&J_8_,&J_9_}), - block_sizes_({_block_0_size, _block_1_size, _block_2_size, _block_3_size, _block_4_size, _block_5_size, _block_6_size, _block_7_size, _block_8_size, _block_9_size}) + public: + CostFunctionBase(const unsigned int &_measurement_size, + const unsigned int &_block_0_size, + const unsigned int &_block_1_size, + const unsigned int &_block_2_size, + const unsigned int &_block_3_size, + const unsigned int &_block_4_size, + const unsigned int &_block_5_size, + const unsigned int &_block_6_size, + const unsigned int &_block_7_size, + const unsigned int &_block_8_size, + const unsigned int &_block_9_size) + : n_blocks_(10), + J_0_(_measurement_size, _block_0_size), + J_1_(_measurement_size, _block_1_size), + J_2_(_measurement_size, _block_2_size), + J_3_(_measurement_size, _block_3_size), + J_4_(_measurement_size, _block_4_size), + J_5_(_measurement_size, _block_5_size), + J_6_(_measurement_size, _block_6_size), + J_7_(_measurement_size, _block_7_size), + J_8_(_measurement_size, _block_8_size), + J_9_(_measurement_size, _block_9_size), + residual_(_measurement_size), + jacobians_({&J_0_, &J_1_, &J_2_, &J_3_, &J_4_, &J_5_, &J_6_, &J_7_, &J_8_, &J_9_}), + block_sizes_({_block_0_size, + _block_1_size, + _block_2_size, + _block_3_size, + _block_4_size, + _block_5_size, + _block_6_size, + _block_7_size, + _block_8_size, + _block_9_size}) + { + for (unsigned int i = 1; i < n_blocks_; i++) + { + if (block_sizes_.at(i) == 0) { - for (unsigned int i = 1; i<n_blocks_; i++) - { - if (block_sizes_.at(i) == 0) - { - n_blocks_ = i; - jacobians_.resize(n_blocks_); - block_sizes_.resize(n_blocks_); - break; - } - } + n_blocks_ = i; + jacobians_.resize(n_blocks_); + block_sizes_.resize(n_blocks_); + break; } - - virtual ~CostFunctionBase() - { - } + } - virtual void evaluateResidualJacobians() = 0; + virtual ~CostFunctionBase() {} - void getResidual(Eigen::VectorXd &residual) - { - residual.resize(residual_.size()); - residual = residual_; - } + virtual void evaluateResidualJacobians() = 0; - std::vector<Eigen::MatrixXd*> getJacobians() - { - return jacobians_; - } + void getResidual(Eigen::VectorXd &residual) + { + residual.resize(residual_.size()); + residual = residual_; + } - void getJacobians(std::vector<Eigen::MatrixXd>& jacobians) - { - jacobians.resize(n_blocks_); - for (unsigned int i = 0; i<n_blocks_; i++) - jacobians.at(i) = (*jacobians_.at(i)); - } + std::vector<Eigen::MatrixXd *> getJacobians() + { + return jacobians_; + } + + void getJacobians(std::vector<Eigen::MatrixXd> &jacobians) + { + jacobians.resize(n_blocks_); + for (unsigned int i = 0; i < n_blocks_; i++) jacobians.at(i) = (*jacobians_.at(i)); + } }; diff --git a/include/core/solver_suitesparse/cost_function_sparse.h b/include/core/solver_suitesparse/cost_function_sparse.h index 52d94c4c15016a1b20c66fa7ecd13cc4bc9af187..1c1c77f95d14c21d00d49e12cf531f45a7aab8b4 100644 --- a/include/core/solver_suitesparse/cost_function_sparse.h +++ b/include/core/solver_suitesparse/cost_function_sparse.h @@ -20,7 +20,7 @@ #pragma once -//wolf includes +// wolf includes #include "core/common/wolf.h" #include "cost_function_sparse_base.h" @@ -29,100 +29,133 @@ namespace wolf { - template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE, - unsigned int BLOCK_7_SIZE, - unsigned int BLOCK_8_SIZE, - unsigned int BLOCK_9_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE, + unsigned int BLOCK_7_SIZE, + unsigned int BLOCK_8_SIZE, + unsigned int BLOCK_9_SIZE> class CostFunctionSparse : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - BLOCK_8_SIZE, - BLOCK_9_SIZE> + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + BLOCK_9_SIZE> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - BLOCK_8_SIZE, - BLOCK_9_SIZE>(_factor_ptr) - { - - } - - void callFunctor() - { -// if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 && -// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE > 0) - (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ , this->jets_9_ ,this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 && -// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 && -// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 && -// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 && -// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 && -// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE ==0 && -// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 && -// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 && -// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_); -// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE ==0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 && -// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0) -// (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_); -// else -// assert("Wrong combination of zero sized blocks"); - - } - + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparse<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + BLOCK_9_SIZE>(_factor_ptr) + { + } + + void callFunctor() + { + // if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE + // > 0 && + // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE + // > 0) + (*this->factor_ptr_)(this->jets_0_, + this->jets_1_, + this->jets_2_, + this->jets_3_, + this->jets_4_, + this->jets_5_, + this->jets_6_, + this->jets_7_, + this->jets_8_, + this->jets_9_, + this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && + // BLOCK_4_SIZE > 0 && + // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, + // this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ + // ,this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && + // BLOCK_4_SIZE > 0 && + // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE ==0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, + // this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && + // BLOCK_4_SIZE > 0 && + // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, + // this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && + // BLOCK_4_SIZE > 0 && + // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, + // this->jets_4_, this->jets_5_,this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && + // BLOCK_4_SIZE > 0 && + // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, + // this->jets_4_, this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && + // BLOCK_4_SIZE ==0 && + // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, + // this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE ==0 && + // BLOCK_4_SIZE ==0 && + // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && + // BLOCK_4_SIZE ==0 && + // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_); + // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE ==0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && + // BLOCK_4_SIZE ==0 && + // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && + // BLOCK_9_SIZE == 0) + // (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_); + // else + // assert("Wrong combination of zero sized blocks"); + } }; template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE, - unsigned int BLOCK_7_SIZE, - unsigned int BLOCK_8_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE, + unsigned int BLOCK_7_SIZE, + unsigned int BLOCK_8_SIZE> class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, @@ -147,380 +180,343 @@ class CostFunctionSparse<FactorT, BLOCK_8_SIZE, 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - BLOCK_8_SIZE, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + 0>(_factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)(this->jets_0_, + this->jets_1_, + this->jets_2_, + this->jets_3_, + this->jets_4_, + this->jets_5_, + this->jets_6_, + this->jets_7_, + this->jets_8_, + this->residuals_jet_); + } }; template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE, - unsigned int BLOCK_7_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE, + unsigned int BLOCK_7_SIZE> class CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - 0, - 0> : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - 0, - 0> + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + 0, + 0> : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + 0, + 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - 0, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + 0, + 0>(_factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)(this->jets_0_, + this->jets_1_, + this->jets_2_, + this->jets_3_, + this->jets_4_, + this->jets_5_, + this->jets_6_, + this->jets_7_, + this->residuals_jet_); + } }; template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE> class CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - 0, - 0> : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - 0, - 0> + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + 0, + 0> : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + 0, + 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - 0, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + 0, + 0>(_factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)(this->jets_0_, + this->jets_1_, + this->jets_2_, + this->jets_3_, + this->jets_4_, + this->jets_5_, + this->jets_6_, + this->residuals_jet_); + } }; template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE> class CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - 0, - 0> : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - 0, - 0> + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + 0, + 0> : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + 0, + 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - 0, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + 0, + 0>(_factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)(this->jets_0_, + this->jets_1_, + this->jets_2_, + this->jets_3_, + this->jets_4_, + this->jets_5_, + this->residuals_jet_); + } }; template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE> class CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - 0, - 0> : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - 0, - 0> + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + 0, + 0> : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + 0, + 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - 0, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + 0, + 0>(_factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)( + this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->residuals_jet_); + } }; template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE> -class CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - 0, - 0> : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - 0, - 0> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE> +class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, 0, 0> + : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, 0, 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - 0, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + 0, + 0>(_factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->residuals_jet_); + } }; template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE> -class CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - 0, - 0> : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - 0, - 0> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE> +class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, 0> + : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - 0, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, 0>( + _factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_); + } }; -template <class FactorT, - const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE> -class CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - 0, - 0> : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - 0, - 0> +template <class FactorT, const unsigned int MEASUREMENT_SIZE, unsigned int BLOCK_0_SIZE, unsigned int BLOCK_1_SIZE> +class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, 0> + : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - 0, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_, this->jets_1_,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, 0>(_factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_); + } }; -template <class FactorT, - const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE> -class CostFunctionSparse<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - 0, - 0> : CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - 0, - 0> +template <class FactorT, const unsigned int MEASUREMENT_SIZE, unsigned int BLOCK_0_SIZE> +class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, 0> + : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, 0> { - public: - CostFunctionSparse(FactorT* _factor_ptr) : - CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - 0, - 0>(_factor_ptr) - { - - } - - void callFunctor() - { - (*this->factor_ptr_)(this->jets_0_,this->residuals_jet_); - } + public: + CostFunctionSparse(FactorT* _factor_ptr) + : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, 0>(_factor_ptr) + { + } + + void callFunctor() + { + (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_); + } }; -} //namespace wolf +} // namespace wolf diff --git a/include/core/solver_suitesparse/cost_function_sparse_base.h b/include/core/solver_suitesparse/cost_function_sparse_base.h index 26e9dc23eebf57ab6b953ee599fffc613263a847..4983d82c1b9d2ec82fe39e35ae8b8f53fda17206 100644 --- a/include/core/solver_suitesparse/cost_function_sparse_base.h +++ b/include/core/solver_suitesparse/cost_function_sparse_base.h @@ -20,7 +20,7 @@ #pragma once -//wolf includes +// wolf includes #include "core/common/wolf.h" #include "cost_function_base.h" @@ -29,174 +29,175 @@ namespace wolf { - template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE = 0, - unsigned int BLOCK_2_SIZE = 0, - unsigned int BLOCK_3_SIZE = 0, - unsigned int BLOCK_4_SIZE = 0, - unsigned int BLOCK_5_SIZE = 0, - unsigned int BLOCK_6_SIZE = 0, - unsigned int BLOCK_7_SIZE = 0, - unsigned int BLOCK_8_SIZE = 0, - unsigned int BLOCK_9_SIZE = 0> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE = 0, + unsigned int BLOCK_2_SIZE = 0, + unsigned int BLOCK_3_SIZE = 0, + unsigned int BLOCK_4_SIZE = 0, + unsigned int BLOCK_5_SIZE = 0, + unsigned int BLOCK_6_SIZE = 0, + unsigned int BLOCK_7_SIZE = 0, + unsigned int BLOCK_8_SIZE = 0, + unsigned int BLOCK_9_SIZE = 0> class CostFunctionSparseBase : CostFunctionBase { - typedef ceres::Jet<double, BLOCK_0_SIZE + - BLOCK_1_SIZE + - BLOCK_2_SIZE + - BLOCK_3_SIZE + - BLOCK_4_SIZE + - BLOCK_5_SIZE + - BLOCK_6_SIZE + - BLOCK_7_SIZE + - BLOCK_8_SIZE + - BLOCK_9_SIZE> WolfJet; - protected: - FactorT* factor_ptr_; - WolfJet jets_0_[BLOCK_0_SIZE]; - WolfJet jets_1_[BLOCK_1_SIZE]; - WolfJet jets_2_[BLOCK_2_SIZE]; - WolfJet jets_3_[BLOCK_3_SIZE]; - WolfJet jets_4_[BLOCK_4_SIZE]; - WolfJet jets_5_[BLOCK_5_SIZE]; - WolfJet jets_6_[BLOCK_6_SIZE]; - WolfJet jets_7_[BLOCK_7_SIZE]; - WolfJet jets_8_[BLOCK_8_SIZE]; - WolfJet jets_9_[BLOCK_9_SIZE]; - WolfJet residuals_jet_[MEASUREMENT_SIZE]; - - public: + typedef ceres::Jet<double, + BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + BLOCK_5_SIZE + + BLOCK_6_SIZE + BLOCK_7_SIZE + BLOCK_8_SIZE + BLOCK_9_SIZE> + WolfJet; - /** \brief Constructor with factor pointer - * - * Constructor with factor pointer - * - */ - CostFunctionSparseBase(FactorT* _factor_ptr); + protected: + FactorT* factor_ptr_; + WolfJet jets_0_[BLOCK_0_SIZE]; + WolfJet jets_1_[BLOCK_1_SIZE]; + WolfJet jets_2_[BLOCK_2_SIZE]; + WolfJet jets_3_[BLOCK_3_SIZE]; + WolfJet jets_4_[BLOCK_4_SIZE]; + WolfJet jets_5_[BLOCK_5_SIZE]; + WolfJet jets_6_[BLOCK_6_SIZE]; + WolfJet jets_7_[BLOCK_7_SIZE]; + WolfJet jets_8_[BLOCK_8_SIZE]; + WolfJet jets_9_[BLOCK_9_SIZE]; + WolfJet residuals_jet_[MEASUREMENT_SIZE]; - /** \brief Default destructor - * - * Default destructor - * - */ - virtual ~CostFunctionSparseBase(); + public: + /** \brief Constructor with factor pointer + * + * Constructor with factor pointer + * + */ + CostFunctionSparseBase(FactorT* _factor_ptr); - /** \brief Evaluate residuals and jacobians of the factor in the current x - * - * Evaluate residuals and jacobians of the factor in the current x - * - */ - virtual void evaluateResidualJacobians(); + /** \brief Default destructor + * + * Default destructor + * + */ + virtual ~CostFunctionSparseBase(); - protected: + /** \brief Evaluate residuals and jacobians of the factor in the current x + * + * Evaluate residuals and jacobians of the factor in the current x + * + */ + virtual void evaluateResidualJacobians(); - /** \brief Calls the functor of the factor evaluating jets - * - * Calls the functor of the factor evaluating jets - * - */ - virtual void callFunctor() = 0; + protected: + /** \brief Calls the functor of the factor evaluating jets + * + * Calls the functor of the factor evaluating jets + * + */ + virtual void callFunctor() = 0; - /** \brief Initialize the infinitesimal part of jets - * - * Initialize the infinitesimal part of jets with zeros and ones - * - */ - void initializeJets(); + /** \brief Initialize the infinitesimal part of jets + * + * Initialize the infinitesimal part of jets with zeros and ones + * + */ + void initializeJets(); - /** \brief Gets the evaluation point - * - * Gets the evaluation point from the state - * - */ - void evaluateX(); + /** \brief Gets the evaluation point + * + * Gets the evaluation point from the state + * + */ + void evaluateX(); }; template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE, - unsigned int BLOCK_7_SIZE, - unsigned int BLOCK_8_SIZE, - unsigned int BLOCK_9_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE, + unsigned int BLOCK_7_SIZE, + unsigned int BLOCK_8_SIZE, + unsigned int BLOCK_9_SIZE> CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - BLOCK_8_SIZE, - BLOCK_9_SIZE>::CostFunctionSparseBase(FactorT* _factor_ptr) : - CostFunctionBase(MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE, BLOCK_5_SIZE, BLOCK_6_SIZE, BLOCK_7_SIZE, BLOCK_8_SIZE,BLOCK_9_SIZE), - factor_ptr_(_factor_ptr) + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + BLOCK_9_SIZE>::CostFunctionSparseBase(FactorT* _factor_ptr) + : CostFunctionBase(MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + BLOCK_9_SIZE), + factor_ptr_(_factor_ptr) { initializeJets(); } template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE, - unsigned int BLOCK_7_SIZE, - unsigned int BLOCK_8_SIZE, - unsigned int BLOCK_9_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE, + unsigned int BLOCK_7_SIZE, + unsigned int BLOCK_8_SIZE, + unsigned int BLOCK_9_SIZE> CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - BLOCK_8_SIZE, - BLOCK_9_SIZE>::~CostFunctionSparseBase() + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + BLOCK_9_SIZE>::~CostFunctionSparseBase() { - } template <class FactorT, const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE, - unsigned int BLOCK_7_SIZE, - unsigned int BLOCK_8_SIZE, - unsigned int BLOCK_9_SIZE> + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE, + unsigned int BLOCK_7_SIZE, + unsigned int BLOCK_8_SIZE, + unsigned int BLOCK_9_SIZE> void CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - BLOCK_8_SIZE, - BLOCK_9_SIZE>::evaluateResidualJacobians() + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + BLOCK_9_SIZE>::evaluateResidualJacobians() { evaluateX(); @@ -204,7 +205,7 @@ void CostFunctionSparseBase<FactorT, // fill the jacobian matrices int jacobian_location = 0; - for (int i = 0; i<n_blocks_; i++) + for (int i = 0; i < n_blocks_; i++) { for (int row = 0; row < MEASUREMENT_SIZE; row++) jacobians_.at(i)->row(row) = residuals_jet_[row].v.segment(jacobian_location, block_sizes_.at(i)); @@ -213,123 +214,102 @@ void CostFunctionSparseBase<FactorT, } // fill the residual vector - for (int i = 0; i < MEASUREMENT_SIZE; i++) - residual_(i) = residuals_jet_[i].a; + for (int i = 0; i < MEASUREMENT_SIZE; i++) residual_(i) = residuals_jet_[i].a; } template <class FactorT, -const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE, - unsigned int BLOCK_7_SIZE, - unsigned int BLOCK_8_SIZE, - unsigned int BLOCK_9_SIZE> - void CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - BLOCK_8_SIZE, - BLOCK_9_SIZE>::initializeJets() + const unsigned int MEASUREMENT_SIZE, + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE, + unsigned int BLOCK_7_SIZE, + unsigned int BLOCK_8_SIZE, + unsigned int BLOCK_9_SIZE> +void CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + BLOCK_9_SIZE>::initializeJets() { int last_jet_idx = 0; // JET 0 - for (int i = 0; i < BLOCK_0_SIZE; i++) - jets_0_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_0_SIZE; i++) jets_0_[i] = WolfJet(0, last_jet_idx++); // JET 1 - for (int i = 0; i < BLOCK_1_SIZE; i++) - jets_1_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_1_SIZE; i++) jets_1_[i] = WolfJet(0, last_jet_idx++); // JET 2 - for (int i = 0; i < BLOCK_2_SIZE; i++) - jets_2_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_2_SIZE; i++) jets_2_[i] = WolfJet(0, last_jet_idx++); // JET 3 - for (int i = 0; i < BLOCK_3_SIZE; i++) - jets_3_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_3_SIZE; i++) jets_3_[i] = WolfJet(0, last_jet_idx++); // JET 4 - for (int i = 0; i < BLOCK_4_SIZE; i++) - jets_4_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_4_SIZE; i++) jets_4_[i] = WolfJet(0, last_jet_idx++); // JET 5 - for (int i = 0; i < BLOCK_5_SIZE; i++) - jets_5_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_5_SIZE; i++) jets_5_[i] = WolfJet(0, last_jet_idx++); // JET 6 - for (int i = 0; i < BLOCK_6_SIZE; i++) - jets_6_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_6_SIZE; i++) jets_6_[i] = WolfJet(0, last_jet_idx++); // JET 7 - for (int i = 0; i < BLOCK_7_SIZE; i++) - jets_7_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_7_SIZE; i++) jets_7_[i] = WolfJet(0, last_jet_idx++); // JET 8 - for (int i = 0; i < BLOCK_8_SIZE; i++) - jets_8_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_8_SIZE; i++) jets_8_[i] = WolfJet(0, last_jet_idx++); // JET 9 - for (int i = 0; i < BLOCK_9_SIZE; i++) - jets_9_[i] = WolfJet(0, last_jet_idx++); + for (int i = 0; i < BLOCK_9_SIZE; i++) jets_9_[i] = WolfJet(0, last_jet_idx++); } template <class FactorT, -const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE, - unsigned int BLOCK_2_SIZE, - unsigned int BLOCK_3_SIZE, - unsigned int BLOCK_4_SIZE, - unsigned int BLOCK_5_SIZE, - unsigned int BLOCK_6_SIZE, - unsigned int BLOCK_7_SIZE, - unsigned int BLOCK_8_SIZE, - unsigned int BLOCK_9_SIZE> - void CostFunctionSparseBase<FactorT, - MEASUREMENT_SIZE, - BLOCK_0_SIZE, - BLOCK_1_SIZE, - BLOCK_2_SIZE, - BLOCK_3_SIZE, - BLOCK_4_SIZE, - BLOCK_5_SIZE, - BLOCK_6_SIZE, - BLOCK_7_SIZE, - BLOCK_8_SIZE, - BLOCK_9_SIZE>::evaluateX() + const unsigned int MEASUREMENT_SIZE, + unsigned int BLOCK_0_SIZE, + unsigned int BLOCK_1_SIZE, + unsigned int BLOCK_2_SIZE, + unsigned int BLOCK_3_SIZE, + unsigned int BLOCK_4_SIZE, + unsigned int BLOCK_5_SIZE, + unsigned int BLOCK_6_SIZE, + unsigned int BLOCK_7_SIZE, + unsigned int BLOCK_8_SIZE, + unsigned int BLOCK_9_SIZE> +void CostFunctionSparseBase<FactorT, + MEASUREMENT_SIZE, + BLOCK_0_SIZE, + BLOCK_1_SIZE, + BLOCK_2_SIZE, + BLOCK_3_SIZE, + BLOCK_4_SIZE, + BLOCK_5_SIZE, + BLOCK_6_SIZE, + BLOCK_7_SIZE, + BLOCK_8_SIZE, + BLOCK_9_SIZE>::evaluateX() { // JET 0 - for (int i = 0; i < BLOCK_0_SIZE; i++) - jets_0_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(0)+i); + for (int i = 0; i < BLOCK_0_SIZE; i++) jets_0_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(0) + i); // JET 1 - for (int i = 0; i < BLOCK_1_SIZE; i++) - jets_1_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(1)+i); + for (int i = 0; i < BLOCK_1_SIZE; i++) jets_1_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(1) + i); // JET 2 - for (int i = 0; i < BLOCK_2_SIZE; i++) - jets_2_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(2)+i); + for (int i = 0; i < BLOCK_2_SIZE; i++) jets_2_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(2) + i); // JET 3 - for (int i = 0; i < BLOCK_3_SIZE; i++) - jets_3_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(3)+i); + for (int i = 0; i < BLOCK_3_SIZE; i++) jets_3_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(3) + i); // JET 4 - for (int i = 0; i < BLOCK_4_SIZE; i++) - jets_4_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(4)+i); + for (int i = 0; i < BLOCK_4_SIZE; i++) jets_4_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(4) + i); // JET 5 - for (int i = 0; i < BLOCK_5_SIZE; i++) - jets_5_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(5)+i); + for (int i = 0; i < BLOCK_5_SIZE; i++) jets_5_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(5) + i); // JET 6 - for (int i = 0; i < BLOCK_6_SIZE; i++) - jets_6_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(6)+i); + for (int i = 0; i < BLOCK_6_SIZE; i++) jets_6_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(6) + i); // JET 7 - for (int i = 0; i < BLOCK_7_SIZE; i++) - jets_7_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(7)+i); + for (int i = 0; i < BLOCK_7_SIZE; i++) jets_7_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(7) + i); // JET 8 - for (int i = 0; i < BLOCK_8_SIZE; i++) - jets_8_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(8)+i); + for (int i = 0; i < BLOCK_8_SIZE; i++) jets_8_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(8) + i); // JET 9 - for (int i = 0; i < BLOCK_9_SIZE; i++) - jets_9_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(9)+i); + for (int i = 0; i < BLOCK_9_SIZE; i++) jets_9_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(9) + i); } -} // wolf namespace +} // namespace wolf diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h index 40bae47e7fda25fba87e1bf7961707de96c313e2..a1ca79663e69f5f3daafb187590d1e6603b2686f 100644 --- a/include/core/solver_suitesparse/qr_solver.h +++ b/include/core/solver_suitesparse/qr_solver.h @@ -20,12 +20,12 @@ #pragma once -//std includes +// std includes #include <core/factor/factor_odom_2d.h> #include <iostream> #include <ctime> -//Wolf includes +// Wolf includes #include "core/state_block/state_block.h" #include "../factor_sparse.h" #include "core/factor/factor_corner_2d.h" @@ -44,561 +44,583 @@ namespace wolf { - class SolverQR { - protected: - ProblemPtr problem_ptr_; - Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::NaturalOrdering<int>> solver_; - Eigen::SparseMatrix<double> A_, R_; - Eigen::VectorXd b_, x_incr_; - std::vector<StateBlockPtr> nodes_; - std::vector<FactorBasePtr> factors_; - std::vector<CostFunctionBasePtr> cost_functions_; - - // ordering - Eigen::SparseMatrix<int> A_nodes_; - Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> acc_node_permutation_; - std::map<double*, unsigned int> id_2_idx_; - Eigen::CCOLAMDOrdering<int> orderer_; - Eigen::VectorXi node_ordering_restrictions_; - Eigen::ArrayXi node_locations_; - std::vector<unsigned int> factor_locations_; - unsigned int n_new_factors_; - - // time - clock_t t_ordering_, t_solving_, t_managing_; - double time_ordering_, time_solving_, time_managing_; - - public: - SolverQR(ProblemPtr problem_ptr_) : - problem_ptr_(problem_ptr_), A_(0, 0), R_(0, 0), A_nodes_(0, 0), acc_node_permutation_(0), n_new_factors_( - 0), time_ordering_(0), time_solving_(0), time_managing_(0) + protected: + ProblemPtr problem_ptr_; + Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::NaturalOrdering<int>> solver_; + Eigen::SparseMatrix<double> A_, R_; + Eigen::VectorXd b_, x_incr_; + std::vector<StateBlockPtr> nodes_; + std::vector<FactorBasePtr> factors_; + std::vector<CostFunctionBasePtr> cost_functions_; + + // ordering + Eigen::SparseMatrix<int> A_nodes_; + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> acc_node_permutation_; + std::map<double *, unsigned int> id_2_idx_; + Eigen::CCOLAMDOrdering<int> orderer_; + Eigen::VectorXi node_ordering_restrictions_; + Eigen::ArrayXi node_locations_; + std::vector<unsigned int> factor_locations_; + unsigned int n_new_factors_; + + // time + clock_t t_ordering_, t_solving_, t_managing_; + double time_ordering_, time_solving_, time_managing_; + + public: + SolverQR(ProblemPtr problem_ptr_) + : problem_ptr_(problem_ptr_), + A_(0, 0), + R_(0, 0), + A_nodes_(0, 0), + acc_node_permutation_(0), + n_new_factors_(0), + time_ordering_(0), + time_solving_(0), + time_managing_(0) + { + node_locations_.resize(0); + factor_locations_.resize(0); + } + + virtual ~SolverQR() {} + + void update() + { + // UPDATE STATE BLOCKS + while (!problem_ptr_->getStateBlockNotificationList().empty()) { - node_locations_.resize(0); - factor_locations_.resize(0); - } - - virtual ~SolverQR() - { - - } - - void update() - { - // UPDATE STATE BLOCKS - while (!problem_ptr_->getStateBlockNotificationList().empty()) + switch (problem_ptr_->getStateBlockNotificationList().front().notification_) { - switch (problem_ptr_->getStateBlockNotificationList().front().notification_) - { - case ADD: - { - addStateBlock(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_); - break; - } - case UPDATE: - { - updateStateBlockStatus(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_); - break; - } - case REMOVE: - { - // TODO removeStateBlock((double *)(problem_ptr_->getStateBlockNotificationList().front().scalar_ptr_)); - break; - } - default: - throw std::runtime_error("SolverQR::update: State Block notification must be ADD, UPATE or REMOVE."); + case ADD: { + addStateBlock(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_); + break; + } + case UPDATE: { + updateStateBlockStatus(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_); + break; } - problem_ptr_->getStateBlockNotificationList().pop_front(); + case REMOVE: { + // TODO removeStateBlock((double + // *)(problem_ptr_->getStateBlockNotificationList().front().scalar_ptr_)); + break; + } + default: + throw std::runtime_error( + "SolverQR::update: State Block notification must be ADD, UPATE or REMOVE."); } - // UPDATE FACTORS - while (!problem_ptr_->getFactorNotificationList().empty()) + problem_ptr_->getStateBlockNotificationList().pop_front(); + } + // UPDATE FACTORS + while (!problem_ptr_->getFactorNotificationList().empty()) + { + switch (problem_ptr_->getFactorNotificationList().front().notification_) { - switch (problem_ptr_->getFactorNotificationList().front().notification_) - { - case ADD: - { - addFactor(problem_ptr_->getFactorNotificationList().front().factor_ptr_); - break; - } - case REMOVE: - { - // TODO: removeFactor(problem_ptr_->getFactorNotificationList().front().id_); - break; - } - default: - throw std::runtime_error("SolverQR::update: Factor notification must be ADD or REMOVE."); + case ADD: { + addFactor(problem_ptr_->getFactorNotificationList().front().factor_ptr_); + break; + } + case REMOVE: { + // TODO: removeFactor(problem_ptr_->getFactorNotificationList().front().id_); + break; } - problem_ptr_->getFactorNotificationList().pop_front(); + default: + throw std::runtime_error("SolverQR::update: Factor notification must be ADD or REMOVE."); } + problem_ptr_->getFactorNotificationList().pop_front(); } + } + + void addStateBlock(StateBlockPtr _state_ptr) + { + t_managing_ = clock(); - void addStateBlock(StateBlockPtr _state_ptr) + std::cout << "adding state unit " << _state_ptr->get() << std::endl; + if (!_state_ptr->isFixed()) { - t_managing_ = clock(); + nodes_.push_back(_state_ptr); + id_2_idx_[_state_ptr->get()] = nodes_.size() - 1; - std::cout << "adding state unit " << _state_ptr->get() << std::endl; - if (!_state_ptr->isFixed()) - { - nodes_.push_back(_state_ptr); - id_2_idx_[_state_ptr->get()] = nodes_.size() - 1; + std::cout << "idx " << id_2_idx_[_state_ptr->get()] << std::endl; + + // Resize accumulated permutations + augmentPermutation(acc_node_permutation_, nNodes()); - std::cout << "idx " << id_2_idx_[_state_ptr->get()] << std::endl; + // Resize state + x_incr_.conservativeResize(x_incr_.size() + _state_ptr->getSize()); - // Resize accumulated permutations - augmentPermutation(acc_node_permutation_, nNodes()); + // Resize problem + A_.conservativeResize(A_.rows(), A_.cols() + _state_ptr->getSize()); + R_.conservativeResize(R_.cols() + _state_ptr->getSize(), R_.cols() + _state_ptr->getSize()); + } + time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC; + } - // Resize state - x_incr_.conservativeResize(x_incr_.size() + _state_ptr->getSize()); + void updateStateBlockStatus(StateBlockPtr _state_ptr) + { + // TODO + } - // Resize problem - A_.conservativeResize(A_.rows(), A_.cols() + _state_ptr->getSize()); - R_.conservativeResize(R_.cols() + _state_ptr->getSize(), R_.cols() + _state_ptr->getSize()); + void addFactor(FactorBasePtr _factor_ptr) + { + std::cout << "adding factor " << _factor_ptr->id() << std::endl; + t_managing_ = clock(); - } - time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC; - } + factors_.push_back(_factor_ptr); + cost_functions_.push_back(createCostFunction(_factor_ptr)); - void updateStateBlockStatus(StateBlockPtr _state_ptr) - { - //TODO - } + unsigned int meas_dim = _factor_ptr->getSize(); - void addFactor(FactorBasePtr _factor_ptr) - { - std::cout << "adding factor " << _factor_ptr->id() << std::endl; - t_managing_ = clock(); + std::vector<Eigen::MatrixXd> jacobians(_factor_ptr->getStateBlockPtrVector().size()); + Eigen::VectorXd error(meas_dim); - factors_.push_back(_factor_ptr); - cost_functions_.push_back(createCostFunction(_factor_ptr)); + cost_functions_.back()->evaluateResidualJacobians(); + cost_functions_.back()->getResidual(error); + cost_functions_.back()->getJacobians(jacobians); - unsigned int meas_dim = _factor_ptr->getSize(); + std::vector<unsigned int> idxs; + for (unsigned int i = 0; i < _factor_ptr->getStateBlockPtrVector().size(); i++) + if (!_factor_ptr->getStateBlockPtrVector().at(i)->isFixed()) + idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->get()]); - std::vector<Eigen::MatrixXd> jacobians(_factor_ptr->getStateBlockPtrVector().size()); - Eigen::VectorXd error(meas_dim); + n_new_factors_++; + factor_locations_.push_back(A_.rows()); - cost_functions_.back()->evaluateResidualJacobians(); - cost_functions_.back()->getResidual(error); - cost_functions_.back()->getJacobians(jacobians); + // Resize problem + A_.conservativeResize(A_.rows() + meas_dim, A_.cols()); + b_.conservativeResize(b_.size() + meas_dim); + A_nodes_.conservativeResize(factors_.size(), nNodes()); - std::vector<unsigned int> idxs; - for (unsigned int i = 0; i < _factor_ptr->getStateBlockPtrVector().size(); i++) - if (!_factor_ptr->getStateBlockPtrVector().at(i)->isFixed()) - idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->get()]); + // ADD MEASUREMENTS + for (unsigned int j = 0; j < idxs.size(); j++) + { + assert((unsigned int)(acc_node_permutation_.indices()(idxs.at(j))) == nodeOrder(idxs.at(j))); + assert(jacobians.at(j).cols() == nodeDim(idxs.at(j))); + assert(jacobians.at(j).rows() == meas_dim); - n_new_factors_++; - factor_locations_.push_back(A_.rows()); + addSparseBlock(jacobians.at(j), A_, A_.rows() - meas_dim, nodeLocation(idxs.at(j))); - // Resize problem - A_.conservativeResize(A_.rows() + meas_dim, A_.cols()); - b_.conservativeResize(b_.size() + meas_dim); - A_nodes_.conservativeResize(factors_.size(), nNodes()); + A_nodes_.coeffRef(A_nodes_.rows() - 1, nodeOrder(idxs.at(j))) = 1; + } - // ADD MEASUREMENTS - for (unsigned int j = 0; j < idxs.size(); j++) - { - assert((unsigned int )(acc_node_permutation_.indices()(idxs.at(j))) == nodeOrder(idxs.at(j))); - assert(jacobians.at(j).cols() == nodeDim(idxs.at(j))); - assert(jacobians.at(j).rows() == meas_dim); + // error + b_.tail(meas_dim) = error; - addSparseBlock(jacobians.at(j), A_, A_.rows() - meas_dim, nodeLocation(idxs.at(j))); + time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC; + } - A_nodes_.coeffRef(A_nodes_.rows() - 1, nodeOrder(idxs.at(j))) = 1; - } + void ordering(const int &_first_ordered_idx) + { + std::cout << "ordering from idx " << _first_ordered_idx << std::endl; + t_ordering_ = clock(); + + // full problem ordering + if (_first_ordered_idx == -1) + { + // ordering ordering factors + node_ordering_restrictions_.resize(nNodes()); + node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose(); - // error - b_.tail(meas_dim) = error; + // computing nodes partial ordering_ + A_nodes_.makeCompressed(); + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(nNodes()); + orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data()); - time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC; + // node ordering to variable ordering + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation(A_.cols()); + nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation); + + // apply partial_ordering orderings + A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView(); + A_ = (A_ * incr_permutation.transpose()).sparseView(); + + // ACCUMULATING PERMUTATIONS + accumulatePermutation(incr_permutation_nodes); } - void ordering(const int & _first_ordered_idx) + // partial ordering + else { - std::cout << "ordering from idx " << _first_ordered_idx << std::endl; - t_ordering_ = clock(); + unsigned int first_ordered_node = nodeOrder(_first_ordered_idx); // nodes_.at(_first_ordered_idx).order; + unsigned int ordered_nodes = nNodes() - first_ordered_node; - // full problem ordering - if (_first_ordered_idx == -1) + if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones { - // ordering ordering factors - node_ordering_restrictions_.resize(nNodes()); - node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose(); + // SUBPROBLEM ORDERING (from first node variable to last one) + // std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 + // << std::endl; + Eigen::SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes); + + // _partial_ordering ordering_ factors + node_ordering_restrictions_.resize(ordered_nodes); + node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose(); // computing nodes partial ordering_ - A_nodes_.makeCompressed(); - Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(nNodes()); - orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data()); + sub_A_nodes_.makeCompressed(); + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation_nodes(ordered_nodes); + orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data()); // node ordering to variable ordering - Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation(A_.cols()); - nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation); + unsigned int ordered_variables = + A_.cols() - nodeLocation(_first_ordered_idx); // nodes_.at(_first_ordered_idx).location; + // std::cout << "first_ordered_node " << first_ordered_node << std::endl; + // std::cout << "A_.cols() " << A_.cols() << std::endl; + // std::cout << "nodes_.at(_first_ordered_idx).location " << + // nodes_.at(_first_ordered_idx).location << std::endl; std::cout << + // "ordered_variables " << ordered_variables << std::endl; + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation(ordered_variables); + nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation); // apply partial_ordering orderings - A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView(); - A_ = (A_ * incr_permutation.transpose()).sparseView(); + A_nodes_.rightCols(ordered_nodes) = + (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView(); + A_.rightCols(ordered_variables) = + (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView(); + R_.rightCols(ordered_variables) = + (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView(); // ACCUMULATING PERMUTATIONS - accumulatePermutation(incr_permutation_nodes); + accumulatePermutation(partial_permutation_nodes); } - - // partial ordering - else - { - unsigned int first_ordered_node = nodeOrder(_first_ordered_idx); //nodes_.at(_first_ordered_idx).order; - unsigned int ordered_nodes = nNodes() - first_ordered_node; - - if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones - { - // SUBPROBLEM ORDERING (from first node variable to last one) - //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl; - Eigen::SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes); - - // _partial_ordering ordering_ factors - node_ordering_restrictions_.resize(ordered_nodes); - node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose(); - - // computing nodes partial ordering_ - sub_A_nodes_.makeCompressed(); - Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation_nodes(ordered_nodes); - orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data()); - - // node ordering to variable ordering - unsigned int ordered_variables = A_.cols() - nodeLocation(_first_ordered_idx); //nodes_.at(_first_ordered_idx).location; -// std::cout << "first_ordered_node " << first_ordered_node << std::endl; -// std::cout << "A_.cols() " << A_.cols() << std::endl; -// std::cout << "nodes_.at(_first_ordered_idx).location " << nodes_.at(_first_ordered_idx).location << std::endl; -// std::cout << "ordered_variables " << ordered_variables << std::endl; - Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation(ordered_variables); - nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation); - - // apply partial_ordering orderings - A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes) - * partial_permutation_nodes.transpose()).sparseView(); - A_.rightCols(ordered_variables) = - (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView(); - R_.rightCols(ordered_variables) = - (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView(); - - // ACCUMULATING PERMUTATIONS - accumulatePermutation(partial_permutation_nodes); - } - } - time_ordering_ += ((double)clock() - t_ordering_) / CLOCKS_PER_SEC; } - - unsigned int findFirstOrderedNode() + time_ordering_ += ((double)clock() - t_ordering_) / CLOCKS_PER_SEC; + } + + unsigned int findFirstOrderedNode() + { + unsigned int first_ordered_node = nNodes(); + unsigned int first_ordered_idx; + for (unsigned int i = 0; i < n_new_factors_; i++) { - unsigned int first_ordered_node = nNodes(); - unsigned int first_ordered_idx; - for (unsigned int i = 0; i < n_new_factors_; i++) + FactorBasePtr ct_ptr = factors_.at(factors_.size() - 1 - i); + std::cout << "factor: " << i << " id: " << factors_.at(factors_.size() - 1 - i)->id() << std::endl; + for (unsigned int j = 0; j < ct_ptr->getStateBlockPtrVector().size(); j++) { - FactorBasePtr ct_ptr = factors_.at(factors_.size() - 1 - i); - std::cout << "factor: " << i << " id: " << factors_.at(factors_.size() - 1 - i)->id() - << std::endl; - for (unsigned int j = 0; j < ct_ptr->getStateBlockPtrVector().size(); j++) + if (!ct_ptr->getStateBlockPtrVector().at(j)->isFixed()) { - if (!ct_ptr->getStateBlockPtrVector().at(j)->isFixed()) + unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->get()]; + // std::cout << "estimated idx " << idx << std::endl; + // std::cout << "node_order(idx) " << node_order(idx) << std::endl; + // std::cout << "first_ordered_node " << first_ordered_node << std::endl; + if (first_ordered_node > nodeOrder(idx)) // nodes_.at(idx).order) { - unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->get()]; - //std::cout << "estimated idx " << idx << std::endl; - //std::cout << "node_order(idx) " << node_order(idx) << std::endl; - //std::cout << "first_ordered_node " << first_ordered_node << std::endl; - if (first_ordered_node > nodeOrder(idx)) //nodes_.at(idx).order) - { - first_ordered_idx = idx; - //std::cout << "first_ordered_idx " << first_ordered_idx << std::endl; - first_ordered_node = nodeOrder(idx); - //std::cout << "first_ordered_node " << first_ordered_node << std::endl; - } + first_ordered_idx = idx; + // std::cout << "first_ordered_idx " << first_ordered_idx << std::endl; + first_ordered_node = nodeOrder(idx); + // std::cout << "first_ordered_node " << first_ordered_node << std::endl; } } } - //std::cout << "found first_ordered_node " << first_ordered_node << std::endl; - //std::cout << "found first_ordered_idx " << first_ordered_idx << std::endl; - - return first_ordered_idx; } + // std::cout << "found first_ordered_node " << first_ordered_node << std::endl; + // std::cout << "found first_ordered_idx " << first_ordered_idx << std::endl; - bool solve(const unsigned int mode) - { - if (n_new_factors_ == 0) - return 1; - - std::cout << "solving mode " << mode << std::endl; + return first_ordered_idx; + } - bool batch, order; - unsigned int first_ordered_idx; - - switch (mode) - { - case 0: - { - batch = true; - order = false; - break; - } - case 1: - { - batch = true; - order = (nNodes() > 1); - break; - } - case 2: - { - first_ordered_idx = findFirstOrderedNode(); - batch = (nodeOrder(first_ordered_idx) == 0); - order = (nNodes() > 1); - } - } + bool solve(const unsigned int mode) + { + if (n_new_factors_ == 0) return 1; - // BATCH - if (batch) - { - // REORDER - if (order) - ordering(-1); + std::cout << "solving mode " << mode << std::endl; - //printProblem(); + bool batch, order; + unsigned int first_ordered_idx; - // SOLVE - t_solving_ = clock(); - A_.makeCompressed(); - solver_.compute(A_); - if (solver_.info() != Eigen::Success) - { - std::cout << "decomposition failed" << std::endl; - return 0; - } - x_incr_ = solver_.solve(-b_); - R_ = solver_.matrixR(); - //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl; - time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC; + switch (mode) + { + case 0: { + batch = true; + order = false; + break; } - // INCREMENTAL - else - { - // REORDER SUBPROBLEM - ordering(first_ordered_idx); - //printProblem(); - - // SOLVE ORDERED SUBPROBLEM - t_solving_ = clock(); - A_nodes_.makeCompressed(); - A_.makeCompressed(); - - // finding measurements block - Eigen::SparseMatrix<int> measurements_to_initial = A_nodes_.col(nodeOrder(first_ordered_idx)); - //std::cout << "measurements_to_initial " << measurements_to_initial << std::endl; - //std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl; - unsigned int first_ordered_measurement = - measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]]; - unsigned int ordered_measurements = A_.rows() - factor_locations_.at(first_ordered_measurement); - unsigned int ordered_variables = A_.cols() - nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location; - unsigned int unordered_variables = nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location; - - Eigen::SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables); - solver_.compute(A_partial); - if (solver_.info() != Eigen::Success) - { - std::cout << "decomposition failed" << std::endl; - return 0; - } - //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl; - x_incr_.tail(ordered_variables) = solver_.solve(-b_.tail(ordered_measurements)); - - // store new part of R - eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables); - //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl; - addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables); - //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl; - R_.makeCompressed(); - - // solving not ordered subproblem - if (unordered_variables > 0) - { - //std::cout << "--------------------- solving unordered part" << std::endl; - Eigen::SparseMatrix<double> R1 = R_.topLeftCorner(unordered_variables, unordered_variables); - //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl; - Eigen::SparseMatrix<double> R2 = R_.topRightCorner(unordered_variables, ordered_variables); - //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl; - solver_.compute(R1); - if (solver_.info() != Eigen::Success) - { - std::cout << "decomposition failed" << std::endl; - return 0; - } - x_incr_.head(unordered_variables) = solver_.solve( - -b_.head(unordered_variables) + R2 * x_incr_.tail(ordered_variables)); - } - + case 1: { + batch = true; + order = (nNodes() > 1); + break; } - // UPDATE X VALUE - for (unsigned int i = 0; i < nodes_.size(); i++) - { - Eigen::Map < Eigen::VectorXd > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize()); - x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize()); + case 2: { + first_ordered_idx = findFirstOrderedNode(); + batch = (nodeOrder(first_ordered_idx) == 0); + order = (nNodes() > 1); } - // Zero the error - b_.setZero(); - - time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC; - n_new_factors_ = 0; - return 1; } - void nodePermutation2VariablesPermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes, - Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm_variables) + // BATCH + if (batch) { - //std::cout << "perm_nodes: " << _perm_nodes.indices().transpose() << std::endl; - nodePermutation2nodeLocations(_perm_nodes, node_locations_); - //std::cout << "locations: " << locations.transpose() << std::endl; - //std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl; + // REORDER + if (order) ordering(-1); + + // printProblem(); - unsigned int last_idx = 0; - for (unsigned int i = 0; i < node_locations_.size(); i++) + // SOLVE + t_solving_ = clock(); + A_.makeCompressed(); + solver_.compute(A_); + if (solver_.info() != Eigen::Success) { - perm_variables.indices().segment(last_idx, nodeDim(i)) = Eigen::VectorXi::LinSpaced( - nodeDim(i), node_locations_(i), node_locations_(i) + nodeDim(i) - 1); - last_idx += nodeDim(i); - //std::cout << i << " perm_variables: " << perm_variables.indices().transpose() << std::endl; + std::cout << "decomposition failed" << std::endl; + return 0; } - //std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl; - } - - void nodePermutation2nodeLocations(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes, - Eigen::ArrayXi& locations) - { - locations = _perm_nodes.indices().array(); - - for (unsigned int i = 0; i < locations.size(); i++) - locations = (locations > locations(i)).select(locations + nodeDim(i) - 1, locations); - } - - void augmentPermutation(Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm, unsigned int new_size) - { - unsigned int old_size = perm.indices().size(); - unsigned int dim = new_size - old_size; - - Eigen::VectorXi new_indices(new_size); - new_indices.head(old_size) = perm.indices(); - new_indices.tail(dim) = Eigen::ArrayXi::LinSpaced(dim, old_size, new_size - 1); - perm.resize(new_size); - perm.indices() = new_indices; - std::cout << "permutation augmented" << std::endl; - - // resize and update locations - node_locations_.conservativeResize(node_locations_.size() + 1); - node_locations_(node_locations_.size() - 1) = x_incr_.size(); - std::cout << "node_locations_ augmented" << std::endl; + x_incr_ = solver_.solve(-b_); + R_ = solver_.matrixR(); + // std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl; + time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC; } - - void accumulatePermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm) + // INCREMENTAL + else { - //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl; - //std::cout << "incr perm " << perm.indices().transpose() << std::endl; - - // acumulate permutation - if (perm.size() == acc_node_permutation_.size()) //full permutation - acc_node_permutation_ = perm * acc_node_permutation_; - else //partial permutation + // REORDER SUBPROBLEM + ordering(first_ordered_idx); + // printProblem(); + + // SOLVE ORDERED SUBPROBLEM + t_solving_ = clock(); + A_nodes_.makeCompressed(); + A_.makeCompressed(); + + // finding measurements block + Eigen::SparseMatrix<int> measurements_to_initial = A_nodes_.col(nodeOrder(first_ordered_idx)); + // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl; + // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << + // measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl; + unsigned int first_ordered_measurement = + measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]]; + unsigned int ordered_measurements = A_.rows() - factor_locations_.at(first_ordered_measurement); + unsigned int ordered_variables = + A_.cols() - nodeLocation(first_ordered_idx); // nodes_.at(first_ordered_idx).location; + unsigned int unordered_variables = + nodeLocation(first_ordered_idx); // nodes_.at(first_ordered_idx).location; + + Eigen::SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables); + solver_.compute(A_partial); + if (solver_.info() != Eigen::Success) { - Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes( - Eigen::VectorXi::LinSpaced(nNodes(), 0, nNodes() - 1)); // identity permutation - incr_permutation_nodes.indices().tail(perm.size()) = perm.indices() - + Eigen::VectorXi::Constant(perm.size(), nNodes() - perm.size()); - //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl; - acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_; + std::cout << "decomposition failed" << std::endl; + return 0; + } + // std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * + // solver_.matrixR() << std::endl; + x_incr_.tail(ordered_variables) = solver_.solve(-b_.tail(ordered_measurements)); + + // store new part of R + eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables); + // std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl; + addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables); + // std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl; + R_.makeCompressed(); + + // solving not ordered subproblem + if (unordered_variables > 0) + { + // std::cout << "--------------------- solving unordered part" << std::endl; + Eigen::SparseMatrix<double> R1 = R_.topLeftCorner(unordered_variables, unordered_variables); + // std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl; + Eigen::SparseMatrix<double> R2 = R_.topRightCorner(unordered_variables, ordered_variables); + // std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl; + solver_.compute(R1); + if (solver_.info() != Eigen::Success) + { + std::cout << "decomposition failed" << std::endl; + return 0; + } + x_incr_.head(unordered_variables) = + solver_.solve(-b_.head(unordered_variables) + R2 * x_incr_.tail(ordered_variables)); } - //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl; - - // update nodes orders and locations - nodePermutation2nodeLocations(acc_node_permutation_, node_locations_); - } - - unsigned int nodeDim(const unsigned int _idx) - { - assert(_idx < nNodes()); - return nodes_.at(_idx)->getSize(); } - - unsigned int nodeOrder(const unsigned int _idx) + // UPDATE X VALUE + for (unsigned int i = 0; i < nodes_.size(); i++) { - assert(_idx < nNodes()); - assert(_idx < acc_node_permutation_.indices().size()); - return acc_node_permutation_.indices()(_idx); + Eigen::Map<Eigen::VectorXd> x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize()); + x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize()); } - - unsigned int nodeLocation(const unsigned int _idx) + // Zero the error + b_.setZero(); + + time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC; + n_new_factors_ = 0; + return 1; + } + + void nodePermutation2VariablesPermutation( + const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes, + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> & perm_variables) + { + // std::cout << "perm_nodes: " << _perm_nodes.indices().transpose() << std::endl; + nodePermutation2nodeLocations(_perm_nodes, node_locations_); + // std::cout << "locations: " << locations.transpose() << std::endl; + // std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl; + + unsigned int last_idx = 0; + for (unsigned int i = 0; i < node_locations_.size(); i++) { - assert(_idx < nNodes()); - assert(_idx < node_locations_.size()); - return node_locations_(_idx); + perm_variables.indices().segment(last_idx, nodeDim(i)) = + Eigen::VectorXi::LinSpaced(nodeDim(i), node_locations_(i), node_locations_(i) + nodeDim(i) - 1); + last_idx += nodeDim(i); + // std::cout << i << " perm_variables: " << perm_variables.indices().transpose() << std::endl; } - - unsigned int nNodes() + // std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl; + } + + void nodePermutation2nodeLocations( + const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes, + Eigen::ArrayXi & locations) + { + locations = _perm_nodes.indices().array(); + + for (unsigned int i = 0; i < locations.size(); i++) + locations = (locations > locations(i)).select(locations + nodeDim(i) - 1, locations); + } + + void augmentPermutation(Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm, unsigned int new_size) + { + unsigned int old_size = perm.indices().size(); + unsigned int dim = new_size - old_size; + + Eigen::VectorXi new_indices(new_size); + new_indices.head(old_size) = perm.indices(); + new_indices.tail(dim) = Eigen::ArrayXi::LinSpaced(dim, old_size, new_size - 1); + perm.resize(new_size); + perm.indices() = new_indices; + std::cout << "permutation augmented" << std::endl; + + // resize and update locations + node_locations_.conservativeResize(node_locations_.size() + 1); + node_locations_(node_locations_.size() - 1) = x_incr_.size(); + std::cout << "node_locations_ augmented" << std::endl; + } + + void accumulatePermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm) + { + // std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << + // std::endl; std::cout << "incr perm " << perm.indices().transpose() << std::endl; + + // acumulate permutation + if (perm.size() == acc_node_permutation_.size()) // full permutation + acc_node_permutation_ = perm * acc_node_permutation_; + else // partial permutation { - return nodes_.size(); + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes( + Eigen::VectorXi::LinSpaced(nNodes(), 0, nNodes() - 1)); // identity permutation + incr_permutation_nodes.indices().tail(perm.size()) = + perm.indices() + Eigen::VectorXi::Constant(perm.size(), nNodes() - perm.size()); + // std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl; + acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_; } - - CostFunctionBasePtr createCostFunction(FactorBasePtr _fac_ptr) + // std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl; + + // update nodes orders and locations + nodePermutation2nodeLocations(acc_node_permutation_, node_locations_); + } + + unsigned int nodeDim(const unsigned int _idx) + { + assert(_idx < nNodes()); + return nodes_.at(_idx)->getSize(); + } + + unsigned int nodeOrder(const unsigned int _idx) + { + assert(_idx < nNodes()); + assert(_idx < acc_node_permutation_.indices().size()); + return acc_node_permutation_.indices()(_idx); + } + + unsigned int nodeLocation(const unsigned int _idx) + { + assert(_idx < nNodes()); + assert(_idx < node_locations_.size()); + return node_locations_(_idx); + } + + unsigned int nNodes() + { + return nodes_.size(); + } + + CostFunctionBasePtr createCostFunction(FactorBasePtr _fac_ptr) + { + // std::cout << "adding fac " << _fac_ptr->id() << std::endl; + //_fac_ptr->print(); + + switch (_fac_ptr->getTypeId()) { - //std::cout << "adding fac " << _fac_ptr->id() << std::endl; - //_fac_ptr->print(); - - switch (_fac_ptr->getTypeId()) - { - case FAC_GPS_FIX_2d: - { - FactorGPS2d* specific_ptr = (FactorGPS2d*)(_fac_ptr); - return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2d, specific_ptr->residualSize, - specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size, - specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size, - specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr)); - break; - } - case FAC_ODOM_2d: - { - FactorOdom2d* specific_ptr = (FactorOdom2d*)(_fac_ptr); - return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2d, specific_ptr->residualSize, - specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size, - specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size, - specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - case FAC_CORNER_2d: - { - FactorCorner2d* specific_ptr = (FactorCorner2d*)(_fac_ptr); - return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2d, specific_ptr->residualSize, - specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size, - specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size, - specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - default: - std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" - << std::endl; - - return nullptr; + case FAC_GPS_FIX_2d: { + FactorGPS2d *specific_ptr = (FactorGPS2d *)(_fac_ptr); + return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2d, + specific_ptr->residualSize, + specific_ptr->block0Size, + specific_ptr->block1Size, + specific_ptr->block2Size, + specific_ptr->block3Size, + specific_ptr->block4Size, + specific_ptr->block5Size, + specific_ptr->block6Size, + specific_ptr->block7Size, + specific_ptr->block8Size, + specific_ptr->block9Size>(specific_ptr)); + break; } - } - - void printResults() - { - std::cout << " solved in " << time_solving_ * 1e3 << " ms | " << R_.nonZeros() << " nonzeros in R" - << std::endl; - std::cout << "x = " << x_incr_.transpose() << std::endl; - } + case FAC_ODOM_2d: { + FactorOdom2d *specific_ptr = (FactorOdom2d *)(_fac_ptr); + return (CostFunctionBasePtr) new CostFunctionSparse<FactorOdom2d, + specific_ptr->residualSize, + specific_ptr->block0Size, + specific_ptr->block1Size, + specific_ptr->block2Size, + specific_ptr->block3Size, + specific_ptr->block4Size, + specific_ptr->block5Size, + specific_ptr->block6Size, + specific_ptr->block7Size, + specific_ptr->block8Size, + specific_ptr->block9Size>(specific_ptr); + break; + } + case FAC_CORNER_2d: { + FactorCorner2d *specific_ptr = (FactorCorner2d *)(_fac_ptr); + return (CostFunctionBasePtr) new CostFunctionSparse<FactorCorner2d, + specific_ptr->residualSize, + specific_ptr->block0Size, + specific_ptr->block1Size, + specific_ptr->block2Size, + specific_ptr->block3Size, + specific_ptr->block4Size, + specific_ptr->block5Size, + specific_ptr->block6Size, + specific_ptr->block7Size, + specific_ptr->block8Size, + specific_ptr->block9Size>(specific_ptr); + break; + } + default: + std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" + << std::endl; - void printProblem() - { - std::cout << std::endl << "A_nodes_: " << std::endl - << Eigen::MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl; - //std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl; - std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl; + return nullptr; } + } + + void printResults() + { + std::cout << " solved in " << time_solving_ * 1e3 << " ms | " << R_.nonZeros() << " nonzeros in R" + << std::endl; + std::cout << "x = " << x_incr_.transpose() << std::endl; + } + + void printProblem() + { + std::cout << std::endl + << "A_nodes_: " << std::endl + << Eigen::MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl + << std::endl; + // std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << + // std::endl; + std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl; + } }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/solver_suitesparse/solver_QR.h b/include/core/solver_suitesparse/solver_QR.h index 6ddcda8a0bf22503b3e97fbf8c5fbdfda3fbb671..a65e87bcb16c824f9a51c0eae904907ca3616e86 100644 --- a/include/core/solver_suitesparse/solver_QR.h +++ b/include/core/solver_suitesparse/solver_QR.h @@ -24,19 +24,19 @@ using namespace Eigen; class SolverQR { - protected: - SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_; - SparseMatrix<double> A, A_ordered, R; - VectorXd b, x, x_ordered, x_ordered_partial; - int n_measurements = 0; - int n_nodes = 0; + protected: + SparseQR<SparseMatrix<double>, NaturalOrdering<int>> solver_; + SparseMatrix<double> A, A_ordered, R; + VectorXd b, x, x_ordered, x_ordered_partial; + int n_measurements = 0; + int n_nodes = 0; - // ordering variables - SparseMatrix<int> A_nodes_ordered; - PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix; + // ordering variables + SparseMatrix<int> A_nodes_ordered; + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix; - CCOLAMDOrdering<int> ordering, partial_ordering; - VectorXi nodes_ordering_factors; + CCOLAMDOrdering<int> ordering, partial_ordering; + VectorXi nodes_ordering_factors; - private: + private: }; diff --git a/include/core/solver_suitesparse/solver_manager.h b/include/core/solver_suitesparse/solver_manager.h index a7660386e87bdaa18ff4879156429f1c636e46df..95c1d31fd2fd1b3c0152a862c6c640a1f6e88d3e 100644 --- a/include/core/solver_suitesparse/solver_manager.h +++ b/include/core/solver_suitesparse/solver_manager.h @@ -20,7 +20,7 @@ #pragma once -//wolf includes +// wolf includes #include "core/factor/factor_GPS_2d.h" #include "core/common/wolf.h" #include "core/state_block/state_block.h" @@ -38,28 +38,27 @@ class SolverManager { - protected: + protected: + public: + SolverManager(ceres::Problem::Options _options); - public: - SolverManager(ceres::Problem::Options _options); + ~SolverManager(); - ~SolverManager(); + ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options); - ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options); + // void computeCovariances(WolfProblemPtr _problem_ptr); - //void computeCovariances(WolfProblemPtr _problem_ptr); + void update(const WolfProblemPtr _problem_ptr); - void update(const WolfProblemPtr _problem_ptr); + void addFactor(FactorBasePtr _fac_ptr); - void addFactor(FactorBasePtr _fac_ptr); + void removeFactor(const unsigned int& _fac_idx); - void removeFactor(const unsigned int& _fac_idx); + void addStateUnit(StateBlockPtr _st_ptr); - void addStateUnit(StateBlockPtr _st_ptr); + void removeAllStateUnits(); - void removeAllStateUnits(); + void updateStateUnitStatus(StateBlockPtr _st_ptr); - void updateStateUnitStatus(StateBlockPtr _st_ptr); - - ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr); + ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr); }; diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h index 4c746537b3b836b4bcb83210104f58cd2fe39648..cbab6408b6742f966f271c0686d7ae7765219f73 100644 --- a/include/core/solver_suitesparse/sparse_utils.h +++ b/include/core/solver_suitesparse/sparse_utils.h @@ -25,7 +25,6 @@ namespace wolf { - void eraseBlockRow(Eigen::SparseMatrixd& A, const unsigned int& _row, const unsigned int& _n_rows) { A.prune([](int i, int, double) { return i >= _row && i < _row + _n_rows; }); @@ -36,22 +35,28 @@ void eraseBlockCol(Eigen::SparseMatrixd& A, const unsigned int& _col, const unsi A.prune([](int, int j, double) { return j >= _col && j < _col + _n_cols; }); } -void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col) +void addSparseBlock(const Eigen::MatrixXd& ins, + Eigen::SparseMatrixd& original, + const unsigned int& row, + const unsigned int& col) { for (auto ins_row = 0; ins_row < ins.rows(); ins_row++) for (auto ins_col = 0; ins_col < ins.cols(); ins_col++) - original.coeffRef(row+ins_row, col+ins_col) += ins(ins_row,ins_col); + original.coeffRef(row + ins_row, col + ins_col) += ins(ins_row, ins_col); original.makeCompressed(); } -void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col) +void insertSparseBlock(const Eigen::MatrixXd& ins, + Eigen::SparseMatrixd& original, + const unsigned int& row, + const unsigned int& col) { for (auto ins_row = 0; ins_row < ins.rows(); ins_row++) for (auto ins_col = 0; ins_col < ins.cols(); ins_col++) - original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col); + original.insert(row + ins_row, col + ins_col) = ins(ins_row, ins_col); original.makeCompressed(); } -} +} // namespace wolf diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h index 94bf908ef3d8c56242f99fea25b1e3ed3193410f..cd35e83a241b589de8ce48b6be629d4aa771eac9 100644 --- a/include/core/state_block/factory_state_block.h +++ b/include/core/state_block/factory_state_block.h @@ -25,7 +25,6 @@ namespace wolf { - /** \brief StateBlock factory class * * This factory can create objects of class StateBlock and classes deriving from StateBlock. @@ -82,32 +81,32 @@ namespace wolf * */ typedef Factory<std::shared_ptr<StateBlock>, const Eigen::VectorXd&, bool> FactoryStateBlock; -template<> +template <> inline std::string FactoryStateBlock::getClass() const { return "FactoryStateBlock"; } typedef Factory<std::shared_ptr<StateBlock>> FactoryStateBlockIdentity; -template<> +template <> inline std::string FactoryStateBlockIdentity::getClass() const { return "FactoryStateBlockIdentity"; } typedef Factory<Eigen::VectorXd> FactoryStateBlockIdentityVector; -template<> +template <> inline std::string FactoryStateBlockIdentityVector::getClass() const { return "FactoryStateBlockIdentityVector"; } -#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \ -namespace \ -{ \ -const bool WOLF_UNUSED StateBlockType##Registered = \ - FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \ -const bool WOLF_UNUSED StateBlockType##IdentityRegistered = \ - FactoryStateBlockIdentity::registerCreator(#StateBlockType, StateBlockType::createIdentity); \ -const bool WOLF_UNUSED StateBlockType##IdentityVectorRegistered = \ - FactoryStateBlockIdentityVector::registerCreator(#StateBlockType, StateBlockType::Identity); \ -} -} +#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \ + namespace \ + { \ + const bool WOLF_UNUSED StateBlockType##Registered = \ + FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \ + const bool WOLF_UNUSED StateBlockType##IdentityRegistered = \ + FactoryStateBlockIdentity::registerCreator(#StateBlockType, StateBlockType::createIdentity); \ + const bool WOLF_UNUSED StateBlockType##IdentityVectorRegistered = \ + FactoryStateBlockIdentityVector::registerCreator(#StateBlockType, StateBlockType::Identity); \ + } +} // namespace wolf diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h index affd72fde8848ecacf89de0bcfe38de2b254d7b3..36e5542fdd44a4211e3b81480fbc83031a40f870 100644 --- a/include/core/state_block/local_parametrization_angle.h +++ b/include/core/state_block/local_parametrization_angle.h @@ -25,25 +25,24 @@ namespace wolf { - class LocalParametrizationAngle : public LocalParametrizationBase { - public: - LocalParametrizationAngle(); - ~LocalParametrizationAngle() override; + public: + LocalParametrizationAngle(); + ~LocalParametrizationAngle() override; - bool plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta, - Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override; - bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, - Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override; - bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, - Eigen::Map<const Eigen::VectorXd>& _x2, - Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override; - bool isValid(const Eigen::VectorXd& state, double tolerance) override; + bool plus(Eigen::Map<const Eigen::VectorXd>& _h, + Eigen::Map<const Eigen::VectorXd>& _delta, + Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override; + bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override; + bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, + Eigen::Map<const Eigen::VectorXd>& _x2, + Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override; + bool isValid(const Eigen::VectorXd& state, double tolerance) override; }; -inline LocalParametrizationAngle::LocalParametrizationAngle() : - LocalParametrizationBase(1,1) +inline LocalParametrizationAngle::LocalParametrizationAngle() : LocalParametrizationBase(1, 1) { // } @@ -55,14 +54,14 @@ inline LocalParametrizationAngle::~LocalParametrizationAngle() inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta, - Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const + Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const { _h_plus_delta(0) = pi2pi(_h(0) + _delta(0)); return true; } inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, - Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const { _jacobian(0) = 1.0; return true; @@ -70,17 +69,17 @@ inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::V inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>& _x1, Eigen::Map<const Eigen::VectorXd>& _x2, - Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) + Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) { - _x2_minus_x1(0) = pi2pi(_x2(0)-_x1(0)); + _x2_minus_x1(0) = pi2pi(_x2(0) - _x1(0)); return true; } inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance) { - //Any real is a valid angle because we use the pi2pi function. Also - //the types don't match. In this case the argument is - //Eigen::Map not Eigen::VectorXd + // Any real is a valid angle because we use the pi2pi function. Also + // the types don't match. In this case the argument is + // Eigen::Map not Eigen::VectorXd return true; } } /* namespace wolf */ diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h index d1c3010e8e2c44361b6ea7a19a87ff36c069fa1a..6fa3dec2dbd12e767b0adee824a095df6a46f91b 100644 --- a/include/core/state_block/local_parametrization_base.h +++ b/include/core/state_block/local_parametrization_base.h @@ -22,30 +22,30 @@ #include "core/common/wolf.h" -namespace wolf { +namespace wolf +{ +class LocalParametrizationBase +{ + protected: + unsigned int global_size_; + unsigned int local_size_; -class LocalParametrizationBase{ - protected: - unsigned int global_size_; - unsigned int local_size_; - public: - LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size); - virtual ~LocalParametrizationBase(); + public: + LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size); + virtual ~LocalParametrizationBase(); - bool plus(const Eigen::VectorXd& _x, - const Eigen::VectorXd& _delta, - Eigen::VectorXd& _x_plus_delta) const; - virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x, - Eigen::Map<const Eigen::VectorXd>& _delta, - Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0; - virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x, - Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0; - virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, - Eigen::Map<const Eigen::VectorXd>& _x2, - Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0; - virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0; - unsigned int getLocalSize() const; - unsigned int getGlobalSize() const; + bool plus(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, Eigen::VectorXd& _x_plus_delta) const; + virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x, + Eigen::Map<const Eigen::VectorXd>& _delta, + Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0; + virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x, + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0; + virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, + Eigen::Map<const Eigen::VectorXd>& _x2, + Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0; + virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0; + unsigned int getLocalSize() const; + unsigned int getGlobalSize() const; }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h index d7e066e9afac1ee73595e73c3c534ee1d38ac840..f3be4e47e1c5d8740492078b78624d22d726c229 100644 --- a/include/core/state_block/local_parametrization_homogeneous.h +++ b/include/core/state_block/local_parametrization_homogeneous.h @@ -22,8 +22,8 @@ #include "core/state_block/local_parametrization_base.h" -namespace wolf { - +namespace wolf +{ /** * \brief Local parametrization for homogeneous vectors. * @@ -38,7 +38,8 @@ namespace wolf { * where \f$\otimes\f$ is the quaternion product, and \f$d{\bf q} = {\bf q}(d {\bf x})\f$ is a unit delta_quaternion * equivalent to a rotation \f$d{\bf x}\f$, obtained with * - * \f[{\bf q}(d{\bf x}) = \left[\begin{array}{c} \frac{ d {\bf x}}{ |d{\bf x}|} \sin(|d{\bf x}|) \\ \cos(|d{\bf x}|) \end{array}\right]\f] + * \f[{\bf q}(d{\bf x}) = \left[\begin{array}{c} \frac{ d {\bf x}}{ |d{\bf x}|} \sin(|d{\bf x}|) \\ \cos(|d{\bf x}|) + * \end{array}\right]\f] * * Contrary to the case of quaternions, it is not required that \f${\bf x}\f$ be a unit homogeneous vector. * In this case, the updated \f${\bf x}^+={\bf x}\oplus d {\bf x}\f$ has the same norm as \f${\bf x}\f$. @@ -48,18 +49,19 @@ namespace wolf { */ class LocalParametrizationHomogeneous : public LocalParametrizationBase { - public: - LocalParametrizationHomogeneous(); - ~LocalParametrizationHomogeneous() override; + public: + LocalParametrizationHomogeneous(); + ~LocalParametrizationHomogeneous() override; - bool plus(Eigen::Map<const Eigen::VectorXd>& _h, - Eigen::Map<const Eigen::VectorXd>& _delta, - Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override; - bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override; - bool minus(Eigen::Map<const Eigen::VectorXd>& _h1, - Eigen::Map<const Eigen::VectorXd>& _h2, - Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override; - bool isValid(const Eigen::VectorXd& state, double tolerance) override; + bool plus(Eigen::Map<const Eigen::VectorXd>& _h, + Eigen::Map<const Eigen::VectorXd>& _delta, + Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override; + bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override; + bool minus(Eigen::Map<const Eigen::VectorXd>& _h1, + Eigen::Map<const Eigen::VectorXd>& _h2, + Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override; + bool isValid(const Eigen::VectorXd& state, double tolerance) override; }; -} // namespace wolf +} // namespace wolf diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h index 777930c7d581ace2019aa06373d8fc7bd23c84ce..f037dba0e61a9a4ad02d0ea5770971360dfe90b2 100644 --- a/include/core/state_block/local_parametrization_quaternion.h +++ b/include/core/state_block/local_parametrization_quaternion.h @@ -22,18 +22,19 @@ #include "core/state_block/local_parametrization_base.h" -namespace wolf { - +namespace wolf +{ /** - * \brief Local or global orientation error - * - * Local or global orientation error. - * - * This enum controls whether the orientation error is composed locally or globally to an orientation specification. - * - * See LocalParametrizationQuaternion for more information. - */ -typedef enum { + * \brief Local or global orientation error + * + * Local or global orientation error. + * + * This enum controls whether the orientation error is composed locally or globally to an orientation specification. + * + * See LocalParametrizationQuaternion for more information. + */ +typedef enum +{ DQ_LOCAL, DQ_GLOBAL } QuaternionDeltaReference; @@ -62,38 +63,35 @@ typedef enum { template <QuaternionDeltaReference DeltaReference = DQ_LOCAL> class LocalParametrizationQuaternion : public LocalParametrizationBase { + public: + LocalParametrizationQuaternion() : LocalParametrizationBase(4, 3) + { + // + } -public: - - LocalParametrizationQuaternion() : - LocalParametrizationBase(4, 3) - { - // - } - - ~LocalParametrizationQuaternion() override - { - // - } + ~LocalParametrizationQuaternion() override + { + // + } - bool plus(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<const Eigen::VectorXd>& _delta_theta, - Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const override; + bool plus(Eigen::Map<const Eigen::VectorXd>& _q, + Eigen::Map<const Eigen::VectorXd>& _delta_theta, + Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const override; - bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override; - bool minus(Eigen::Map<const Eigen::VectorXd>& _q1, - Eigen::Map<const Eigen::VectorXd>& _q2, - Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override; - // inline bool isValid(const Eigen::VectorXd& state) override; - // template<QuaternionDeltaReference DeltaReference> - bool isValid(const Eigen::VectorXd& _state, double tolerance) override - { - return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance; - } + bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q, + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override; + bool minus(Eigen::Map<const Eigen::VectorXd>& _q1, + Eigen::Map<const Eigen::VectorXd>& _q2, + Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override; + // inline bool isValid(const Eigen::VectorXd& state) override; + // template<QuaternionDeltaReference DeltaReference> + bool isValid(const Eigen::VectorXd& _state, double tolerance) override + { + return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance; + } }; typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal; typedef LocalParametrizationQuaternion<DQ_LOCAL> LocalParametrizationQuaternionLocal; -} // namespace wolf +} // namespace wolf diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h index 67c5feb3715ccbd79eb3cb9d19ff0e9d0e755108..cbec8d9641dba0459aadf0b535c98d5a06d03abc 100644 --- a/include/core/state_block/state_angle.h +++ b/include/core/state_block/state_angle.h @@ -25,33 +25,31 @@ namespace wolf { - class StateAngle : public StateBlock { - public: - StateAngle(double _angle, bool _fixed = false, bool _transformable = true); - StateAngle(const VectorXd& _angle, bool _fixed = false, bool _transformable = true); - - static Eigen::VectorXd Identity(); + public: + StateAngle(double _angle, bool _fixed = false, bool _transformable = true); + StateAngle(const VectorXd& _angle, bool _fixed = false, bool _transformable = true); + + static Eigen::VectorXd Identity(); - WOLF_STATE_BLOCK_CREATE(StateAngle); + WOLF_STATE_BLOCK_CREATE(StateAngle); - ~StateAngle() override; + ~StateAngle() override; - virtual void transform(const VectorComposite& _transformation) override; + virtual void transform(const VectorComposite& _transformation) override; }; -inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) : - StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) +inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) + : StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) { state_(0) = pi2pi(_angle); } -inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable) : - StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) +inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable) + : StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) { - if(_angle.size() != 1) - throw std::runtime_error("The angle state vector must be of size 1!"); + if (_angle.size() != 1) throw std::runtime_error("The angle state vector must be of size 1!"); state_(0) = pi2pi(_angle(0)); } @@ -68,7 +66,7 @@ inline StateAngle::~StateAngle() inline void StateAngle::transform(const VectorComposite& _transformation) { - if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles + if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles } } /* namespace wolf */ diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h index 835b6d9ec99960f17d1440bfe2056e282b20009f..cc5f509fea8a8d11dc5c2d303dbe717672f77087 100644 --- a/include/core/state_block/state_block.h +++ b/include/core/state_block/state_block.h @@ -23,44 +23,45 @@ /* * Macro for defining StateBlocks creators. * - * Place a call to this macro inside your class declaration + * Place a call to this macro inside your class declaration * (in the state_block_class.h file), preferably just after the constructors. * * In order to use this macro, the derived state_block class, StateBlockClass, * must have the constructor callable as: * * StateBlockClass(const Eigen::VectorXd& _state, bool _fixed) - * + * * And the static method: - * + * * static Eigen::VectorXd Identity() */ -#define WOLF_STATE_BLOCK_CREATE(StateBlockClass) \ -static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed) \ -{ \ - return std::make_shared<StateBlockClass>(_state, _fixed); \ -} \ -static StateBlockPtr createIdentity() \ -{ \ - return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), false); \ -} \ +#define WOLF_STATE_BLOCK_CREATE(StateBlockClass) \ + static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed) \ + { \ + return std::make_shared<StateBlockClass>(_state, _fixed); \ + } \ + static StateBlockPtr createIdentity() \ + { \ + return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), false); \ + } // Fwd references -namespace wolf{ +namespace wolf +{ class NodeBase; class LocalParametrizationBase; -} +} // namespace wolf -//Wolf includes +// Wolf includes #include "core/common/wolf.h" #include "core/composite/vector_composite.h" -//std includes +// std includes #include <iostream> #include <mutex> -namespace wolf { - +namespace wolf +{ /** \brief class StateBlock * * This class implements a state block for general nonlinear optimization. It offers the following functionality: @@ -72,200 +73,221 @@ namespace wolf { */ class StateBlock : public std::enable_shared_from_this<StateBlock> { -public: - - protected: - - std::string type_; ///< Type of the derived class - std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not - - std::atomic<SizeEigen> state_size_; ///< State vector size - Eigen::VectorXd state_; ///< State vector storing the state values - mutable std::mutex mut_state_; ///< State vector mutex - - LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold - - std::atomic_bool state_updated_; ///< Flag to indicate whether the state has been updated or not - std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not - std::atomic_bool local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not - - bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame - - public: - /** \brief Constructor from size - * - * \param _size is state size - * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter - * \param _local_param_ptr pointer to the local parametrization for the block - */ - StateBlock(const std::string& _type, const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true); - - /** \brief Constructor from vector - * - * \param _state is state vector - * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter - * \param _local_param_ptr pointer to the local parametrization for the block - **/ - StateBlock(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true); - - ///< Explicitly not copyable/movable - StateBlock(const StateBlock& o) = delete; - StateBlock(StateBlock&& o) = delete; - StateBlock& operator=(const StateBlock& o) = delete; - - /** \brief Destructor - **/ - virtual ~StateBlock(); - - /** \brief Returns the type of the state block - **/ - std::string getType() const; - - /** \brief Returns the state vector - **/ - Eigen::VectorXd getState() const; - - /** \brief Returns the state vector data array - **/ - double* getStateData(); - - /** \brief Sets the state vector - **/ - void setState(const Eigen::VectorXd& _state, const bool _notify = true); - - /** \brief Returns the state size - **/ - SizeEigen getSize() const; - - /**\brief Returns the size of the local parametrization - */ - SizeEigen getLocalSize() const; + public: + protected: + std::string type_; ///< Type of the derived class + std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not + + std::atomic<SizeEigen> state_size_; ///< State vector size + Eigen::VectorXd state_; ///< State vector storing the state values + mutable std::mutex mut_state_; ///< State vector mutex + + LocalParametrizationBasePtr + local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold + + std::atomic_bool state_updated_; ///< Flag to indicate whether the state has been updated or not + std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not + std::atomic_bool + local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not + + bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame + + public: + /** \brief Constructor from size + * + * \param _size is state size + * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter + * \param _local_param_ptr pointer to the local parametrization for the block + */ + StateBlock(const std::string& _type, + const SizeEigen _size, + bool _fixed = false, + LocalParametrizationBasePtr _local_param_ptr = nullptr, + bool _transformable = true); + + /** \brief Constructor from vector + * + * \param _state is state vector + * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter + * \param _local_param_ptr pointer to the local parametrization for the block + **/ + StateBlock(const std::string& _type, + const Eigen::VectorXd& _state, + bool _fixed = false, + LocalParametrizationBasePtr _local_param_ptr = nullptr, + bool _transformable = true); + + ///< Explicitly not copyable/movable + StateBlock(const StateBlock& o) = delete; + StateBlock(StateBlock&& o) = delete; + StateBlock& operator=(const StateBlock& o) = delete; + + /** \brief Destructor + **/ + virtual ~StateBlock(); + + /** \brief Returns the type of the state block + **/ + std::string getType() const; + + /** \brief Returns the state vector + **/ + Eigen::VectorXd getState() const; + + /** \brief Returns the state vector data array + **/ + double* getStateData(); + + /** \brief Sets the state vector + **/ + void setState(const Eigen::VectorXd& _state, const bool _notify = true); + + /** \brief Returns the state size + **/ + SizeEigen getSize() const; + + /**\brief Returns the size of the local parametrization + */ + SizeEigen getLocalSize() const; + + /** \brief Returns if the state is fixed (not estimated) + **/ + bool isFixed() const; + + /** \brief Sets the state as fixed + **/ + void fix(); + + /** \brief Sets the state as estimated + **/ + void unfix(); + + /** \brief Sets the state status + **/ + void setFixed(bool _fixed); + + /** \brief Returns if the state has a local parametrization + **/ + bool hasLocalParametrization() const; + + /** \brief Returns the state local parametrization ptr + **/ + LocalParametrizationBasePtr getLocalParametrization() const; + + /** \brief Sets a local parametrization + **/ + void setLocalParametrization(LocalParametrizationBasePtr _local_param); + + /** \brief Removes the state_block local parametrization + **/ + void removeLocalParametrization(); + + /** \brief Return if state has been updated + **/ + bool stateUpdated() const; + + /** \brief Return if fix/unfix has been updated + **/ + bool fixUpdated() const; + + /** \brief Return if local parameterization has been updated + **/ + bool localParamUpdated() const; + + /** \brief Set state_updated_ to false + **/ + void resetStateUpdated(); + + /** \brief Set fix_updated_ to false + **/ + void resetFixUpdated(); + + /** \brief Set localk_param_updated_ to false + **/ + void resetLocalParamUpdated(); + + virtual void setIdentity(bool _notify = true); + void setZero(bool _notify = true); + + virtual Eigen::VectorXd identity() const; + Eigen::VectorXd zero() const; - /** \brief Returns if the state is fixed (not estimated) - **/ - bool isFixed() const; + /** + * \brief perturb all states states with random noise + * following an uniform distribution in [ -amplitude, amplitude ] + */ + void perturb(double amplitude = 0.1); - /** \brief Sets the state as fixed - **/ - void fix(); + bool isTransformable() const + { + return transformable_; + } - /** \brief Sets the state as estimated - **/ - void unfix(); + void setTransformable(bool _trf = true) + { + transformable_ = _trf; + } - /** \brief Sets the state status - **/ - void setFixed(bool _fixed); + void setNonTransformable() + { + transformable_ = false; + } - /** \brief Returns if the state has a local parametrization - **/ - bool hasLocalParametrization() const; + virtual void transform(const VectorComposite& _transformation) = 0; - /** \brief Returns the state local parametrization ptr - **/ - LocalParametrizationBasePtr getLocalParametrization() const; - - /** \brief Sets a local parametrization - **/ - void setLocalParametrization(LocalParametrizationBasePtr _local_param); - - /** \brief Removes the state_block local parametrization - **/ - void removeLocalParametrization(); - - /** \brief Return if state has been updated - **/ - bool stateUpdated() const; - - /** \brief Return if fix/unfix has been updated - **/ - bool fixUpdated() const; - - /** \brief Return if local parameterization has been updated - **/ - bool localParamUpdated() const; - - /** \brief Set state_updated_ to false - **/ - void resetStateUpdated(); - - /** \brief Set fix_updated_ to false - **/ - void resetFixUpdated(); - - /** \brief Set localk_param_updated_ to false - **/ - void resetLocalParamUpdated(); - - virtual void setIdentity(bool _notify = true); - void setZero (bool _notify = true); - - virtual Eigen::VectorXd identity() const; - Eigen::VectorXd zero() const; - - /** - * \brief perturb all states states with random noise - * following an uniform distribution in [ -amplitude, amplitude ] - */ - void perturb(double amplitude = 0.1); - - bool isTransformable() const { - return transformable_; - } - - void setTransformable(bool _trf = true) {transformable_ = _trf;} - - void setNonTransformable() {transformable_ = false;} - - virtual void transform(const VectorComposite& _transformation) = 0; - - void plus(const Eigen::VectorXd& _dv); - - bool isValid(double tolerance = Constants::EPS); + void plus(const Eigen::VectorXd& _dv); + bool isValid(double tolerance = Constants::EPS); }; - -} // namespace wolf +} // namespace wolf // IMPLEMENTATION #include "core/state_block/local_parametrization_base.h" #include "core/common/node_base.h" #include "core/problem/problem.h" -namespace wolf { - -inline StateBlock::StateBlock(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) : - type_(_type), - fixed_(_fixed), - state_size_(_state.size()), - state_(_state), - local_param_ptr_(_local_param_ptr), - state_updated_(false), - fix_updated_(false), - local_param_updated_(false), - transformable_(_transformable) +namespace wolf { -// std::cout << "constructed +sb" << std::endl; +inline StateBlock::StateBlock(const std::string& _type, + const Eigen::VectorXd& _state, + bool _fixed, + LocalParametrizationBasePtr _local_param_ptr, + bool _transformable) + : type_(_type), + fixed_(_fixed), + state_size_(_state.size()), + state_(_state), + local_param_ptr_(_local_param_ptr), + state_updated_(false), + fix_updated_(false), + local_param_updated_(false), + transformable_(_transformable) +{ + // std::cout << "constructed +sb" << std::endl; } -inline StateBlock::StateBlock(const std::string& _type, const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) : - type_(_type), - fixed_(_fixed), - state_size_(_size), - state_(Eigen::VectorXd::Zero(_size)), - local_param_ptr_(_local_param_ptr), - state_updated_(false), - fix_updated_(false), - local_param_updated_(false), - transformable_(_transformable) +inline StateBlock::StateBlock(const std::string& _type, + const SizeEigen _size, + bool _fixed, + LocalParametrizationBasePtr _local_param_ptr, + bool _transformable) + : type_(_type), + fixed_(_fixed), + state_size_(_size), + state_(Eigen::VectorXd::Zero(_size)), + local_param_ptr_(_local_param_ptr), + state_updated_(false), + fix_updated_(false), + local_param_updated_(false), + transformable_(_transformable) { // std::cout << "constructed +sb" << std::endl; } inline StateBlock::~StateBlock() { -// std::cout << "destructed -sb" << std::endl; + // std::cout << "destructed -sb" << std::endl; } inline std::string StateBlock::getType() const @@ -286,8 +308,7 @@ inline SizeEigen StateBlock::getSize() const inline SizeEigen StateBlock::getLocalSize() const { - if(local_param_ptr_) - return local_param_ptr_->getLocalSize(); + if (local_param_ptr_) return local_param_ptr_->getLocalSize(); return getSize(); } @@ -318,14 +339,14 @@ inline LocalParametrizationBasePtr StateBlock::getLocalParametrization() const inline void StateBlock::removeLocalParametrization() { - assert(local_param_ptr_ != nullptr && "state block without local parametrization"); + assert(local_param_ptr_ != nullptr && "state block without local parametrization"); local_param_ptr_.reset(); local_param_updated_.store(true); } inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param) { - assert(_local_param != nullptr && "setting a null local parametrization"); + assert(_local_param != nullptr && "setting a null local parametrization"); local_param_ptr_ = _local_param; local_param_updated_.store(true); } @@ -367,7 +388,7 @@ inline double* StateBlock::getStateData() inline void StateBlock::setIdentity(bool _notify) { - setState( Eigen::VectorXd::Zero(state_size_), _notify ); + setState(Eigen::VectorXd::Zero(state_size_), _notify); } inline void StateBlock::setZero(bool _notify) @@ -390,4 +411,4 @@ inline bool StateBlock::isValid(double tolerance) return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true; } -}// namespace wolf +} // namespace wolf diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h index 508a405350d89ff9794f72d69af78b01fe32b9a8..d105cf2da5955749c97cae423635950c0b112c08 100644 --- a/include/core/state_block/state_block_derived.h +++ b/include/core/state_block/state_block_derived.h @@ -24,7 +24,6 @@ namespace wolf { - /** * @brief State block for general parameters * @@ -38,15 +37,16 @@ template <size_t size> class StateParams : public StateBlock { public: - StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : - StateBlock("StateParams" + toString(size), _state, _fixed, nullptr, false) + StateParams(const Eigen::VectorXd& _state, bool _fixed = false) + : StateBlock("StateParams" + toString(size), _state, _fixed, nullptr, false) { - if (_state.size() != size) - throw std::runtime_error("Wrong vector size for StateParams" + toString(size) + ":" + toString(_state.size())); + if (_state.size() != size) + throw std::runtime_error("Wrong vector size for StateParams" + toString(size) + ":" + + toString(_state.size())); } static Eigen::VectorXd Identity() { - return Eigen::Matrix<double,size,1>::Zero(); + return Eigen::Matrix<double, size, 1>::Zero(); } WOLF_STATE_BLOCK_CREATE(StateParams<size>); void transform(const VectorComposite& _transformation) override @@ -81,7 +81,7 @@ class StatePoint2d : public StateBlock StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) : StateBlock("StatePoint2d", _state, _fixed, nullptr, _transformable) { - if (_state.size() != 2) + if (_state.size() != 2) throw std::runtime_error("Wrong vector size for StatePoint2d: " + toString(_state.size())); } static Eigen::VectorXd Identity() @@ -111,7 +111,7 @@ class StateVector2d : public StateBlock StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) : StateBlock("StateVector2d", _state, _fixed, nullptr, _transformable) { - if (_state.size() != 2) + if (_state.size() != 2) throw std::runtime_error("Wrong vector size for StateVector2d: " + toString(_state.size())); } static Eigen::VectorXd Identity() @@ -137,12 +137,10 @@ class StateVector2d : public StateBlock class StatePoint3d : public StateBlock { public: - StatePoint3d(const Eigen::VectorXd& _state, - bool _fixed = false, - bool _transformable = true) + StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) : StateBlock("StatePoint3d", _state, _fixed, nullptr, _transformable) { - if (_state.size() != 3) + if (_state.size() != 3) throw std::runtime_error("Wrong vector size for StatePoint3d: " + toString(_state.size())); } static Eigen::VectorXd Identity() @@ -173,7 +171,7 @@ class StateVector3d : public StateBlock StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) : StateBlock("StateVector3d", _state, _fixed, nullptr, _transformable) { - if (_state.size() != 3) + if (_state.size() != 3) throw std::runtime_error("Wrong vector size for StateVector3d: " + toString(_state.size())); } static Eigen::VectorXd Identity() diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h index 608de913b16afae02fae8f0f13ffd5a2379ae8ce..2da7eba410d893a00dd275cf9aa1946245038e63 100644 --- a/include/core/state_block/state_homogeneous_3d.h +++ b/include/core/state_block/state_homogeneous_3d.h @@ -23,36 +23,35 @@ #include "core/state_block/state_block.h" #include "core/state_block/local_parametrization_homogeneous.h" -namespace wolf { - +namespace wolf +{ class StateHomogeneous3d : public StateBlock { - public: - StateHomogeneous3d(bool _fixed = false, bool _transformable = true); - StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true); - ~StateHomogeneous3d() override; - static Eigen::VectorXd Identity(); - WOLF_STATE_BLOCK_CREATE(StateHomogeneous3d); + public: + StateHomogeneous3d(bool _fixed = false, bool _transformable = true); + StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true); + ~StateHomogeneous3d() override; + static Eigen::VectorXd Identity(); + WOLF_STATE_BLOCK_CREATE(StateHomogeneous3d); - void setIdentity(bool _notify = true) override; - Eigen::VectorXd identity() const override; + void setIdentity(bool _notify = true) override; + Eigen::VectorXd identity() const override; - virtual void transform(const VectorComposite& _transformation) override; + virtual void transform(const VectorComposite& _transformation) override; }; -inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) : - StateBlock("StateHomogeneous3d", _state, _fixed, nullptr, _transformable) +inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) + : StateBlock("StateHomogeneous3d", _state, _fixed, nullptr, _transformable) { - if(_state.size() != 4) - throw std::runtime_error("Homogeneous 3d must be size 4."); + if (_state.size() != 4) throw std::runtime_error("Homogeneous 3d must be size 4."); local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>(); } -inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) : - StateBlock("StateHomogeneous3d", 4, _fixed, nullptr, _transformable) +inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) + : StateBlock("StateHomogeneous3d", 4, _fixed, nullptr, _transformable) { local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>(); - state_ << 0, 0, 0, 1; // Set origin + state_ << 0, 0, 0, 1; // Set origin } inline StateHomogeneous3d::~StateHomogeneous3d() @@ -78,7 +77,8 @@ inline Eigen::VectorXd StateHomogeneous3d::Identity() inline void StateHomogeneous3d::transform(const VectorComposite& _transformation) { if (transformable_) - setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!! + setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())) + .coeffs()); // TODO is this correct?????? probably not!!! } -} // namespace wolf +} // namespace wolf diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h index 00d3c00bd62d2b56ba50d028e4cc29809b30a692..30dbf4b9dcc834fca80699bd2c57c825443b30ef 100644 --- a/include/core/state_block/state_quaternion.h +++ b/include/core/state_block/state_quaternion.h @@ -23,49 +23,58 @@ #include "core/state_block/state_block.h" #include "core/state_block/local_parametrization_quaternion.h" -namespace wolf { - +namespace wolf +{ class StateQuaternion : public StateBlock { - public: - StateQuaternion(bool _fixed = false, bool _transformable = true); - StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true); - StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true); - ~StateQuaternion() override; - static Eigen::VectorXd Identity(); - WOLF_STATE_BLOCK_CREATE(StateQuaternion); - - void setIdentity(bool _notify = true) override; - Eigen::VectorXd identity() const override; - - virtual void transform(const VectorComposite& _transformation) override; + public: + StateQuaternion(bool _fixed = false, bool _transformable = true); + StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true); + StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true); + ~StateQuaternion() override; + static Eigen::VectorXd Identity(); + WOLF_STATE_BLOCK_CREATE(StateQuaternion); + + void setIdentity(bool _notify = true) override; + Eigen::VectorXd identity() const override; + + virtual void transform(const VectorComposite& _transformation) override; }; -inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) : - StateBlock("StateQuaternion", _quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) +inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) + : StateBlock("StateQuaternion", + _quaternion.coeffs(), + _fixed, + std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), + _transformable) { // TODO: remove these lines after issue #381 is addressed - if(not isValid(1e-5)) - throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!"); - + if (not isValid(1e-5)) throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!"); + state_.normalize(); } -inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) : - StateBlock("StateQuaternion", _state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) +inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) + : StateBlock("StateQuaternion", + _state, + _fixed, + std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), + _transformable) { - if(state_.size() != 4) - throw std::runtime_error("The quaternion state vector must be of size 4!"); + if (state_.size() != 4) throw std::runtime_error("The quaternion state vector must be of size 4!"); // TODO: remove these lines after issue #381 is addressed - if(not isValid(1e-5)) - throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!"); + if (not isValid(1e-5)) throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!"); state_.normalize(); } -inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) : - StateBlock("StateQuaternion", 4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) +inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) + : StateBlock("StateQuaternion", + 4, + _fixed, + std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), + _transformable) { state_ = Eigen::Quaterniond::Identity().coeffs(); // @@ -97,4 +106,4 @@ inline void StateQuaternion::transform(const VectorComposite& _transformation) setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); } -} // namespace wolf +} // namespace wolf diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 60627e9d97770b37355e1f5ec3cbec5da3426830..5f16217e22fc415cd26c2044383d56413080b0a2 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -68,26 +68,21 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj bool hasTimeStamp(const TimeStamp& _ts) const; bool hasFrame(FrameBaseConstPtr _frame) const; - virtual void printHeader(int depth, // + virtual void printHeader(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream, std::string _tabs = "") const; - void print(int depth, // + void print(int depth, // bool factored_by, // - bool metric, // + bool metric, // bool state_blocks, std::ostream& stream = std::cout, std::string _tabs = "") const; - virtual CheckLog localCheck(bool _verbose, - std::ostream& _stream, - std::string _tabs = "") const; - bool check(CheckLog& _log, - bool _verbose, - std::ostream& _stream, - std::string _tabs = "") const; + virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const; + bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const; private: FrameBasePtr addFrame(FrameBasePtr _frame_ptr); @@ -152,10 +147,11 @@ inline bool TrajectoryBase::hasTimeStamp(const TimeStamp& _ts) const inline bool TrajectoryBase::hasFrame(FrameBaseConstPtr _frame) const { - return std::find_if( - frame_map_.cbegin(), frame_map_.cend(), [&_frame](const std::pair<TimeStamp, FrameBaseConstPtr>& frm_it) { - return frm_it.second == _frame; - }) != frame_map_.end(); + return std::find_if(frame_map_.cbegin(), + frame_map_.cend(), + [&_frame](const std::pair<TimeStamp, FrameBaseConstPtr>& frm_it) { + return frm_it.second == _frame; + }) != frame_map_.end(); } } // namespace wolf diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h index 7fcbe3ec641e6690af6817477b2c47d28b867344..d77b9dd26a42ff7bd7988260cc408d769819083c 100644 --- a/include/core/tree_manager/factory_tree_manager.h +++ b/include/core/tree_manager/factory_tree_manager.h @@ -28,28 +28,31 @@ namespace wolf { - -typedef Factory<std::shared_ptr<TreeManagerBase>, - const YAML::Node&> FactoryTreeManager; -template<> +typedef Factory<std::shared_ptr<TreeManagerBase>, const YAML::Node&> FactoryTreeManager; +template <> inline std::string FactoryTreeManager::getClass() const { return "FactoryTreeManager"; } -typedef Factory<std::shared_ptr<TreeManagerBase>, - const std::string&, - const std::vector<std::string>&> FactoryTreeManagerYaml; -template<> +typedef Factory<std::shared_ptr<TreeManagerBase>, const std::string&, const std::vector<std::string>&> + FactoryTreeManagerYaml; +template <> inline std::string FactoryTreeManagerYaml::getClass() const { return "FactoryTreeManagerYaml"; } -#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \ - namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \ - wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \ - namespace{ const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \ - wolf::FactoryTreeManagerYaml::registerCreator(#TreeManagerType, TreeManagerType::create); } \ +#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \ + namespace \ + { \ + const bool WOLF_UNUSED TreeManagerType##Registered = \ + wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); \ + } \ + namespace \ + { \ + const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \ + wolf::FactoryTreeManagerYaml::registerCreator(#TreeManagerType, TreeManagerType::create); \ + } } /* namespace wolf */ diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h index 3e43ccfab8eab538827bbc3212df9e0fc96ba4a3..af62e3e780e26e5ecfd730c2b32b6dbb0acdbbd8 100644 --- a/include/core/tree_manager/tree_manager_base.h +++ b/include/core/tree_manager/tree_manager_base.h @@ -39,39 +39,36 @@ namespace wolf * must have a constructor available with the API: * * TreeManagerClass(const ParamsTreeManagerPtr _params); - * - * Also, there should be the schema file 'TreeManagerClass.schema' containing the specifications + * + * Also, there should be the schema file 'TreeManagerClass.schema' containing the specifications * of the user input yaml file. */ -#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \ -static TreeManagerBasePtr create(const YAML::Node& _input_node) \ -{ \ - auto params = std::make_shared<ParamsTreeManagerClass>(_input_node); \ - \ - return std::make_shared<TreeManagerClass>(params); \ -} \ -static TreeManagerBasePtr create(const std::string& _yaml_filepath, \ - const std::vector<std::string>& _folders_schema) \ -{ \ - auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ - \ - if (not server.applySchema(#TreeManagerClass)) \ - { \ - WOLF_ERROR(server.getLog()); \ - return nullptr; \ - } \ - auto params = std::make_shared<ParamsTreeManagerClass>(server.getNode()); \ - \ - return std::make_shared<TreeManagerClass>(params); \ -} \ +#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \ + static TreeManagerBasePtr create(const YAML::Node& _input_node) \ + { \ + auto params = std::make_shared<ParamsTreeManagerClass>(_input_node); \ + \ + return std::make_shared<TreeManagerClass>(params); \ + } \ + static TreeManagerBasePtr create(const std::string& _yaml_filepath, \ + const std::vector<std::string>& _folders_schema) \ + { \ + auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ + \ + if (not server.applySchema(#TreeManagerClass)) \ + { \ + WOLF_ERROR(server.getLog()); \ + return nullptr; \ + } \ + auto params = std::make_shared<ParamsTreeManagerClass>(server.getNode()); \ + \ + return std::make_shared<TreeManagerClass>(params); \ + } struct ParamsTreeManagerBase : public ParamsBase { ParamsTreeManagerBase() = default; - ParamsTreeManagerBase(const YAML::Node& _input_node): - ParamsBase(_input_node) - { - } + ParamsTreeManagerBase(const YAML::Node& _input_node) : ParamsBase(_input_node) {} ~ParamsTreeManagerBase() override = default; @@ -83,20 +80,23 @@ struct ParamsTreeManagerBase : public ParamsBase class TreeManagerBase : public NodeBase { - public: - TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params) : - NodeBase("TREE_MANAGER", _type), - params_(_params) - {} + public: + TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params) + : NodeBase("TREE_MANAGER", _type), params_(_params) + { + } - virtual ~TreeManagerBase() {} + virtual ~TreeManagerBase() {} - virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0; + virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0; - bool hasChildren() const override{return true;}; + bool hasChildren() const override + { + return true; + }; - protected: - ParamsTreeManagerBasePtr params_; + protected: + ParamsTreeManagerBasePtr params_; }; } /* namespace wolf */ diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h index 9f0c0e8c0749de3f15ef073e8f3e2aca6868fafd..7e86ed37836b886c43ca20390a5cfaeb8bf91bea 100644 --- a/include/core/tree_manager/tree_manager_sliding_window.h +++ b/include/core/tree_manager/tree_manager_sliding_window.h @@ -24,50 +24,47 @@ namespace wolf { - WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindow) WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindow) struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase { - ParamsTreeManagerSlidingWindow() = default; - ParamsTreeManagerSlidingWindow(const YAML::Node& _node_input) : - ParamsTreeManagerBase(_node_input) - { - n_frames = _node_input["n_frames"] .as<unsigned int>(); - n_fix_first_frames = _node_input["n_fix_first_frames"] .as<unsigned int>(); - viral_remove_parent_without_children = _node_input["viral_remove_parent_without_children"] .as<bool>(); - if (n_frames <= n_fix_first_frames) - throw std::runtime_error("TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than 'n_frames'!"); - } - std::string print() const override - { - return ParamsTreeManagerBase::print() + "\n" - + "n_frames: " + toString(n_frames) + "\n" - + "n_fix_first_frames: " + toString(n_fix_first_frames) + "\n" - + "viral_remove_parent_without_children: " + toString(viral_remove_parent_without_children) + "\n"; - } + ParamsTreeManagerSlidingWindow() = default; + ParamsTreeManagerSlidingWindow(const YAML::Node& _node_input) : ParamsTreeManagerBase(_node_input) + { + n_frames = _node_input["n_frames"].as<unsigned int>(); + n_fix_first_frames = _node_input["n_fix_first_frames"].as<unsigned int>(); + viral_remove_parent_without_children = _node_input["viral_remove_parent_without_children"].as<bool>(); + if (n_frames <= n_fix_first_frames) + throw std::runtime_error( + "TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than " + "'n_frames'!"); + } + std::string print() const override + { + return ParamsTreeManagerBase::print() + "\n" + "n_frames: " + toString(n_frames) + "\n" + + "n_fix_first_frames: " + toString(n_fix_first_frames) + "\n" + + "viral_remove_parent_without_children: " + toString(viral_remove_parent_without_children) + "\n"; + } - unsigned int n_frames; - unsigned int n_fix_first_frames; - bool viral_remove_parent_without_children; + unsigned int n_frames; + unsigned int n_fix_first_frames; + bool viral_remove_parent_without_children; }; class TreeManagerSlidingWindow : public TreeManagerBase { - public: - TreeManagerSlidingWindow(ParamsTreeManagerSlidingWindowPtr _params) : - TreeManagerBase("TreeManagerSlidingWindow", _params), - params_sw_(_params) - {}; - WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow) + public: + TreeManagerSlidingWindow(ParamsTreeManagerSlidingWindowPtr _params) + : TreeManagerBase("TreeManagerSlidingWindow", _params), params_sw_(_params){}; + WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow) - ~TreeManagerSlidingWindow() override{} + ~TreeManagerSlidingWindow() override {} - void keyFrameCallback(FrameBasePtr _frame) override; + void keyFrameCallback(FrameBasePtr _frame) override; - protected: - ParamsTreeManagerSlidingWindowPtr params_sw_; + protected: + ParamsTreeManagerSlidingWindowPtr params_sw_; }; } /* namespace wolf */ diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h index 33dd309665cd24a77b86a6cbcdd065a0488c551e..aa1fa012304519a49404a053e4f258bd9daa7958 100644 --- a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h +++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h @@ -24,48 +24,46 @@ namespace wolf { - WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate) WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate) struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow { - ParamsTreeManagerSlidingWindowDualRate() = default; - ParamsTreeManagerSlidingWindowDualRate(const YAML::Node& _input_node) : - ParamsTreeManagerSlidingWindow(_input_node) - { - n_frames_recent = _input_node["n_frames_recent"].as<unsigned int>(); - rate_old_frames = _input_node["rate_old_frames"].as<unsigned int>(); - if (n_frames_recent >= n_frames) - { - throw std::runtime_error("ParamsTreeManagerSlidingWindowDualRate: 'n_frames_recent' should be smaller than 'n_frames'"); - } - } - std::string print() const override + ParamsTreeManagerSlidingWindowDualRate() = default; + ParamsTreeManagerSlidingWindowDualRate(const YAML::Node& _input_node) : ParamsTreeManagerSlidingWindow(_input_node) + { + n_frames_recent = _input_node["n_frames_recent"].as<unsigned int>(); + rate_old_frames = _input_node["rate_old_frames"].as<unsigned int>(); + if (n_frames_recent >= n_frames) { - return ParamsTreeManagerBase::print() + "\n" - + "n_frames_recent: " + toString(n_frames_recent) + "\n" - + "rate_old_frames: " + toString(rate_old_frames) + "\n"; + throw std::runtime_error( + "ParamsTreeManagerSlidingWindowDualRate: 'n_frames_recent' should be smaller than 'n_frames'"); } + } + std::string print() const override + { + return ParamsTreeManagerBase::print() + "\n" + "n_frames_recent: " + toString(n_frames_recent) + "\n" + + "rate_old_frames: " + toString(rate_old_frames) + "\n"; + } - unsigned int n_frames_recent, rate_old_frames; + unsigned int n_frames_recent, rate_old_frames; }; class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow { - public: - TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params); - - WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate) + public: + TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params); + + WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate) - ~TreeManagerSlidingWindowDualRate() override{} + ~TreeManagerSlidingWindowDualRate() override {} - void keyFrameCallback(FrameBasePtr _frame) override; + void keyFrameCallback(FrameBasePtr _frame) override; - protected: - ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_; - unsigned int count_frames_; - //TrajectoryIter first_recent_frame_it_; + protected: + ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_; + unsigned int count_frames_; + // TrajectoryIter first_recent_frame_it_; }; } /* namespace wolf */ diff --git a/include/core/utils/eigen_assert.h b/include/core/utils/eigen_assert.h index a6035cc8b236e8199146a0daac0b5e67d1298e6b..c478277fc3a82d8ccccc7e7ab945bbf5242c0cff 100644 --- a/include/core/utils/eigen_assert.h +++ b/include/core/utils/eigen_assert.h @@ -22,8 +22,8 @@ #include <Eigen/Dense> -namespace Eigen { - +namespace Eigen +{ ////////////////////////////////////////////////////////// /** Check Eigen Matrix sizes, both statically and dynamically * @@ -32,7 +32,8 @@ namespace Eigen { * The WOLF project implements many template functions using Eigen Matrix and Quaterniond, in different versions * (Static size, Dynamic size, Map, Matrix expression). * - * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h): + * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's + * StaticAssert.h): * * EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE * EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE @@ -40,7 +41,8 @@ namespace Eigen { * * but they do not work if the evaluated types are of dynamic size. * - * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind: + * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this + * kind: * * template<typename Derived> * inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){ @@ -57,44 +59,44 @@ namespace Eigen { * The function : MatrixSizeCheck <Rows, Cols>::check(M) checks that the Matrix M is of size ( Rows x Cols ). * This check is performed statically or dynamically, depending on the type of argument provided. */ -template<int Size, int DesiredSize> +template <int Size, int DesiredSize> struct StaticDimCheck { - template<typename T> - StaticDimCheck(const T&) - { - static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match"); - } + template <typename T> + StaticDimCheck(const T&) + { + static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match"); + } }; -template<int DesiredSize> +template <int DesiredSize> struct StaticDimCheck<Eigen::Dynamic, DesiredSize> { - template<typename T> - StaticDimCheck(const T& t) - { - if (t != DesiredSize) std::cerr << "t : " << t << " != DesiredSize : " << DesiredSize << std::endl; - assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match"); - } + template <typename T> + StaticDimCheck(const T& t) + { + if (t != DesiredSize) std::cerr << "t : " << t << " != DesiredSize : " << DesiredSize << std::endl; + assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match"); + } }; -template<int DesiredR, int DesiredC> +template <int DesiredR, int DesiredC> struct MatrixSizeCheck { - /** \brief Assert matrix size dynamically or statically - * - * @param t the variable for which we wish to assert the size. - * - * Usage: to assert that t is size (Rows x Cols) - * - * MatrixSizeCheck<Rows, Cols>::check(t); - */ - template<typename T> - static void check(const Eigen::MatrixBase<T>& t) - { - StaticDimCheck<Eigen::MatrixBase<T>::RowsAtCompileTime, DesiredR>(t.rows()); - StaticDimCheck<Eigen::MatrixBase<T>::ColsAtCompileTime, DesiredC>(t.cols()); - } + /** \brief Assert matrix size dynamically or statically + * + * @param t the variable for which we wish to assert the size. + * + * Usage: to assert that t is size (Rows x Cols) + * + * MatrixSizeCheck<Rows, Cols>::check(t); + */ + template <typename T> + static void check(const Eigen::MatrixBase<T>& t) + { + StaticDimCheck<Eigen::MatrixBase<T>::RowsAtCompileTime, DesiredR>(t.rows()); + StaticDimCheck<Eigen::MatrixBase<T>::ColsAtCompileTime, DesiredC>(t.cols()); + } }; template <int Dim> diff --git a/include/core/utils/graph_search.h b/include/core/utils/graph_search.h index 142f40d06626df24dd9e502852fb8028172df66c..b5e4abcd6e5e37f6dba5f8b2157c422e1cdafef2 100644 --- a/include/core/utils/graph_search.h +++ b/include/core/utils/graph_search.h @@ -28,34 +28,30 @@ namespace wolf { - class GraphSearch { - private: - - std::map<NodeStateBlocksPtr, std::pair<FactorBasePtr,NodeStateBlocksPtr>> parents_; - - public: - - GraphSearch(); + private: + std::map<NodeStateBlocksPtr, std::pair<FactorBasePtr, NodeStateBlocksPtr>> parents_; - ~GraphSearch(); + public: + GraphSearch(); - FactorBasePtrList computeShortestPath(NodeStateBlocksPtr node1, - NodeStateBlocksPtr node2, - const unsigned int max_graph_dist = 0); + ~GraphSearch(); - std::set<NodeStateBlocksPtr> getNeighborFrames(const std::set<NodeStateBlocksPtr>& nodes); + FactorBasePtrList computeShortestPath(NodeStateBlocksPtr node1, + NodeStateBlocksPtr node2, + const unsigned int max_graph_dist = 0); - static FactorBasePtrList shortestPath(NodeStateBlocksPtr node1, - NodeStateBlocksPtr node2, - const unsigned int max_graph_dist = 0) - { - GraphSearch graph_search; + std::set<NodeStateBlocksPtr> getNeighborFrames(const std::set<NodeStateBlocksPtr>& nodes); - return graph_search.computeShortestPath(node1, node2, max_graph_dist); - } + static FactorBasePtrList shortestPath(NodeStateBlocksPtr node1, + NodeStateBlocksPtr node2, + const unsigned int max_graph_dist = 0) + { + GraphSearch graph_search; + return graph_search.computeShortestPath(node1, node2, max_graph_dist); + } }; } // namespace wolf diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h index 23a54c750e8f5a6c785c5c60c84dc4d1e150251a..8255d1ec4506c14a3c722cffc722ad4580028302 100644 --- a/include/core/utils/loader.h +++ b/include/core/utils/loader.h @@ -27,7 +27,6 @@ namespace wolf { - class Loader { protected: diff --git a/include/core/utils/logging.h b/include/core/utils/logging.h index 09af0b8d11ee66997bde8db73be9ea170e296afd..86b02e32fa48426f951689410836cdd7dd4a256b 100644 --- a/include/core/utils/logging.h +++ b/include/core/utils/logging.h @@ -31,235 +31,231 @@ // Wolf includes #include "core/utils/singleton.h" -namespace wolf { -namespace internal { -namespace do_not_enter_where_the_wolf_lives { - +namespace wolf +{ +namespace internal +{ +namespace do_not_enter_where_the_wolf_lives +{ #define __INTERNAL_WOLF_MAIN_LOGGER_NAME_ "wolf_main_console" -static const auto repeated_brace = std::make_tuple("{}", - "{}{}", - "{}{}{}", - "{}{}{}{}", - "{}{}{}{}{}", - "{}{}{}{}{}{}", - "{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", - "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}"); // up to 25 args. Should be fine +static const auto repeated_brace = + std::make_tuple("{}", + "{}{}", + "{}{}{}", + "{}{}{}{}", + "{}{}{}{}{}", + "{}{}{}{}{}{}", + "{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", + "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}"); // up to 25 args. Should be fine class Logger { -public: + public: + Logger(const std::string& name); - Logger(const std::string& name); + Logger(std::string&& name); - Logger(std::string&& name); + ~Logger(); - ~Logger(); + // Not copyable/movable + Logger(Logger&) = delete; + void operator=(Logger&) = delete; + Logger(Logger&&) = delete; + void operator=(Logger&&) = delete; - // Not copyable/movable - Logger(Logger&) = delete; - void operator=(Logger&) = delete; - Logger(Logger&&) = delete; - void operator=(Logger&&) = delete; + template <typename... Args> + void info(Args&&... args) const; - template<typename... Args> - void info(Args&&... args) const; + template <typename... Args> + void warn(Args&&... args) const; - template<typename... Args> - void warn(Args&&... args) const; + template <typename... Args> + void error(Args&&... args) const; - template<typename... Args> - void error(Args&&... args) const; + template <typename... Args> + void debug(Args&&... args) const; - template<typename... Args> - void debug(Args&&... args) const; + template <typename... Args> + void trace(Args&&... args) const; - template<typename... Args> - void trace(Args&&... args) const; + bool set_async_queue(const std::size_t q_size); - bool set_async_queue(const std::size_t q_size); + void set_pattern(const std::string& p); - void set_pattern(const std::string& p); + protected: + const std::string log_name_; -protected: - - const std::string log_name_; - - std::shared_ptr<spdlog::logger> console_; + std::shared_ptr<spdlog::logger> console_; }; -inline Logger::Logger(const std::string& name) : - log_name_(name) +inline Logger::Logger(const std::string& name) : log_name_(name) { - // Create main logger - console_ = spdlog::stdout_color_mt(log_name_); + // Create main logger + console_ = spdlog::stdout_color_mt(log_name_); #ifdef _WOLF_TRACE - console_->set_level(spdlog::level::trace); + console_->set_level(spdlog::level::trace); #endif - // Enable asynchronous logging - // Queue size must be a power of 2 - spdlog::set_async_mode(4096); - - if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_) - // Logging pattern is : - // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content - //set_pattern("[%t][%H:%M:%S.%F][%l] %v"); - // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content -// set_pattern("[%l][%x - %H:%M:%S.%F] %v"); - set_pattern("[%l][%H:%M:%S] %v"); - else - // Logging pattern is : - // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content - //set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v"); - // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content - set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v"); + // Enable asynchronous logging + // Queue size must be a power of 2 + spdlog::set_async_mode(4096); + + if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_) + // Logging pattern is : + // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content + // set_pattern("[%t][%H:%M:%S.%F][%l] %v"); + // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content + // set_pattern("[%l][%x - %H:%M:%S.%F] %v"); + set_pattern("[%l][%H:%M:%S] %v"); + else + // Logging pattern is : + // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content + // set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v"); + // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content + set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v"); } -inline Logger::Logger(std::string&& name) : - log_name_(std::forward<std::string>(name)) +inline Logger::Logger(std::string&& name) : log_name_(std::forward<std::string>(name)) { - // Create main logger - console_ = spdlog::stdout_color_mt(log_name_); + // Create main logger + console_ = spdlog::stdout_color_mt(log_name_); #ifdef _WOLF_TRACE - console_->set_level(spdlog::level::trace); + console_->set_level(spdlog::level::trace); #endif - // Enable asynchronous logging - // Queue size must be a power of 2 - spdlog::set_async_mode(4096); - - if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_) - // Logging pattern is : - // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content - //set_pattern("[%t][%H:%M:%S.%F][%l] %v"); - // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content -// set_pattern("[%l][%x - %H:%M:%S.%F] %v"); - set_pattern("[%l][%H:%M:%S] %v"); - else - // Logging pattern is : - // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content - //set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v"); - // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content - set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v"); + // Enable asynchronous logging + // Queue size must be a power of 2 + spdlog::set_async_mode(4096); + + if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_) + // Logging pattern is : + // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content + // set_pattern("[%t][%H:%M:%S.%F][%l] %v"); + // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content + // set_pattern("[%l][%x - %H:%M:%S.%F] %v"); + set_pattern("[%l][%H:%M:%S] %v"); + else + // Logging pattern is : + // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content + // set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v"); + // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content + set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v"); } inline Logger::~Logger() { - spdlog::drop(log_name_); + spdlog::drop(log_name_); } -template<typename... Args> +template <typename... Args> void Logger::info(Args&&... args) const { - console_->info(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...); + console_->info(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...); } -template<typename... Args> +template <typename... Args> void Logger::warn(Args&&... args) const { - console_->warn(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...); + console_->warn(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...); } -template<typename... Args> +template <typename... Args> void Logger::error(Args&&... args) const { - console_->error(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...); + console_->error(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...); } -template<typename... Args> +template <typename... Args> void Logger::debug(Args&&... args) const { - console_->debug(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...); + console_->debug(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...); } -template<typename... Args> +template <typename... Args> void Logger::trace(Args&&... args) const { - console_->trace(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...); + console_->trace(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...); } inline bool Logger::set_async_queue(const std::size_t q_size) { - bool p2 = q_size%2 == 0; + bool p2 = q_size % 2 == 0; - if (p2) spdlog::set_async_mode(q_size); + if (p2) spdlog::set_async_mode(q_size); - return q_size; + return q_size; } inline void Logger::set_pattern(const std::string& p) { - console_->set_pattern(p); + console_->set_pattern(p); } using LoggerPtr = std::unique_ptr<Logger>; class LoggerManager { -public: - - LoggerManager() = default; - ~LoggerManager() = default; - - bool exists(const std::string& name) const - { - std::lock_guard<std::mutex> lock(mut_); - return existsImpl(name); - } - - const Logger& getLogger(const std::string& name) /*const*/ - { - std::lock_guard<std::mutex> lock(mut_); - - if (!existsImpl(name)) addLogger(name); - - return *(logger_map_.at(name)); - } - -protected: - - mutable std::mutex mut_; - - std::map<const std::string, const LoggerPtr> logger_map_; - - bool addLogger(const std::string& name) - { - /// @note would be easier with cpp17 map.try_emplace... - const bool created = existsImpl(name) ? - false : - logger_map_.emplace(name, std::make_unique<Logger>(name)).second; - return created; - } - - bool existsImpl(const std::string& name) const - { - return (logger_map_.find(name) != logger_map_.end()); - //return (spdlog::get(name) != nullptr); - } + public: + LoggerManager() = default; + ~LoggerManager() = default; + + bool exists(const std::string& name) const + { + std::lock_guard<std::mutex> lock(mut_); + return existsImpl(name); + } + + const Logger& getLogger(const std::string& name) /*const*/ + { + std::lock_guard<std::mutex> lock(mut_); + + if (!existsImpl(name)) addLogger(name); + + return *(logger_map_.at(name)); + } + + protected: + mutable std::mutex mut_; + + std::map<const std::string, const LoggerPtr> logger_map_; + + bool addLogger(const std::string& name) + { + /// @note would be easier with cpp17 map.try_emplace... + const bool created = + existsImpl(name) ? false : logger_map_.emplace(name, std::make_unique<Logger>(name)).second; + return created; + } + + bool existsImpl(const std::string& name) const + { + return (logger_map_.find(name) != logger_map_.end()); + // return (spdlog::get(name) != nullptr); + } }; } /* namespace do_not_enter_where_the_wolf_lives */ -using WolfLogger = Singleton<do_not_enter_where_the_wolf_lives::Logger>; +using WolfLogger = Singleton<do_not_enter_where_the_wolf_lives::Logger>; using WolfLoggerManager = Singleton<do_not_enter_where_the_wolf_lives::LoggerManager>; } /* namespace internal */ @@ -270,34 +266,42 @@ using WolfLoggerManager = Singleton<do_not_enter_where_the_wolf_lives::LoggerMan /// @brief NAMED LOGGING -#define WOLF_ASYNC_QUEUE_LOG_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).set_async_queue(x); +#define WOLF_ASYNC_QUEUE_LOG_NAMED(name, ...) \ + wolf::internal::WolfLoggerManager::get().getLogger(name).set_async_queue(x); #define WOLF_INFO_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).info(__VA_ARGS__); -#define WOLF_INFO_NAMED_COND(name, cond, ...) if (cond) WOLF_INFO_NAMED(name, __VA_ARGS__); +#define WOLF_INFO_NAMED_COND(name, cond, ...) \ + if (cond) WOLF_INFO_NAMED(name, __VA_ARGS__); #define WOLF_WARN_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).warn(__VA_ARGS__); -#define WOLF_WARN_NAMED_COND(name, cond, ...) if (cond) WOLF_WARN_NAMED(name, __VA_ARGS__); +#define WOLF_WARN_NAMED_COND(name, cond, ...) \ + if (cond) WOLF_WARN_NAMED(name, __VA_ARGS__); #define WOLF_ERROR_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).error(__VA_ARGS__); -#define WOLF_ERROR_NAMED_COND(name, cond, ...) if (cond) WOLF_ERROR_NAMED(name, __VA_ARGS__); +#define WOLF_ERROR_NAMED_COND(name, cond, ...) \ + if (cond) WOLF_ERROR_NAMED(name, __VA_ARGS__); #ifdef _WOLF_DEBUG - #define WOLF_DEBUG_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).debug(__VA_ARGS__); - #define WOLF_DEBUG_NAMED_COND(name, cond, ...) if (cond) WOLF_DEBUG_NAMED(name, __VA_ARGS__); +#define WOLF_DEBUG_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).debug(__VA_ARGS__); +#define WOLF_DEBUG_NAMED_COND(name, cond, ...) \ + if (cond) WOLF_DEBUG_NAMED(name, __VA_ARGS__); #else - #define WOLF_DEBUG_NAMED(name, ...) - #define WOLF_DEBUG_NAMED_COND(cond, name, ...) +#define WOLF_DEBUG_NAMED(name, ...) +#define WOLF_DEBUG_NAMED_COND(cond, name, ...) #endif #ifdef _WOLF_TRACE - #define WOLF_TRACE_NAMED(name, ...) \ - {char this_file[] = __FILE__;\ - wolf::internal::WolfLoggerManager::get().getLogger(name).trace("[", basename(this_file), " L", __LINE__, \ - " : ", __FUNCTION__, "] ", __VA_ARGS__);} - #define WOLF_TRACE_NAMED_COND(name, cond, ...) if (cond) WOLF_TRACE_NAMED_COND(name, __VA_ARGS__); +#define WOLF_TRACE_NAMED(name, ...) \ + { \ + char this_file[] = __FILE__; \ + wolf::internal::WolfLoggerManager::get().getLogger(name).trace( \ + "[", basename(this_file), " L", __LINE__, " : ", __FUNCTION__, "] ", __VA_ARGS__); \ + } +#define WOLF_TRACE_NAMED_COND(name, cond, ...) \ + if (cond) WOLF_TRACE_NAMED_COND(name, __VA_ARGS__); #else - #define WOLF_TRACE_NAMED(...) - #define WOLF_TRACE_NAMED_cond(name, cond, ...) +#define WOLF_TRACE_NAMED(...) +#define WOLF_TRACE_NAMED_cond(name, cond, ...) #endif /// @brief MAIN LOGGING @@ -305,29 +309,36 @@ using WolfLoggerManager = Singleton<do_not_enter_where_the_wolf_lives::LoggerMan #define WOLF_ASYNC_QUEUE_LOG(x) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).set_async_queue(x); #define WOLF_INFO(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).info(__VA_ARGS__); -#define WOLF_INFO_COND(cond, ...) if (cond) WOLF_INFO(__VA_ARGS__); +#define WOLF_INFO_COND(cond, ...) \ + if (cond) WOLF_INFO(__VA_ARGS__); #define WOLF_WARN(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).warn(__VA_ARGS__); -#define WOLF_WARN_COND(cond, ...) if (cond) WOLF_WARN(__VA_ARGS__); +#define WOLF_WARN_COND(cond, ...) \ + if (cond) WOLF_WARN(__VA_ARGS__); #define WOLF_ERROR(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).error(__VA_ARGS__); -#define WOLF_ERROR_COND(cond, ...) if (cond) WOLF_ERROR(__VA_ARGS__); +#define WOLF_ERROR_COND(cond, ...) \ + if (cond) WOLF_ERROR(__VA_ARGS__); #ifdef _WOLF_DEBUG - #define WOLF_DEBUG(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).debug(__VA_ARGS__); - #define WOLF_DEBUG_COND(cond, ...) if (cond) WOLF_DEBUG(__VA_ARGS__); +#define WOLF_DEBUG(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).debug(__VA_ARGS__); +#define WOLF_DEBUG_COND(cond, ...) \ + if (cond) WOLF_DEBUG(__VA_ARGS__); #else - #define WOLF_DEBUG(...) - #define WOLF_DEBUG_COND(cond, ...) +#define WOLF_DEBUG(...) +#define WOLF_DEBUG_COND(cond, ...) #endif #ifdef _WOLF_TRACE - #define WOLF_TRACE(...) \ - {char this_file[] = __FILE__;\ - wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).trace("[", basename(this_file), " L", __LINE__, \ - " : ", __FUNCTION__, "] ", __VA_ARGS__);} - #define WOLF_TRACE_COND(cond, ...) if (cond) WOLF_TRACE(__VA_ARGS__); +#define WOLF_TRACE(...) \ + { \ + char this_file[] = __FILE__; \ + wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_) \ + .trace("[", basename(this_file), " L", __LINE__, " : ", __FUNCTION__, "] ", __VA_ARGS__); \ + } +#define WOLF_TRACE_COND(cond, ...) \ + if (cond) WOLF_TRACE(__VA_ARGS__); #else - #define WOLF_TRACE(...) - #define WOLF_TRACE_COND(cond, ...) +#define WOLF_TRACE(...) +#define WOLF_TRACE_COND(cond, ...) #endif diff --git a/include/core/utils/singleton.h b/include/core/utils/singleton.h index c92f18c75ecc3dd48538411ae9fadeef3531579a..408073eb1751cd6e84ee319cfa2890740c424030 100644 --- a/include/core/utils/singleton.h +++ b/include/core/utils/singleton.h @@ -22,9 +22,10 @@ #include <memory> -namespace wolf { -namespace internal { - +namespace wolf +{ +namespace internal +{ /** * \brief A thread-safer (?) Singleton implementation with * argument forwarding. @@ -32,29 +33,27 @@ namespace internal { template <class T> class Singleton { - using SingletonOPtr = std::unique_ptr<T>; - -public: - - template <typename... Args> - static T& get(Args&&... args) - { - static SingletonOPtr instance_(new T(std::forward<Args>(args)...)); + using SingletonOPtr = std::unique_ptr<T>; - return *instance_; - } + public: + template <typename... Args> + static T& get(Args&&... args) + { + static SingletonOPtr instance_(new T(std::forward<Args>(args)...)); - constexpr Singleton(const Singleton&) = delete; - constexpr Singleton(const Singleton&&) = delete; + return *instance_; + } - constexpr Singleton& operator=(const Singleton&) const = delete; - constexpr Singleton& operator=(const Singleton&&) const = delete; + constexpr Singleton(const Singleton&) = delete; + constexpr Singleton(const Singleton&&) = delete; -protected: + constexpr Singleton& operator=(const Singleton&) const = delete; + constexpr Singleton& operator=(const Singleton&&) const = delete; - constexpr Singleton() = default; - virtual ~Singleton() = default; + protected: + constexpr Singleton() = default; + virtual ~Singleton() = default; }; -} // namespace internal -} // namespace wolf +} // namespace internal +} // namespace wolf diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h index 97f082db2e91c3ac2cbd0bf6c79adc5f6f31af9e..5f85f806e6902b8a1574262ef7dd93e3b7b109d7 100644 --- a/include/core/utils/utils_gtest.h +++ b/include/core/utils/utils_gtest.h @@ -66,36 +66,40 @@ namespace internal { enum GTestColor { - COLOR_DEFAULT, - COLOR_RED, - COLOR_GREEN, - COLOR_YELLOW + COLOR_DEFAULT, + COLOR_RED, + COLOR_GREEN, + COLOR_YELLOW }; extern void ColoredPrintf(GTestColor color, const char* fmt, ...); -#define PRINTF(...) \ - do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\ - "[ ] "); \ - testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \ - while(0) - -#define PRINT_TEST_FINISHED \ -{ \ - const ::testing::TestInfo* const test_info = \ - ::testing::UnitTest::GetInstance()->current_test_info(); \ - PRINTF(std::string("Finished test case ").append(test_info->test_case_name()).\ - append(" of test ").append(test_info->name()).append(".\n").c_str()); \ -} +#define PRINTF(...) \ + do \ + { \ + testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN, "[ ] "); \ + testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); \ + } while (0) + +#define PRINT_TEST_FINISHED \ + { \ + const ::testing::TestInfo* const test_info = ::testing::UnitTest::GetInstance()->current_test_info(); \ + PRINTF(std::string("Finished test case ") \ + .append(test_info->test_case_name()) \ + .append(" of test ") \ + .append(test_info->name()) \ + .append(".\n") \ + .c_str()); \ + } // C++ stream interface class TestCout : public std::stringstream { -public: - ~TestCout() override - { - PRINTF("%s\n", str().c_str()); - } + public: + ~TestCout() override + { + PRINTF("%s\n", str().c_str()); + } }; /* Usage : @@ -120,72 +124,104 @@ TEST(Test, Foo) */ #define TEST_COUT testing::internal::TestCout() -} // namespace internal +} // namespace internal /* Macros related to testing Eigen classes: */ -#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \ - return (lhs - rhs).isMuchSmallerThan(1, precision); \ - }, \ - C_expect, C_actual); - -#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \ - return (lhs - rhs).isMuchSmallerThan(1, precision); \ - }, \ - C_expect, C_actual); - -#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ - return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \ - }, \ - Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs()); - -#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ - return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \ - }, \ - Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs()); - -#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ - return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \ - }, \ - C_expect, C_actual); - -#define ASSERT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ - return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \ - }, \ - C_expect, C_actual); - -#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ - assert(lhs.size() == 3 and rhs.size() == 3); \ - Eigen::VectorXd er = lhs - rhs; \ - er(2) = pi2pi((double)er(2)); \ - return er.isMuchSmallerThan(1, precision); \ - }, \ - C_expect, C_actual); - -#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ - assert(lhs.size() == 3 and rhs.size() == 3); \ - Eigen::VectorXd er = lhs - rhs; \ - er(2) = pi2pi((double)er(2)); \ - return er.isMuchSmallerThan(1, precision); \ - }, \ - C_expect, C_actual); - -#define EXPECT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ - assert(lhs.size() == 7 and rhs.size() == 7); \ - Eigen::Vector4d er; \ - er.head(3) = lhs.head(3) - rhs.head(3); \ - er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \ - return er.isMuchSmallerThan(1, precision); \ - }, \ - C_expect, C_actual); - -#define ASSERT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ - assert(lhs.size() == 7 and rhs.size() == 7); \ - Eigen::Vector4d er; \ - er.head(3) = lhs.head(3) - rhs.head(3); \ - er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \ - return er.isMuchSmallerThan(1, precision); \ - }, \ - C_expect, C_actual); - -} // namespace testing +#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) \ + EXPECT_PRED2([](const Eigen::MatrixXd lhs, \ + const Eigen::MatrixXd rhs) { return (lhs - rhs).isMuchSmallerThan(1, precision); }, \ + C_expect, \ + C_actual); + +#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) \ + ASSERT_PRED2([](const Eigen::MatrixXd lhs, \ + const Eigen::MatrixXd rhs) { return (lhs - rhs).isMuchSmallerThan(1, precision); }, \ + C_expect, \ + C_actual); + +#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) \ + EXPECT_PRED2( \ + [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ + return Eigen::Quaterniond(Eigen::Vector4d(lhs)) \ + .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \ + }, \ + Quaterniond(C_expect).coeffs(), \ + Quaterniond(C_actual).coeffs()); + +#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) \ + ASSERT_PRED2( \ + [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ + return Eigen::Quaterniond(Eigen::Vector4d(lhs)) \ + .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \ + }, \ + Quaterniond(C_expect).coeffs(), \ + Quaterniond(C_actual).coeffs()); + +#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) \ + EXPECT_PRED2( \ + [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ + return Eigen::Quaterniond(Eigen::Vector4d(lhs)) \ + .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \ + }, \ + C_expect, \ + C_actual); + +#define ASSERT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) \ + ASSERT_PRED2( \ + [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ + return Eigen::Quaterniond(Eigen::Vector4d(lhs)) \ + .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \ + }, \ + C_expect, \ + C_actual); + +#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) \ + EXPECT_PRED2( \ + [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ + assert(lhs.size() == 3 and rhs.size() == 3); \ + Eigen::VectorXd er = lhs - rhs; \ + er(2) = pi2pi((double)er(2)); \ + return er.isMuchSmallerThan(1, precision); \ + }, \ + C_expect, \ + C_actual); + +#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) \ + ASSERT_PRED2( \ + [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ + assert(lhs.size() == 3 and rhs.size() == 3); \ + Eigen::VectorXd er = lhs - rhs; \ + er(2) = pi2pi((double)er(2)); \ + return er.isMuchSmallerThan(1, precision); \ + }, \ + C_expect, \ + C_actual); + +#define EXPECT_POSE3d_APPROX(C_expect, C_actual, precision) \ + EXPECT_PRED2( \ + [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ + assert(lhs.size() == 7 and rhs.size() == 7); \ + Eigen::Vector4d er; \ + er.head(3) = lhs.head(3) - rhs.head(3); \ + er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))) \ + .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \ + return er.isMuchSmallerThan(1, precision); \ + }, \ + C_expect, \ + C_actual); + +#define ASSERT_POSE3d_APPROX(C_expect, C_actual, precision) \ + EXPECT_PRED2( \ + [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \ + assert(lhs.size() == 7 and rhs.size() == 7); \ + Eigen::Vector4d er; \ + er.head(3) = lhs.head(3) - rhs.head(3); \ + er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))) \ + .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \ + return er.isMuchSmallerThan(1, precision); \ + }, \ + C_expect, \ + C_actual); + +} // namespace testing diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index a6e566b54921d449ac6b3f632b84de5fa86c140b..42af536adc8024fa3f556c8c9df67650350d60cf 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -21,54 +21,77 @@ #include "core/capture/capture_base.h" #include "core/sensor/sensor_base.h" -namespace wolf{ - +namespace wolf +{ using namespace Eigen; unsigned int CaptureBase::capture_id_count_ = 0; CaptureBase::CaptureBase(const std::string& _type, - const TimeStamp& _ts, - SensorBasePtr _sensor_ptr, - StateBlockPtr _p_ptr, - StateBlockPtr _o_ptr, - StateBlockPtr _intr_ptr) : - NodeStateBlocks("CAPTURE", _type), - frame_ptr_(), // nullptr - sensor_ptr_(_sensor_ptr), - capture_id_(++capture_id_count_), - time_stamp_(_ts) + const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, + StateBlockPtr _p_ptr, + StateBlockPtr _o_ptr, + StateBlockPtr _intr_ptr) + : NodeStateBlocks("CAPTURE", _type), + frame_ptr_(), // nullptr + sensor_ptr_(_sensor_ptr), + capture_id_(++capture_id_count_), + time_stamp_(_ts) { assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!"); if (_sensor_ptr) { if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P')) { - WOLF_ERROR_COND(not _p_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state P dynamic but provided _p_ptr is nullptr") + WOLF_ERROR_COND(not _p_ptr, + "In CaptureBase constructor of type ", + _type, + " the sensor ", + _sensor_ptr->getType(), + " ", + _sensor_ptr->getName(), + " has state P dynamic but provided _p_ptr is nullptr") assert(_p_ptr && "Pointer to dynamic position params is null!"); addStateBlock('P', _p_ptr); } else - assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist"); + assert(_p_ptr == nullptr && + "Provided dynamic sensor position but the sensor position is static or doesn't exist"); if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O')) { - WOLF_ERROR_COND(not _o_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state O dynamic but provided _o_ptr is nullptr") + WOLF_ERROR_COND(not _o_ptr, + "In CaptureBase constructor of type ", + _type, + " the sensor ", + _sensor_ptr->getType(), + " ", + _sensor_ptr->getName(), + " has state O dynamic but provided _o_ptr is nullptr") assert(_o_ptr && "Pointer to dynamic orientation params is null!"); addStateBlock('O', _o_ptr); } else - assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist"); + assert(_o_ptr == nullptr && + "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist"); if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I')) { - WOLF_ERROR_COND(not _intr_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state I dynamic but provided _i_ptr is nullptr") + WOLF_ERROR_COND(not _intr_ptr, + "In CaptureBase constructor of type ", + _type, + " the sensor ", + _sensor_ptr->getType(), + " ", + _sensor_ptr->getName(), + " has state I dynamic but provided _i_ptr is nullptr") assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!"); addStateBlock('I', _intr_ptr); } else - assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist"); - + assert(_intr_ptr == nullptr && + "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist"); } else if (_p_ptr || _o_ptr || _intr_ptr) { @@ -83,20 +106,18 @@ void CaptureBase::remove(bool viral_remove_parent_without_children) * In case multi-threading, solver can be called while removing. * This order avoids SolverManager to ignore notifications (efficiency) */ - if (is_removing_) - return; + if (is_removing_) return; - is_removing_ = true; + is_removing_ = true; CaptureBasePtr this_C = shared_from_this_capture(); // keep this alive while removing it // SensorBase::last_capture_ - if (getSensor() and getSensor()->getLastCapture() == this_C) - getSensor()->updateLastCapture(); + if (getSensor() and getSensor()->getLastCapture() == this_C) getSensor()->updateLastCapture(); // remove downstream while (not feature_list_.empty()) { - feature_list_.front()->remove(viral_remove_parent_without_children); // remove downstream + feature_list_.front()->remove(viral_remove_parent_without_children); // remove downstream } // remove from upstream @@ -106,7 +127,7 @@ void CaptureBase::remove(bool viral_remove_parent_without_children) F->removeCapture(this_C); if (viral_remove_parent_without_children and not F->hasChildren()) { - F->remove(viral_remove_parent_without_children); // remove upstream + F->remove(viral_remove_parent_without_children); // remove upstream } } @@ -121,7 +142,9 @@ bool CaptureBase::hasChildren() const bool CaptureBase::process() { - assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead."); + assert(getSensor() != nullptr && + "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call " + "sensor->process(capture) instead."); return getSensor()->process(shared_from_this_capture()); } @@ -153,22 +176,18 @@ FactorBasePtrList CaptureBase::getFactorList() void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const { for (auto f_ptr : getFeatureList()) - if (not f_ptr->isRemoving()) - f_ptr->getFactorList(_fac_list); + if (not f_ptr->isRemoving()) f_ptr->getFactorList(_fac_list); } void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) { for (auto f_ptr : getFeatureList()) - if (not f_ptr->isRemoving()) - f_ptr->getFactorList(_fac_list); + if (not f_ptr->isRemoving()) f_ptr->getFactorList(_fac_list); } -bool CaptureBase::hasFeature(const FeatureBaseConstPtr &_feature) const +bool CaptureBase::hasFeature(const FeatureBaseConstPtr& _feature) const { - return std::find(feature_list_.begin(), - feature_list_.end(), - _feature) != feature_list_.end(); + return std::find(feature_list_.begin(), feature_list_.end(), _feature) != feature_list_.end(); } StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const @@ -180,7 +199,7 @@ StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const else return getSensor()->getStateBlock(_key); } - else // No sensor associated, or sensor without this state block: assume sensor params are here + else // No sensor associated, or sensor without this state block: assume sensor params are here return NodeStateBlocks::getStateBlock(_key); } @@ -193,7 +212,7 @@ StateBlockPtr CaptureBase::getStateBlock(const char& _key) else return getSensor()->getStateBlock(_key); } - else // No sensor associated, or sensor without this state block: assume sensor params are here + else // No sensor associated, or sensor without this state block: assume sensor params are here return NodeStateBlocks::getStateBlock(_key); } @@ -209,9 +228,15 @@ void CaptureBase::unfix() void CaptureBase::move(FrameBasePtr _frm_ptr) { - WOLF_WARN_COND(this->getFrame() == nullptr, "Moving Capture ", id(), " at ts=", getTimeStamp(), " not linked to any frame. Consider just linking it with link() instead of move()!"); + WOLF_WARN_COND(this->getFrame() == nullptr, + "Moving Capture ", + id(), + " at ts=", + getTimeStamp(), + " not linked to any frame. Consider just linking it with link() instead of move()!"); - assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && "Forbidden: trying to move a capture already linked to a KF!"); + assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && + "Forbidden: trying to move a capture already linked to a KF!"); // Unlink unlink(); @@ -227,7 +252,7 @@ void CaptureBase::link(FrameBasePtr _frm_ptr) WOLF_WARN_COND(_frm_ptr == nullptr, "Linking Capture ", id(), " to a nullptr"); - if(_frm_ptr) + if (_frm_ptr) { _frm_ptr->addCapture(shared_from_this_capture()); this->setFrame(_frm_ptr); @@ -237,10 +262,10 @@ void CaptureBase::link(FrameBasePtr _frm_ptr) void CaptureBase::unlink() { - WOLF_WARN_COND(this->getFrame() == nullptr, "Unlinking a not linked Capture ", id(), ". Nothing to do, skipping..."); + WOLF_WARN_COND( + this->getFrame() == nullptr, "Unlinking a not linked Capture ", id(), ". Nothing to do, skipping..."); - if (not this->getFrame()) - return; + if (not this->getFrame()) return; assert(getFactorList().empty() && " unlinking a capture constrained by factors!"); @@ -251,8 +276,7 @@ void CaptureBase::unlink() void CaptureBase::setProblem(ProblemPtr _problem) { - if (_problem == nullptr || _problem == this->getProblem()) - return; + if (_problem == nullptr || _problem == this->getProblem()) return; assert(getFrame() && "Cannot set problem: Capture has no Frame!"); @@ -260,21 +284,22 @@ void CaptureBase::setProblem(ProblemPtr _problem) // update SensorBase::last_capture_ if (getSensor() and - (not getSensor()->getLastCapture() or - getSensor()->getLastCapture()->getTimeStamp() < time_stamp_)) - getSensor()->setLastCapture(shared_from_this_capture()); + (not getSensor()->getLastCapture() or getSensor()->getLastCapture()->getTimeStamp() < time_stamp_)) + getSensor()->setLastCapture(shared_from_this_capture()); - for (auto ft : feature_list_) - ft->setProblem(_problem); + for (auto ft : feature_list_) ft->setProblem(_problem); } -void CaptureBase::printHeader(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const +void CaptureBase::printHeader(int _depth, + bool _factored_by, + bool _metric, + bool _state_blocks, + std::ostream& _stream, + std::string _tabs) const { - _stream << _tabs << "Cap" << id() - << " " << getType() - << " ts = " << std::setprecision(3) << getTimeStamp(); + _stream << _tabs << "Cap" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp(); - if(getSensor() != nullptr) + if (getSensor() != nullptr) { _stream << " -> Sen" << getSensor()->id(); } @@ -286,85 +311,88 @@ void CaptureBase::printHeader(int _depth, bool _factored_by, bool _metric, bool { _stream << " <-- "; for (auto fac : getFactoredBySet()) - if (fac) - _stream << "Fac" << fac->id() << " \t"; + if (fac) _stream << "Fac" << fac->id() << " \t"; } _stream << std::endl; - if (getStateBlockMap().size() > 0) - printState(_factored_by, _metric, _state_blocks, _stream, _tabs); + if (getStateBlockMap().size() > 0) printState(_factored_by, _metric, _state_blocks, _stream, _tabs); } -void CaptureBase::print(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const +void CaptureBase::print(int _depth, + bool _factored_by, + bool _metric, + bool _state_blocks, + std::ostream& _stream, + std::string _tabs) const { printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs); if (_depth >= 3) for (auto f : getFeatureList()) - if (f) - f->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + " "); + if (f) f->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + " "); } CheckLog CaptureBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const { - CheckLog log; + CheckLog log; std::stringstream inconsistency_explanation; if (_verbose) { _stream << _tabs << "Cap" << id() << " @ " << this << " -> Sen"; - if (getSensor()) _stream << getSensor()->id(); - else _stream << "-"; + if (getSensor()) + _stream << getSensor()->id(); + else + _stream << "-"; _stream << std::endl; - _stream << _tabs << " " << "-> Prb @ " << getProblem() << std::endl; - _stream << _tabs << " " << "-> Frm" << getFrame()->id() << " @ " << getFrame() << std::endl; + _stream << _tabs << " " + << "-> Prb @ " << getProblem() << std::endl; + _stream << _tabs << " " + << "-> Frm" << getFrame()->id() << " @ " << getFrame() << std::endl; } // check problem and frame pointers auto frm_ptr = getFrame(); - inconsistency_explanation << "Capture problem pointer " << getProblem() - << " different from frame problem pointer " << frm_ptr->getProblem() << "\n"; + inconsistency_explanation << "Capture problem pointer " << getProblem() << " different from frame problem pointer " + << frm_ptr->getProblem() << "\n"; log.assertTrue((getProblem() == frm_ptr->getProblem()), inconsistency_explanation); // check frame - inconsistency_explanation << "Cap" << id() << " @ " << this - << " ---> Frm" << frm_ptr->id() << " @ " << frm_ptr - << " -X-> Frm" << id() << "\n"; - auto frm_cap_list = frm_ptr->getCaptureList(); + inconsistency_explanation << "Cap" << id() << " @ " << this << " ---> Frm" << frm_ptr->id() << " @ " << frm_ptr + << " -X-> Frm" << id() << "\n"; + auto frm_cap_list = frm_ptr->getCaptureList(); auto frame_has_cap = std::find(frm_cap_list.begin(), frm_cap_list.end(), shared_from_this_capture()); log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation); - // check features - for(auto f : getFeatureList()) + for (auto f : getFeatureList()) { - inconsistency_explanation << "Cap " << id() << " @ " << this - << " ---> Ftr" << f->id() << " @ " << f - << " -X-> Cap" << id(); + inconsistency_explanation << "Cap " << id() << " @ " << this << " ---> Ftr" << f->id() << " @ " << f + << " -X-> Cap" << id(); log.assertTrue((f->getCapture() == shared_from_this_capture()), inconsistency_explanation); } - //Check that the capture is within time tolerance of some processor - auto frame = getFrame(); + // Check that the capture is within time tolerance of some processor + auto frame = getFrame(); double time_diff = fabs(getTimeStamp() - frame->getTimeStamp()); - //It looks like some gtests add captures by hand, without using processors, so the processor list is empty. - //This initialization is needed because if the list empty the execution does not go into the loop and the - //assertion fails - bool match_any_prc_timetolerance=false; - if(getSensor() != nullptr ) + // It looks like some gtests add captures by hand, without using processors, so the processor list is empty. + // This initialization is needed because if the list empty the execution does not go into the loop and the + // assertion fails + bool match_any_prc_timetolerance = false; + if (getSensor() != nullptr) { match_any_prc_timetolerance = getSensor()->getProcessorList().empty(); - for(auto prc : getSensor()->getProcessorList()) + for (auto prc : getSensor()->getProcessorList()) { match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance()); } - inconsistency_explanation << "Cap " << id() << " @ " << this - << " ts =" << getTimeStamp() << "Frm" << frame->id() - << " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of" - << " any processor in sensor " << getSensor()->id() << "\n"; + inconsistency_explanation << "Cap " << id() << " @ " << this << " ts =" << getTimeStamp() << "Frm" + << frame->id() << " ts = " << frame->getTimeStamp() << " their time difference (" + << time_diff << ") does not match any time tolerance of" + << " any processor in sensor " << getSensor()->id() << "\n"; log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation); } - + // check NodeStateBlocks auto node_log = checkStatesAndFactoredBy(_verbose, _stream, _tabs); log.compose(node_log); @@ -377,7 +405,7 @@ bool CaptureBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, st auto local_log = localCheck(_verbose, _stream, _tabs); _log.compose(local_log); - for(auto f : getFeatureList()) f->check(_log, _verbose, _stream, _tabs + " "); + for (auto f : getFeatureList()) f->check(_log, _verbose, _stream, _tabs + " "); return _log.is_consistent_; } -} // namespace wolf +} // namespace wolf diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp index b6c628d33bccb95cf87948c45480ae2e1603e945..30dd73d05521f7526d9bb919cd7419573cdfc07c 100644 --- a/src/capture/capture_diff_drive.cpp +++ b/src/capture/capture_diff_drive.cpp @@ -21,27 +21,27 @@ #include "core/capture/capture_diff_drive.h" #include "core/math/rotations.h" -namespace wolf { - -CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts, - const SensorBasePtr& _sensor_ptr, +namespace wolf +{ +CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts, + const SensorBasePtr& _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr, - StateBlockPtr _p_ptr, - StateBlockPtr _o_ptr, - StateBlockPtr _intr_ptr) : - CaptureMotion("CaptureDiffDrive", - _ts, - _sensor_ptr, - _data, - _data_cov, - _capture_origin_ptr, - _p_ptr, - _o_ptr, - _intr_ptr) + CaptureBasePtr _capture_origin_ptr, + StateBlockPtr _p_ptr, + StateBlockPtr _o_ptr, + StateBlockPtr _intr_ptr) + : CaptureMotion("CaptureDiffDrive", + _ts, + _sensor_ptr, + _data, + _data_cov, + _capture_origin_ptr, + _p_ptr, + _o_ptr, + _intr_ptr) { // } -} // namespace wolf +} // namespace wolf diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp index a2394cf9f5136ea86a9eefe6b9958024419a5205..7ef37912443ac717072f6240146a93b7846e7a7b 100644 --- a/src/capture/capture_landmarks_external.cpp +++ b/src/capture/capture_landmarks_external.cpp @@ -20,33 +20,36 @@ #include "core/capture/capture_landmarks_external.h" -namespace wolf{ - -CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts, - SensorBasePtr _sensor_ptr, - const std::vector<int>& _ids, - const std::vector<Eigen::VectorXd>& _detections, - const std::vector<Eigen::MatrixXd>& _covs, - const std::vector<double>& _qualities) : - CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr) +namespace wolf +{ +CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, + const std::vector<int>& _ids, + const std::vector<Eigen::VectorXd>& _detections, + const std::vector<Eigen::MatrixXd>& _covs, + const std::vector<double>& _qualities) + : CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr) { - if (_ids.size() != _detections.size() or - _ids.size() != _covs.size() or - _ids.size() != _qualities.size()) - throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size."); - + if (_ids.size() != _detections.size() or _ids.size() != _covs.size() or _ids.size() != _qualities.size()) + throw std::runtime_error( + "CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same " + "size."); + for (auto i = 0; i < _detections.size(); i++) addDetection(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i)); } CaptureLandmarksExternal::~CaptureLandmarksExternal() { - // + // } -void CaptureLandmarksExternal::addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality) +void CaptureLandmarksExternal::addDetection(const int& _id, + const Eigen::VectorXd& _detection, + const Eigen::MatrixXd& _cov, + const double& _quality) { detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality}); } -} // namespace wolf +} // namespace wolf diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index e6a35dbe303c0bb03ca4d908da0ed998abc3f0cd..9ba0776f367190e8911438097989e75bf931e02a 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -22,35 +22,37 @@ namespace wolf { - -CaptureMotion::CaptureMotion(const std::string & _type, - const TimeStamp& _ts, - SensorBasePtr _sensor_ptr, +CaptureMotion::CaptureMotion(const std::string& _type, + const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, - CaptureBasePtr _capture_origin_ptr) : - CaptureBase(_type, _ts, _sensor_ptr), - data_(_data), - data_cov_(_sensor_ptr ? _sensor_ptr->computeNoiseCov(_data) : Eigen::MatrixXd::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly - buffer_(), - capture_origin_ptr_(_capture_origin_ptr) + CaptureBasePtr _capture_origin_ptr) + : CaptureBase(_type, _ts, _sensor_ptr), + data_(_data), + data_cov_(_sensor_ptr ? _sensor_ptr->computeNoiseCov(_data) + : Eigen::MatrixXd::Zero( + _data.rows(), + _data.rows())), // Someone should test this zero and do something smart accordingly + buffer_(), + capture_origin_ptr_(_capture_origin_ptr) { // } -CaptureMotion::CaptureMotion(const std::string & _type, - const TimeStamp& _ts, - SensorBasePtr _sensor_ptr, +CaptureMotion::CaptureMotion(const std::string& _type, + const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr, - StateBlockPtr _p_ptr , - StateBlockPtr _o_ptr , - StateBlockPtr _intr_ptr ) : - CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr), - data_(_data), - data_cov_(_data_cov), - buffer_(), - capture_origin_ptr_(_capture_origin_ptr) + CaptureBasePtr _capture_origin_ptr, + StateBlockPtr _p_ptr, + StateBlockPtr _o_ptr, + StateBlockPtr _intr_ptr) + : CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr), + data_(_data), + data_cov_(_data_cov), + buffer_(), + capture_origin_ptr_(_capture_origin_ptr) { // } @@ -60,17 +62,15 @@ CaptureMotion::~CaptureMotion() // } -bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance) const +bool CaptureMotion::containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const { assert(_ts.ok() and this->time_stamp_.ok()); // the same capture is within tolerance - if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance) - return true; + if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance) return true; // buffer is empty, and the capture is not within tolerance - if (this->getBuffer().empty()) - return false; + if (this->getBuffer().empty()) return false; // buffer encloses timestamp, if ts is: // from : buffer.first.ts - tt @@ -83,16 +83,16 @@ bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolera return false; } - - - -void CaptureMotion::printHeader(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const +void CaptureMotion::printHeader(int _depth, + bool _factored_by, + bool _metric, + bool _state_blocks, + std::ostream& _stream, + std::string _tabs) const { - _stream << _tabs << "CapM" << id() - << " " << getType() - << " ts = " << std::setprecision(3) << getTimeStamp(); + _stream << _tabs << "CapM" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp(); - if(getSensor() != nullptr) + if (getSensor() != nullptr) { _stream << " -> Sen" << getSensor()->id(); } @@ -102,8 +102,7 @@ void CaptureMotion::printHeader(int _depth, bool _factored_by, bool _metric, boo if (auto OC = getOriginCapture()) { _stream << " -> Origin: Cap" << OC->id(); - if (auto OF = OC->getFrame()) - _stream << " ; Frm" << OF->id(); + if (auto OF = OC->getFrame()) _stream << " ; Frm" << OF->id(); } _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : ""); @@ -111,29 +110,29 @@ void CaptureMotion::printHeader(int _depth, bool _factored_by, bool _metric, boo { _stream << " <-- "; for (auto fac : getFactoredBySet()) - if (fac) - _stream << "Fac" << fac->id() << " \t"; + if (fac) _stream << "Fac" << fac->id() << " \t"; } _stream << std::endl; - if (getStateBlockMap().size() > 0) - printState(_factored_by, _metric, _state_blocks, _stream, _tabs); + if (getStateBlockMap().size() > 0) printState(_factored_by, _metric, _state_blocks, _stream, _tabs); - _stream << _tabs << " " << "buffer size : " << getBuffer().size(); + _stream << _tabs << " " + << "buffer size : " << getBuffer().size(); if (getBuffer().size() > 0) _stream << " ; nbr of data samples : " << getBuffer().size() - 1; _stream << std::endl; - if ( _metric && ! getBuffer().empty()) + if (_metric && !getBuffer().empty()) { - _stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl; -// if (hasCalibration()) -// { -// _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl; -// _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl; -// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl; -// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl; -// } + _stream << _tabs << " " + << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl; + // if (hasCalibration()) + // { + // _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << + // std::endl; _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << + // std::endl; _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" + // << std::endl; _stream << _tabs << " " << "delta correct: (" << + // getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl; + // } } - -} } +} // namespace wolf diff --git a/src/capture/capture_odom_2d.cpp b/src/capture/capture_odom_2d.cpp index c9d3b6550078199b34d92be1dc386dd31d4fa145..de755b1b5c7efec7c5ec2772b0ae87926722b5ee 100644 --- a/src/capture/capture_odom_2d.cpp +++ b/src/capture/capture_odom_2d.cpp @@ -22,22 +22,21 @@ namespace wolf { - -CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, +CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, - CaptureBasePtr _capture_origin_ptr): - CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr) + CaptureBasePtr _capture_origin_ptr) + : CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr) { // } -CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, +CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr): - CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr) + CaptureBasePtr _capture_origin_ptr) + : CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr) { // } diff --git a/src/capture/capture_odom_3d.cpp b/src/capture/capture_odom_3d.cpp index f359b7e96da9eda89b98910141045c7197c3883f..e68ed98376dd5ec2e3cee26f64ce7576650c0830 100644 --- a/src/capture/capture_odom_3d.cpp +++ b/src/capture/capture_odom_3d.cpp @@ -22,22 +22,21 @@ namespace wolf { - -CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, +CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, - CaptureBasePtr _capture_origin_ptr): - CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr) + CaptureBasePtr _capture_origin_ptr) + : CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr) { // } -CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, +CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr): - CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr) + CaptureBasePtr _capture_origin_ptr) + : CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr) { // } @@ -48,4 +47,3 @@ CaptureOdom3d::~CaptureOdom3d() } } /* namespace wolf */ - diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp index 5910874bb92f8ead6504835a160051e90a3cee74..ce8a4d543c9f6fd5ac0b1036f719b40503028ec8 100644 --- a/src/capture/capture_pose.cpp +++ b/src/capture/capture_pose.cpp @@ -22,7 +22,6 @@ namespace wolf { - CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, diff --git a/src/capture/capture_void.cpp b/src/capture/capture_void.cpp index 2c960af33c8e8b818b40d024511b23d88d9e4559..a18c8db422e9ed20f37e75e2d445c43bf94c859f 100644 --- a/src/capture/capture_void.cpp +++ b/src/capture/capture_void.cpp @@ -20,17 +20,17 @@ #include "core/capture/capture_void.h" -namespace wolf { - -CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr) : - CaptureBase("CaptureVoid", _ts, _sensor_ptr) +namespace wolf +{ +CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr) + : CaptureBase("CaptureVoid", _ts, _sensor_ptr) { // } CaptureVoid::~CaptureVoid() { - //std::cout << "deleting CaptureVoid " << id() << std::endl; + // std::cout << "deleting CaptureVoid " << id() << std::endl; } -} // namespace wolf +} // namespace wolf diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp index 9143b059f6c737e6efdc16f10d888e9eba2073f0..eb1631397088389010aeb15b41a9513d2aafeecf 100644 --- a/src/ceres_wrapper/local_parametrization_wrapper.cpp +++ b/src/ceres_wrapper/local_parametrization_wrapper.cpp @@ -20,22 +20,21 @@ #include "core/ceres_wrapper/local_parametrization_wrapper.h" -namespace wolf { - +namespace wolf +{ bool LocalParametrizationWrapper::Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const { Eigen::Map<const Eigen::VectorXd> x_raw_map((double*)x_raw, GlobalSize()); Eigen::Map<const Eigen::VectorXd> delta_raw_map((double*)delta_raw, LocalSize()); - Eigen::Map<Eigen::VectorXd> x_plus_map((double*)x_plus_delta_raw, GlobalSize()); + Eigen::Map<Eigen::VectorXd> x_plus_map((double*)x_plus_delta_raw, GlobalSize()); return local_parametrization_ptr_->plus(x_raw_map, delta_raw_map, x_plus_map); }; bool LocalParametrizationWrapper::ComputeJacobian(const double* x, double* jacobian) const { - Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize()); - Eigen::Map<Eigen::MatrixRowXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize()); + Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize()); + Eigen::Map<Eigen::MatrixRowXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize()); return local_parametrization_ptr_->computeJacobian(x_map, jacobian_map); }; -} // namespace wolf - +} // namespace wolf diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp index 984af4d6e55c47648cf63129e99e199a66f9bcf5..1862081067ac1b879ec011182f25578b0a9bca7b 100644 --- a/src/ceres_wrapper/qr_manager.cpp +++ b/src/ceres_wrapper/qr_manager.cpp @@ -20,16 +20,16 @@ #include "qr_manager.h" -namespace wolf { - -QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) : - SolverManager(_wolf_problem), - A_(), // empty matrix - b_(), - any_state_block_removed_(false), - new_state_blocks_(0), - N_batch_(_N_batch), - pending_changes_(false) +namespace wolf +{ +QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) + : SolverManager(_wolf_problem), + A_(), // empty matrix + b_(), + any_state_block_removed_(false), + new_state_blocks_(0), + N_batch_(_N_batch), + pending_changes_(false) { // } @@ -46,8 +46,7 @@ std::string QRManager::solve(const unsigned int& _report_level) update(); // Decomposition - if (!computeDecomposition()) - return std::string("decomposition failed\n"); + if (!computeDecomposition()) return std::string("decomposition failed\n"); // Solve Eigen::VectorXd x_incr = solver_.solve(-b_); @@ -55,7 +54,8 @@ std::string QRManager::solve(const unsigned int& _report_level) // update state blocks for (auto sb_pair : sb_2_col_) - sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false); + sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), + false); if (_report_level == 1) return std::string("Success!\n"); @@ -72,34 +72,40 @@ void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) void QRManager::computeCovariances(const StateBlockPtrList& _sb_list) { - //std::cout << "computing covariances.." << std::endl; + // std::cout << "computing covariances.." << std::endl; update(); computeDecomposition(); -// std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl; -// std::cout << Eigen::MatrixXd(solver_.matrixR()) << std::endl; + // std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl; + // std::cout << Eigen::MatrixXd(solver_.matrixR()) << std::endl; covariance_solver_.compute(solver_.matrixR().topRows(solver_.matrixR().cols())); - Eigen::SparseMatrix<double, Eigen::ColMajor> I(A_.cols(),A_.cols()); + Eigen::SparseMatrix<double, Eigen::ColMajor> I(A_.cols(), A_.cols()); I.setIdentity(); Eigen::SparseMatrix<double, Eigen::ColMajor> iR = covariance_solver_.solve(I); - Eigen::MatrixXd Sigma_full = solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose(); + Eigen::MatrixXd Sigma_full = + solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose(); -// std::cout << "A' A = \n" << Eigen::MatrixXd(A_.transpose() * A_)<< std::endl; -// std::cout << "P iR iR' P' = \n" << Eigen::MatrixXd(P * iR * iR.transpose() * P.transpose()) << std::endl; -// std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXd(Sigma_full * A_.transpose() * A_) << std::endl; -// std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl; -// std::cout << "Sigma = \n" << Sigma_full << std::endl; + // std::cout << "A' A = \n" << Eigen::MatrixXd(A_.transpose() * A_)<< std::endl; + // std::cout << "P iR iR' P' = \n" << Eigen::MatrixXd(P * iR * iR.transpose() * P.transpose()) << std::endl; + // std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXd(Sigma_full * A_.transpose() * A_) << std::endl; + // std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl; + // std::cout << "Sigma = \n" << Sigma_full << std::endl; // STORE DESIRED COVARIANCES for (auto sb_row = _sb_list.begin(); sb_row != _sb_list.end(); sb_row++) - for (auto sb_col = sb_row; sb_col!=_sb_list.end(); sb_col++) + for (auto sb_col = sb_row; sb_col != _sb_list.end(); sb_col++) { - //std::cout << "cov state block " << sb_col->get() << std::endl; + // std::cout << "cov state block " << sb_col->get() << std::endl; assert(sb_2_col_.find(*sb_col) != sb_2_col_.end() && "state block not found"); - //std::cout << "block: " << sb_2_col_[*sb_row] << "," << sb_2_col_[*sb_col] << std::endl << Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()) << std::endl; - wolf_problem_->addCovarianceBlock(*sb_row, *sb_col, Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize())); + // std::cout << "block: " << sb_2_col_[*sb_row] << "," << sb_2_col_[*sb_col] << std::endl << + // Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()) << + // std::endl; + wolf_problem_->addCovarianceBlock( + *sb_row, + *sb_col, + Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize())); } } @@ -126,24 +132,22 @@ bool QRManager::computeDecomposition() } // resize and setZero A, b - A_.resize(meas_size,state_size); + A_.resize(meas_size, state_size); b_.resize(meas_size); } if (any_state_block_removed_ || new_state_blocks_ >= N_batch_) { // relinearize all factors - for (auto fac_pair : fac_2_row_) - relinearizeFactor(fac_pair.first); + for (auto fac_pair : fac_2_row_) relinearizeFactor(fac_pair.first); any_state_block_removed_ = false; - new_state_blocks_ = 0; + new_state_blocks_ = 0; } // Decomposition solver_.compute(A_); - if (solver_.info() != Eigen::Success) - return false; + if (solver_.info() != Eigen::Success) return false; } pending_changes_ = false; @@ -153,7 +157,7 @@ bool QRManager::computeDecomposition() void QRManager::addFactor(FactorBasePtr _fac_ptr) { - //std::cout << "add factor " << _fac_ptr->id() << std::endl; + // std::cout << "add factor " << _fac_ptr->id() << std::endl; assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor"); fac_2_row_[_fac_ptr] = A_.rows(); A_.conservativeResize(A_.rows() + _fac_ptr->getSize(), A_.cols()); @@ -169,7 +173,7 @@ void QRManager::addFactor(FactorBasePtr _fac_ptr) void QRManager::removeFactor(FactorBasePtr _fac_ptr) { - //std::cout << "remove factor " << _fac_ptr->id() << std::endl; + // std::cout << "remove factor " << _fac_ptr->id() << std::endl; assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor"); eraseBlockRow(A_, fac_2_row_[_fac_ptr], _fac_ptr->getSize()); b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()).setZero(); @@ -179,7 +183,7 @@ void QRManager::removeFactor(FactorBasePtr _fac_ptr) void QRManager::addStateBlock(StateBlockPtr _st_ptr) { - //std::cout << "add state block " << _st_ptr.get() << std::endl; + // std::cout << "add state block " << _st_ptr.get() << std::endl; assert(sb_2_col_.find(_st_ptr) == sb_2_col_.end() && "adding existing state block"); sb_2_col_[_st_ptr] = A_.cols(); A_.conservativeResize(A_.rows(), A_.cols() + _st_ptr->getSize()); @@ -190,7 +194,7 @@ void QRManager::addStateBlock(StateBlockPtr _st_ptr) void QRManager::removeStateBlock(StateBlockPtr _st_ptr) { - //std::cout << "remove state block " << _st_ptr.get() << std::endl; + // std::cout << "remove state block " << _st_ptr.get() << std::endl; assert(sb_2_col_.find(_st_ptr) != sb_2_col_.end() && "removing unknown state block"); eraseBlockCol(A_, sb_2_col_[_st_ptr], _st_ptr->getSize()); @@ -204,34 +208,33 @@ void QRManager::removeStateBlock(StateBlockPtr _st_ptr) void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr) { - //std::cout << "update state block " << _st_ptr.get() << std::endl; + // std::cout << "update state block " << _st_ptr.get() << std::endl; if (_st_ptr->isFixed()) { - if (sb_2_col_.find(_st_ptr) != sb_2_col_.end()) - removeStateBlock(_st_ptr); + if (sb_2_col_.find(_st_ptr) != sb_2_col_.end()) removeStateBlock(_st_ptr); } - else - if (sb_2_col_.find(_st_ptr) == sb_2_col_.end()) - addStateBlock(_st_ptr); + else if (sb_2_col_.find(_st_ptr) == sb_2_col_.end()) + addStateBlock(_st_ptr); } void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr) { // evaluate factor std::vector<const double*> fac_states_ptr; - for (auto sb : _fac_ptr->getStateBlockPtrVector()) - fac_states_ptr.push_back(sb->get()); - Eigen::VectorXd residual(_fac_ptr->getSize()); + for (auto sb : _fac_ptr->getStateBlockPtrVector()) fac_states_ptr.push_back(sb->get()); + Eigen::VectorXd residual(_fac_ptr->getSize()); std::vector<Eigen::MatrixXd> jacobians; - _fac_ptr->evaluate(fac_states_ptr,residual,jacobians); + _fac_ptr->evaluate(fac_states_ptr, residual, jacobians); // Fill jacobians Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols()); for (auto i = 0; i < jacobians.size(); i++) if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed()) { - assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added"); - assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols"); + assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && + "factor involving a state block not added"); + assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && + "bad A number of cols"); // insert since A_block_row has just been created so it's empty for sure insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]); } diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index c242d4911d5e8487cc102f480a8cea1f249b8f2d..3caa98e8fbdebaf5db1bdb19f7eed2914dcd5993 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -29,35 +29,32 @@ namespace wolf { - -SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) : - SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>()) +SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) : SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>()) { } -SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, - const ParamsCeresPtr& _params) - : SolverManager(_wolf_problem, _params) - , params_ceres_(_params) - , n_iter_acc_(0) - , n_iter_max_(0) - , n_convergence_(0) - , n_interrupted_(0) - , n_no_convergence_(0) +SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const ParamsCeresPtr& _params) + : SolverManager(_wolf_problem, _params), + params_ceres_(_params), + n_iter_acc_(0), + n_iter_max_(0), + n_convergence_(0), + n_interrupted_(0), + n_no_convergence_(0) { - covariance_ = std::make_unique<ceres::Covariance>(params_ceres_->covariance_options); + covariance_ = std::make_unique<ceres::Covariance>(params_ceres_->covariance_options); ceres_problem_ = std::make_unique<ceres::Problem>(params_ceres_->problem_options); if (params_ceres_->interrupt_on_problem_change) - getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_, - params_ceres_->min_num_iterations, - params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET)); + getSolverOptions().callbacks.push_back( + new IterationUpdateCallback(wolf_problem_, + params_ceres_->min_num_iterations, + params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET)); } SolverCeres::~SolverCeres() { - while (!fac_2_residual_idx_.empty()) - removeFactorDerived(fac_2_residual_idx_.begin()->first); + while (!fac_2_residual_idx_.empty()) removeFactorDerived(fac_2_residual_idx_.begin()->first); while (!state_blocks_.empty()) { @@ -79,7 +76,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level) std::string report; - //return report + // return report if (report_level == ReportVerbosity::BRIEF) report = summary_.BriefReport(); else if (report_level == ReportVerbosity::FULL) @@ -102,7 +99,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level) } bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks) -{ +{ // update problem update(); @@ -112,12 +109,11 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ switch (_blocks) { - case CovarianceBlocksToBeComputed::ALL: - { + case CovarianceBlocksToBeComputed::ALL: { // first create a vector containing all state blocks std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks; - //frame state blocks - for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap()) + // frame state blocks + for (auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap()) for (auto key : fr_pair.second->getKeys()) { const auto& sb = fr_pair.second->getStateBlock(key); @@ -125,32 +121,31 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ } // landmark state blocks - for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) + for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) for (auto key : l_ptr->getKeys()) { const auto& sb = l_ptr->getStateBlock(key); all_state_blocks.push_back(sb); } - + // double loop all against all (without repetitions) for (unsigned int i = 0; i < all_state_blocks.size(); i++) { assert(isStateBlockRegisteredDerived(all_state_blocks[i])); - for (unsigned int j = i; j < all_state_blocks.size(); j++) + for (unsigned int j = i; j < all_state_blocks.size(); j++) { assert(isStateBlockRegisteredDerived(all_state_blocks[i])); - state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]); + state_block_pairs.emplace_back(all_state_blocks[i], all_state_blocks[j]); double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]), getAssociatedMemBlockPtr(all_state_blocks[j])); } } break; } - case CovarianceBlocksToBeComputed::ALL_MARGINALS: - { + case CovarianceBlocksToBeComputed::ALL_MARGINALS: { // first create a vector containing all state blocks - for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap()) + for (auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap()) for (auto key1 : fr_pair.second->getKeys()) { const auto& sb1 = fr_pair.second->getStateBlock(key1); @@ -163,13 +158,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ state_block_pairs.emplace_back(sb1, sb2); double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2)); - if (sb1 == sb2) - break; + if (sb1 == sb2) break; } } // landmark state blocks - for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) + for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) for (auto key1 : l_ptr->getKeys()) { const auto& sb1 = l_ptr->getStateBlock(key1); @@ -182,16 +176,14 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ state_block_pairs.emplace_back(sb1, sb2); double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2)); - if (sb1 == sb2) - break; + if (sb1 == sb2) break; } } break; } - case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS: - { - //robot-robot + case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS: { + // robot-robot auto last_key_frame = wolf_problem_->getLastFrame(); assert(isStateBlockRegisteredDerived(last_key_frame->getP())); @@ -209,7 +201,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ getAssociatedMemBlockPtr(last_key_frame->getO())); // landmarks - for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) + for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) for (auto key : l_ptr->getKeys()) { const auto& sb = l_ptr->getStateBlock(key); @@ -231,66 +223,65 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ state_block_pairs.emplace_back(sb, sb2); double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2)); - if (sb == sb2) - break; + if (sb == sb2) break; } } - + break; } - case CovarianceBlocksToBeComputed::GAUSS: - { + case CovarianceBlocksToBeComputed::GAUSS: { // State blocks: // - Last KF: PV // last KF FrameBasePtr frame = wolf_problem_->getLastFrame(); - if (not frame) - return false; + if (not frame) return false; while (frame) { - if (frame->has('P') and - isStateBlockRegisteredDerived(frame->getStateBlock('P')) and - frame->has('V') and - isStateBlockRegisteredDerived(frame->getStateBlock('V'))) + if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and + frame->has('V') and isStateBlockRegisteredDerived(frame->getStateBlock('V'))) { break; } - //else - WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one..."); + // else + WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", + frame->id(), + " hasn't the state blocks or they are not registered. Trying with the previous one..."); frame = frame->getPreviousFrame(); } if (not frame) { - WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and V, returning..."); + WOLF_WARN( + "SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P " + "and V, returning..."); return false; } // only marginals of P and V - WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: " - "\nP @", frame->getStateBlock('P'), - "\nV @", frame->getStateBlock('V')); - state_block_pairs.emplace_back(frame->getStateBlock('P'), - frame->getStateBlock('P')); + WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", + frame->id(), + " with state blocks: " + "\nP @", + frame->getStateBlock('P'), + "\nV @", + frame->getStateBlock('V')); + state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P')); double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')), getAssociatedMemBlockPtr(frame->getStateBlock('P'))); - state_block_pairs.emplace_back(frame->getStateBlock('V'), - frame->getStateBlock('V')); + state_block_pairs.emplace_back(frame->getStateBlock('V'), frame->getStateBlock('V')); double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')), getAssociatedMemBlockPtr(frame->getStateBlock('V'))); break; } - case CovarianceBlocksToBeComputed::GAUSS_TUCAN: - { + case CovarianceBlocksToBeComputed::GAUSS_TUCAN: { // State blocks: // - Last KF: PV // last KF FrameBasePtr frame = wolf_problem_->getLastFrame(); - if (not frame) - return false; + if (not frame) return false; StateBlockPtr sb_gnss_T; while (frame) @@ -303,60 +294,60 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ break; } - if (frame->has('P') and - isStateBlockRegisteredDerived(frame->getStateBlock('P')) and - frame->has('O') and - isStateBlockRegisteredDerived(frame->getStateBlock('O')) and - frame->has('V') and - isStateBlockRegisteredDerived(frame->getStateBlock('V')) and - sb_gnss_T and + if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and + frame->has('O') and isStateBlockRegisteredDerived(frame->getStateBlock('O')) and + frame->has('V') and isStateBlockRegisteredDerived(frame->getStateBlock('V')) and sb_gnss_T and isStateBlockRegisteredDerived(sb_gnss_T)) { break; } // else - WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one..."); + WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", + frame->id(), + " hasn't the state blocks or they are not registered. Trying with the previous one..."); frame = frame->getPreviousFrame(); } if (not frame) { - WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P, O, V and CaptureGnss with T, returning..."); + WOLF_WARN( + "SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P, " + "O, V and CaptureGnss with T, returning..."); return false; } // only marginals of P, O, V and T - WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: " - "\nP @", frame->getStateBlock('P'), - "\nO @", frame->getStateBlock('O'), - "\nV @", frame->getStateBlock('V'), - "\nT @", sb_gnss_T); - state_block_pairs.emplace_back(frame->getStateBlock('P'), - frame->getStateBlock('P')); + WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", + frame->id(), + " with state blocks: " + "\nP @", + frame->getStateBlock('P'), + "\nO @", + frame->getStateBlock('O'), + "\nV @", + frame->getStateBlock('V'), + "\nT @", + sb_gnss_T); + state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P')); double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')), getAssociatedMemBlockPtr(frame->getStateBlock('P'))); - state_block_pairs.emplace_back(frame->getStateBlock('O'), - frame->getStateBlock('O')); + state_block_pairs.emplace_back(frame->getStateBlock('O'), frame->getStateBlock('O')); double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('O')), getAssociatedMemBlockPtr(frame->getStateBlock('O'))); - state_block_pairs.emplace_back(frame->getStateBlock('V'), - frame->getStateBlock('V')); + state_block_pairs.emplace_back(frame->getStateBlock('V'), frame->getStateBlock('V')); double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')), getAssociatedMemBlockPtr(frame->getStateBlock('V'))); state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T); - double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), - getAssociatedMemBlockPtr(sb_gnss_T)); + double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), getAssociatedMemBlockPtr(sb_gnss_T)); break; } - case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION: - { + case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION: { // State blocks: // - Last KF: PV // last KF FrameBasePtr frame = wolf_problem_->getLastFrame(); - if (not frame) - return false; + if (not frame) return false; StateBlockPtr sb_gnss_T; while (frame) @@ -369,42 +360,46 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ break; } - if (frame->has('P') and - isStateBlockRegisteredDerived(frame->getStateBlock('P')) and - sb_gnss_T and + if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and sb_gnss_T and isStateBlockRegisteredDerived(sb_gnss_T)) { break; } // else - WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one..."); + WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", + frame->id(), + " hasn't the state blocks or they are not registered. Trying with the previous one..."); frame = frame->getPreviousFrame(); } if (not frame) { - WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and CaptureGnss with T, returning..."); + WOLF_WARN( + "SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P " + "and CaptureGnss with T, returning..."); return false; } // only marginals of P and T - WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: " - "\nP @", frame->getStateBlock('P'), - "\nT @", sb_gnss_T); - state_block_pairs.emplace_back(frame->getStateBlock('P'), - frame->getStateBlock('P')); + WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", + frame->id(), + " with state blocks: " + "\nP @", + frame->getStateBlock('P'), + "\nT @", + sb_gnss_T); + state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P')); double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')), getAssociatedMemBlockPtr(frame->getStateBlock('P'))); state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T); - double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), - getAssociatedMemBlockPtr(sb_gnss_T)); + double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), getAssociatedMemBlockPtr(sb_gnss_T)); break; } default: - throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value"); - + throw std::runtime_error( + "SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value"); } - //std::cout << "pairs... " << double_pairs.size() << std::endl; + // std::cout << "pairs... " << double_pairs.size() << std::endl; // COMPUTE DESIRED COVARIANCES if (covariance_->Compute(double_pairs, ceres_problem_.get())) @@ -415,7 +410,8 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { - Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize()); + Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov( + state_block_pairs[i].first->getLocalSize(), state_block_pairs[i].second->getLocalSize()); covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data()); wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); // std::cout << "covariance got switch: " << std::endl << cov << std::endl; @@ -429,7 +425,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) { - //std::cout << "SolverCeres: computing covariances..." << std::endl; + // std::cout << "SolverCeres: computing covariances..." << std::endl; // update problem update(); @@ -441,25 +437,26 @@ bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st // double loop all against all (without repetitions) for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++) { - if (*st_it1 == nullptr){ + if (*st_it1 == nullptr) + { continue; } assert(isStateBlockRegisteredDerived(*st_it1)); for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++) { - if (*st_it2 == nullptr){ + if (*st_it2 == nullptr) + { continue; } assert(isStateBlockRegisteredDerived(*st_it2)); state_block_pairs.emplace_back(*st_it1, *st_it2); - double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)), - getAssociatedMemBlockPtr((*st_it2))); + double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)), getAssociatedMemBlockPtr((*st_it2))); } } - //std::cout << "pairs... " << double_pairs.size() << std::endl; + // std::cout << "pairs... " << double_pairs.size() << std::endl; // COMPUTE DESIRED COVARIANCES if (covariance_->Compute(double_pairs, ceres_problem_.get())) @@ -470,7 +467,8 @@ bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { - Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize()); + Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov( + state_block_pairs[i].first->getLocalSize(), state_block_pairs[i].second->getLocalSize()); covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data()); wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); // std::cout << "covariance got from st_list: " << std::endl << cov << std::endl; @@ -486,7 +484,7 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) { assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map"); - auto cost_func_ptr = createCostFunction(fac_ptr); + auto cost_func_ptr = createCostFunction(fac_ptr); fac_2_costfunction_[fac_ptr] = cost_func_ptr; std::vector<double*> res_block_mem; @@ -494,32 +492,37 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector()) { assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb"); - res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) ); + res_block_mem.emplace_back(getAssociatedMemBlockPtr(st)); } auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr; - //std::cout << "Added residual block corresponding to constraint " << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl; + // std::cout << "Added residual block corresponding to constraint " << + // std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() << + // std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl; - assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); - assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map"); + assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && + "ceres residuals different from wrapper residuals"); + assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && + "adding a factor that is already in the fac_2_residual_idx_ map"); - fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(), - loss_func_ptr, - res_block_mem); + fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(), loss_func_ptr, res_block_mem); - assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); + assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && + "ceres residuals different from wrapper residuals"); } void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr) { - assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map"); + assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && + "removing a factor that is not in the fac_2_residual map"); ceres_problem_->RemoveResidualBlock(fac_2_residual_idx_[_fac_ptr]); fac_2_residual_idx_.erase(_fac_ptr); fac_2_costfunction_.erase(_fac_ptr); - assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); + assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && + "ceres residuals different from wrapper residuals"); } void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr) @@ -532,25 +535,24 @@ void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr) if (state_ptr->hasLocalParametrization()) { - auto p = state_blocks_local_param_.emplace(state_ptr, - std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization())); + auto p = state_blocks_local_param_.emplace( + state_ptr, std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization())); local_parametrization_ptr = p.first->second.get(); } - ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr), - state_ptr->getSize(), - local_parametrization_ptr); + ceres_problem_->AddParameterBlock( + getAssociatedMemBlockPtr(state_ptr), state_ptr->getSize(), local_parametrization_ptr); - if (state_ptr->isFixed()) - ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr)); + if (state_ptr->isFixed()) ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr)); updateStateBlockStatusDerived(state_ptr); } void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr) { - //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl; + // std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) + // << std::endl; assert(state_ptr); assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres"); @@ -591,8 +593,7 @@ void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPt } // Remove all involved factors (it does not remove any parameter block) - for (auto fac : involved_factors) - removeFactorDerived(fac); + for (auto fac : involved_factors) removeFactorDerived(fac); // Remove state block (it removes all involved residual blocks but they just were removed) removeStateBlockDerived(state_ptr); @@ -601,8 +602,7 @@ void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPt addStateBlockDerived(state_ptr); // Add all involved factors - for (auto fac : involved_factors) - addFactorDerived(fac); + for (auto fac : involved_factors) addFactorDerived(fac); } bool SolverCeres::hasConverged() @@ -617,8 +617,7 @@ bool SolverCeres::wasStopped() unsigned int SolverCeres::iterations() { - if (summary_.num_successful_steps + summary_.num_unsuccessful_steps < 1) - return 0; + if (summary_.num_successful_steps + summary_.num_unsuccessful_steps < 1) return 0; return summary_.num_successful_steps + summary_.num_unsuccessful_steps; } @@ -650,7 +649,7 @@ ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac return createNumericDiffCostFunction(_fac_ptr); else - throw std::invalid_argument( "Wrong Jacobian Method!" ); + throw std::invalid_argument("Wrong Jacobian Method!"); } bool SolverCeres::checkDerived(std::string prefix) const @@ -662,17 +661,31 @@ bool SolverCeres::checkDerived(std::string prefix) const ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size()) { - WOLF_ERROR("SolverCeres::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size()); + WOLF_ERROR("SolverCeres::check: number of residuals mismatch - in ", + prefix, + ":\n\tceres_problem_->NumResidualBlocks(): ", + ceres_problem_->NumResidualBlocks(), + "\n\tfactors_.size(): ", + factors_.size(), + "\n\tfac_2_costfunction_.size(): ", + fac_2_costfunction_.size(), + "\n\tfac_2_residual_idx_.size(): ", + fac_2_residual_idx_.size()); ok = false; } if (ceres_problem_->NumParameterBlocks() != state_blocks_.size()) { - WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix); + WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", + ceres_problem_->NumParameterBlocks(), + " != state_blocks_.size(): ", + state_blocks_.size(), + ") - in ", + prefix); ok = false; } // Check parameter blocks - //for (auto&& state_block_pair : state_blocks_) + // for (auto&& state_block_pair : state_blocks_) for (const auto& state_block_pair : state_blocks_) { if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data())) @@ -688,25 +701,33 @@ bool SolverCeres::checkDerived(std::string prefix) const // SolverManager::factors_ if (factors_.count(fac_res_pair.first) == 0) { - WOLF_ERROR("SolverCeres::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix); + WOLF_ERROR("SolverCeres::check: factor ", + fac_res_pair.first->id(), + " (in fac_2_residual_idx_) not in factors_ - in ", + prefix); ok = false; } // costfunction - residual if (fac_2_costfunction_.count(fac_res_pair.first) == 0) { - WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix); + WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", + prefix); ok = false; } - //if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) - if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) + // if (fac_2_costfunction_[fac_res_pair.first].get() != + // ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) + if (fac_2_costfunction_.at(fac_res_pair.first).get() != + ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) { WOLF_ERROR("SolverCeres::check: fac_2_costfunction_ and ceres mismatch - in ", prefix); ok = false; } // factor - residual - if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor()) + if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>( + ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) + ->getFactor()) { WOLF_ERROR("SolverCeres::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix); ok = false; @@ -717,7 +738,12 @@ bool SolverCeres::checkDerived(std::string prefix) const ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks); if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size()) { - WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix); + WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual", + param_blocks.size(), + " different from number of state blocks in factor ", + fac_res_pair.first->getStateBlockPtrVector().size(), + " - in ", + prefix); ok = false; } else @@ -737,11 +763,14 @@ bool SolverCeres::checkDerived(std::string prefix) const return ok; } -void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col) +void insertSparseBlock(const Eigen::MatrixXd& ins, + Eigen::SparseMatrixd& original, + const unsigned int& row, + const unsigned int& col) { for (auto ins_row = 0; ins_row < ins.rows(); ins_row++) for (auto ins_col = 0; ins_col < ins.cols(); ins_col++) - original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col); + original.insert(row + ins_row, col + ins_col) = ins(ins_row, ins_col); original.makeCompressed(); } @@ -753,17 +782,19 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const // fac_2_residual_idx_ // fac_2_costfunction_ // state_blocks_local_param_ - int ix_row = 0; // index of the factor/measurement in the - for (auto fac_res_pair: fac_2_residual_idx_){ + int ix_row = 0; // index of the factor/measurement in the + for (auto fac_res_pair : fac_2_residual_idx_) + { FactorBasePtr fac_ptr = fac_res_pair.first; ix_row += fac_ptr->getSize(); // retrieve all stateblocks data related to this factor std::vector<const double*> fac_states_ptr; - for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){ + for (auto sb : fac_res_pair.first->getStateBlockPtrVector()) + { fac_states_ptr.push_back(sb->getStateData()); } - + // retrieve residual value, not used afterwards in this case since we just care about jacobians Eigen::VectorXd residual(fac_ptr->getSize()); // retrieve jacobian in group size, not local size @@ -781,39 +812,44 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols"); // insert since A_block_row has just been created so it's empty for sure - if (sb->hasLocalParametrization()){ - // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian - Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize()); - Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize()); + if (sb->hasLocalParametrization()) + { + // if the state block has a local parameterization, we need to right multiply by the manifold + // element / tangent element jacobian + Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize()); + Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map( + J_manif_tang.data(), sb->getSize(), sb->getLocalSize()); Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize()); sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map); - insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col) + insertSparseBlock(jacobians[i] * J_manif_tang, + A_block_row, + 0, + sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col) } - else { + else + { insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize()); } } } - + // fill the weighted jacobian matrix block row A.block(ix_row, 0, fac_ptr->getSize(), A.cols()); - } - // compute the hessian matrix from the weighted jacobian + // compute the hessian matrix from the weighted jacobian H = A.transpose() * A; return H; } -bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st, +bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) { - return state_blocks_local_param_.count(st) == 1 - && state_blocks_local_param_.at(st)->getLocalParametrization() == local_param - && ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) - == state_blocks_local_param_.at(st).get(); + return state_blocks_local_param_.count(st) == 1 && + state_blocks_local_param_.at(st)->getLocalParametrization() == local_param && + ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get(); } void SolverCeres::printProfilingDerived(std::ostream& _stream) const @@ -821,16 +857,13 @@ void SolverCeres::printProfilingDerived(std::ostream& _stream) const _stream << "Iterations:" << "\n\tUser-defined limit: " << params_ceres_->solver_options.max_num_iterations << "\n\tAverage iterations: " << n_iter_acc_ / n_solve_ - << "\n\tMax. iterations: " << n_iter_max_ - << "\nTermination:" + << "\n\tMax. iterations: " << n_iter_max_ << "\nTermination:" << "\n\tConvergence: " << 1e2 * n_convergence_ / n_solve_ << " %" << "\n\tInterrupted by problem change: " << 1e2 * n_interrupted_ / n_solve_ << " %" - << "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve_ << " %" - << std::endl; + << "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve_ << " %" << std::endl; } // register in the factory WOLF_REGISTER_SOLVER(SolverCeres) -} // namespace wolf - +} // namespace wolf diff --git a/src/common/node_base.cpp b/src/common/node_base.cpp index 5d9393c6125caf0444aee7498b74e0247496ddb3..b680bc94b2addeb98127691965319e82620e72c5 100644 --- a/src/common/node_base.cpp +++ b/src/common/node_base.cpp @@ -20,9 +20,9 @@ #include "core/common/node_base.h" -namespace wolf { - -//init static node counter +namespace wolf +{ +// init static node counter unsigned int NodeBase::node_id_count_ = 0; -} // namespace wolf +} // namespace wolf diff --git a/src/common/node_state_blocks.cpp b/src/common/node_state_blocks.cpp index 2b0ff729cea35a7684393694a53e89d3e4ea0d87..9ee3c37cf95b8d7e0dc58944b8141fa2de4a446d 100644 --- a/src/common/node_state_blocks.cpp +++ b/src/common/node_state_blocks.cpp @@ -144,11 +144,12 @@ StateBlockPtr NodeStateBlocks::addStateBlock(const char& _sb_key, const StateBlo } void NodeStateBlocks::emplaceFactorStateBlock(const char& _key, - const Eigen::VectorXd& _x, - const Eigen::MatrixXd& _cov, - unsigned int _start_idx) + const Eigen::VectorXd& _x, + const Eigen::MatrixXd& _cov, + unsigned int _start_idx) { - assert(isCovariance(_cov) and "NodeStateBlocks::emplaceFactorStateBlock: provided _cov is not a covariance matrix"); + assert(isCovariance(_cov) and + "NodeStateBlocks::emplaceFactorStateBlock: provided _cov is not a covariance matrix"); StateBlockPtr _sb = getStateBlock(_key); @@ -162,7 +163,8 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key, // not segment of state not allowed if (_start_idx != 0) throw std::runtime_error( - "NodeStateBlocks::emplaceFactorStateBlock: Prior for a segment of a state with local size different from " + "NodeStateBlocks::emplaceFactorStateBlock: Prior for a segment of a state with local size different " + "from " "size " "not allowed"); // state size @@ -182,7 +184,8 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key, // state size if (_x.size() + _start_idx > _sb->getSize()) throw std::runtime_error( - "NodeStateBlocks::emplaceFactorStateBlock: Prior for a segment of a state with local size different from " + "NodeStateBlocks::emplaceFactorStateBlock: Prior for a segment of a state with local size different " + "from " "size " "not allowed"); } @@ -190,8 +193,8 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key, // existing prior if (factor_prior_map_.count(_key)) throw std::runtime_error( - std::string("NodeStateBlocks::emplaceFactorStateBlock: There already is a factor prior in this key ") + _key + - ", factor " + toString(factor_prior_map_.at(_key)->id())); + std::string("NodeStateBlocks::emplaceFactorStateBlock: There already is a factor prior in this key ") + + _key + ", factor " + toString(factor_prior_map_.at(_key)->id())); // SET STATE if (_x.size() == _sb->getSize()) @@ -262,7 +265,8 @@ void NodeStateBlocks::emplacePriors(const SpecStateComposite& _specs) if (prior.isFixed()) state_block_composite_.at(key)->fix(); // emplace factor prior (if specified) - if (prior.isFactor()) emplaceFactorStateBlock(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal()); + if (prior.isFactor()) + emplaceFactorStateBlock(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal()); // set state (already set inside emplaceFactorStateBlock) else @@ -674,8 +678,8 @@ CheckLog NodeStateBlocks::checkStatesAndFactoredBy(bool _verbose, std::ostream& // check factor_prior_map_ does not have any factor not in factored_by for (auto fac_pri : getPriorFactorMap()) { - inconsistency_explanation << "Fac" << fac_pri.second->id() << " prior factor in " << getCategory() << id() << " @ " - << shared_from_this().get() << " not found in factored_by_set_\n"; + inconsistency_explanation << "Fac" << fac_pri.second->id() << " prior factor in " << getCategory() << id() + << " @ " << shared_from_this().get() << " not found in factored_by_set_\n"; log.assertTrue(getFactoredBySet().count(fac_pri.second), inconsistency_explanation); } return log; diff --git a/src/common/time_stamp.cpp b/src/common/time_stamp.cpp index 627e39310398e654454b7195ca65af24ea0f96ee..7756ac8a2768fe13fda51b5aab53caaaef2d7bb1 100644 --- a/src/common/time_stamp.cpp +++ b/src/common/time_stamp.cpp @@ -20,9 +20,9 @@ #include "core/common/time_stamp.h" -namespace wolf { - -TimeStamp TimeStamp::Now ( ) +namespace wolf +{ +TimeStamp TimeStamp::Now() { TimeStamp t(0); t.setToNow(); @@ -31,46 +31,41 @@ TimeStamp TimeStamp::Now ( ) std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts) { - if (!_ts.ok()) - os << "TimeStamp is invalid! "; - os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream + if (!_ts.ok()) os << "TimeStamp is invalid! "; + os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right + << _ts.getNanoSeconds(); // write obj to stream os << std::setfill(' '); return os; } -TimeStamp::TimeStamp() : - //time_stamp_(0) - time_stamp_nano_(0) - , - is_valid_(false) +TimeStamp::TimeStamp() + : // time_stamp_(0) + time_stamp_nano_(0), + is_valid_(false) { -// setToNow(); + // setToNow(); } -TimeStamp::TimeStamp(const TimeStamp& _ts) : - time_stamp_nano_(_ts.time_stamp_nano_), - is_valid_(_ts.is_valid_) +TimeStamp::TimeStamp(const TimeStamp& _ts) : time_stamp_nano_(_ts.time_stamp_nano_), is_valid_(_ts.is_valid_) { // } -TimeStamp::TimeStamp(const double& _ts) : - time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0), - is_valid_(_ts >= 0) +TimeStamp::TimeStamp(const double& _ts) + : time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts * 1e9) : 0), is_valid_(_ts >= 0) { // } -TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) : - time_stamp_nano_(_sec*NANOSECS+_nsec), - is_valid_(true) +TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) + : time_stamp_nano_(_sec * NANOSECS + _nsec), is_valid_(true) { // } TimeStamp::~TimeStamp() { - //nothing to do + // nothing to do } void TimeStamp::setToNow() @@ -81,30 +76,30 @@ void TimeStamp::setToNow() setOk(); } -TimeStamp TimeStamp::operator +(const double& dt) const +TimeStamp TimeStamp::operator+(const double& dt) const { TimeStamp ts(*this); ts += dt; return ts; } -TimeStamp TimeStamp::operator -(const double& dt) const +TimeStamp TimeStamp::operator-(const double& dt) const { TimeStamp ts(*this); ts -= dt; return ts; } -void TimeStamp::operator -=(const double& dt) +void TimeStamp::operator-=(const double& dt) { - unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS); - is_valid_ = (time_stamp_nano_ >= dt_nano); - time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano); + unsigned long int dt_nano = (unsigned long int)(dt * NANOSECS); + is_valid_ = (time_stamp_nano_ >= dt_nano); + time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano); } -void TimeStamp::print(std::ostream & ost) const +void TimeStamp::print(std::ostream& ost) const { ost << *this; } -} // namespace wolf +} // namespace wolf diff --git a/src/composite/matrix_composite.cpp b/src/composite/matrix_composite.cpp index fcd838c5199003a398e262658cb83f113071da28..adced770869c033e24c0e7681288ca8711d6cf05 100644 --- a/src/composite/matrix_composite.cpp +++ b/src/composite/matrix_composite.cpp @@ -20,9 +20,9 @@ #include "core/composite/matrix_composite.h" -namespace wolf{ - -bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk) +namespace wolf +{ +bool MatrixComposite::emplace(const char& _row, const char& _col, const Eigen::MatrixXd& _mat_blk) { // check rows if (size_rows_.count(_row) == 0) @@ -41,22 +41,20 @@ bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::M return true; } -bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_mat_blk) const +bool MatrixComposite::at(const char& _row, const char& _col, Eigen::MatrixXd& _mat_blk) const { auto row_it = this->find(_row); - if(row_it != this->end()) - return false; + if (row_it != this->end()) return false; auto col_it = row_it->second.find(_col); - if(col_it != row_it->second.end()) - return false; + if (col_it != row_it->second.end()) return false; _mat_blk = col_it->second; return true; } -Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) +Eigen::MatrixXd& MatrixComposite::at(const char& _row, const char& _col) { auto row_it = this->find(_row); assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite."); @@ -67,7 +65,7 @@ Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) return col_it->second; } -const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) const +const Eigen::MatrixXd& MatrixComposite::at(const char& _row, const char& _col) const { auto row_it = this->find(_row); assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite."); @@ -78,24 +76,24 @@ const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) c return col_it->second; } -wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) const +wolf::MatrixComposite MatrixComposite::operator*(const MatrixComposite& _N) const { MatrixComposite MN; - for (const auto &pair_i_Mi : (*this)) + for (const auto& pair_i_Mi : (*this)) { - const auto &i = pair_i_Mi.first; - const auto &Mi = pair_i_Mi.second; + const auto& i = pair_i_Mi.first; + const auto& Mi = pair_i_Mi.second; - for (const auto &pair_k_Nk : _N) + for (const auto& pair_k_Nk : _N) { - const auto &k = pair_k_Nk.first; - const auto &Nk = pair_k_Nk.second; + const auto& k = pair_k_Nk.first; + const auto& Nk = pair_k_Nk.second; - for (const auto &pair_j_Nkj : Nk) + for (const auto& pair_j_Nkj : Nk) { - const auto &j = pair_j_Nkj.first; - const auto &Nkj = pair_j_Nkj.second; - const auto &Mik = Mi.at(k); + const auto& j = pair_j_Nkj.first; + const auto& Nkj = pair_j_Nkj.second; + const auto& Mik = Mi.at(k); if (MN.count(i, j) == 0) MN.emplace(i, j, Mik * Nkj); @@ -107,19 +105,19 @@ wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) con return MN; } -wolf::MatrixComposite MatrixComposite::operator +(const MatrixComposite &_N) const +wolf::MatrixComposite MatrixComposite::operator+(const MatrixComposite& _N) const { MatrixComposite MpN; - for (const auto &pair_i_Mi : *this) + for (const auto& pair_i_Mi : *this) { - const auto &i = pair_i_Mi.first; - const auto &Mi = pair_i_Mi.second; + const auto& i = pair_i_Mi.first; + const auto& Mi = pair_i_Mi.second; for (const auto& pair_j_Mij : Mi) { - const auto& j = pair_j_Mij.first; + const auto& j = pair_j_Mij.first; const auto& Mij = pair_j_Mij.second; - const auto& Nij = _N.at(i,j); + const auto& Nij = _N.at(i, j); MpN.emplace(i, j, Mij + Nij); } @@ -127,19 +125,19 @@ wolf::MatrixComposite MatrixComposite::operator +(const MatrixComposite &_N) con return MpN; } -wolf::MatrixComposite MatrixComposite::operator -(const MatrixComposite &_N) const +wolf::MatrixComposite MatrixComposite::operator-(const MatrixComposite& _N) const { MatrixComposite MpN; - for (const auto &pair_i_Mi : *this) + for (const auto& pair_i_Mi : *this) { - const auto &i = pair_i_Mi.first; - const auto &Mi = pair_i_Mi.second; + const auto& i = pair_i_Mi.first; + const auto& Mi = pair_i_Mi.second; for (const auto& pair_j_Mij : Mi) { - const auto& j = pair_j_Mij.first; + const auto& j = pair_j_Mij.first; const auto& Mij = pair_j_Mij.second; - const auto& Nij = _N.at(i,j); + const auto& Nij = _N.at(i, j); MpN.emplace(i, j, Mij - Nij); } @@ -147,34 +145,34 @@ wolf::MatrixComposite MatrixComposite::operator -(const MatrixComposite &_N) con return MpN; } -MatrixComposite MatrixComposite::operator - (void) const +MatrixComposite MatrixComposite::operator-(void) const { MatrixComposite m; for (const auto& pair_rkey_row : *this) { - m.unordered_map<char,unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<char, MatrixXd>()); + m.unordered_map<char, unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first, + unordered_map<char, MatrixXd>()); for (const auto& pair_ckey_blk : pair_rkey_row.second) { - m[pair_rkey_row.first].emplace(pair_ckey_blk.first, - pair_ckey_blk.second); + m[pair_rkey_row.first].emplace(pair_ckey_blk.first, -pair_ckey_blk.second); } } return m; } - -wolf::VectorComposite MatrixComposite::operator *(const VectorComposite &_x) const +wolf::VectorComposite MatrixComposite::operator*(const VectorComposite& _x) const { VectorComposite y; - for (const auto &pair_rkey_row : *this) + for (const auto& pair_rkey_row : *this) { - const auto &rkey = pair_rkey_row.first; - const auto &row = pair_rkey_row.second; + const auto& rkey = pair_rkey_row.first; + const auto& row = pair_rkey_row.second; - for (const auto &pair_ckey_mat : row) + for (const auto& pair_ckey_mat : row) { - const auto &ckey = pair_ckey_mat.first; - const auto &J_r_c = pair_ckey_mat.second; + const auto& ckey = pair_ckey_mat.first; + const auto& J_r_c = pair_ckey_mat.second; const auto& it = y.find(rkey); if (it != y.end()) @@ -188,14 +186,11 @@ wolf::VectorComposite MatrixComposite::operator *(const VectorComposite &_x) con MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov) { - - // simplify names of operands const auto& J = *this; const auto& Q = _Cov; - MatrixComposite S; // S = J * Q * J.tr - + MatrixComposite S; // S = J * Q * J.tr /* Covariance propagation * @@ -239,23 +234,23 @@ MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov) */ // Iterate on the output matrix S first, row i, col j. - for (const auto& pair_i_Si : J) // (1) + for (const auto& pair_i_Si : J) // (1) { const auto& i = pair_i_Si.first; const auto& J_i = pair_i_Si.second; double i_size = J_i.begin()->second.rows(); - for (const auto& pair_j_Sj : J) // (2) + for (const auto& pair_j_Sj : J) // (2) { - const auto& j = pair_j_Sj.first; + const auto& j = pair_j_Sj.first; const auto& J_j = pair_j_Sj.second; double j_size = J_j.begin()->second.rows(); - MatrixXd S_ij(MatrixXd::Zero(i_size, j_size)); // (3) + MatrixXd S_ij(MatrixXd::Zero(i_size, j_size)); // (3) - for (const auto& pair_n_Qn : Q) // (4) + for (const auto& pair_n_Qn : Q) // (4) { const auto& n = pair_n_Qn.first; const auto& Q_n = pair_n_Qn.second; @@ -264,33 +259,33 @@ MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov) const auto& J_in = J_i.at(n); - MatrixXd QJt_nj(MatrixXd::Zero(n_size, j_size)); // (5) + MatrixXd QJt_nj(MatrixXd::Zero(n_size, j_size)); // (5) - for (const auto& pair_k_Qnk : Q_n) // (6) + for (const auto& pair_k_Qnk : Q_n) // (6) { const auto& k = pair_k_Qnk.first; const auto& Q_nk = pair_k_Qnk.second; const auto& J_jk = J_j.at(k); - QJt_nj += Q_nk * J_jk.transpose(); // (7) + QJt_nj += Q_nk * J_jk.transpose(); // (7) } - S_ij += J_in * QJt_nj; // (8) + S_ij += J_in * QJt_nj; // (8) } - S.emplace(i,j,S_ij); + S.emplace(i, j, S_ij); if (j == i) break; // avoid computing the symmetrical block! else - S.emplace(j, i, S_ij.transpose()); // (9) + S.emplace(j, i, S_ij.transpose()); // (9) } } return S; } -MatrixComposite MatrixComposite::operator * (double scalar_right) const +MatrixComposite MatrixComposite::operator*(double scalar_right) const { MatrixComposite S(*this); for (const auto& pair_rkey_row : *this) @@ -305,41 +300,39 @@ MatrixComposite MatrixComposite::operator * (double scalar_right) const return S; } -MatrixComposite operator * (double scalar_left, const MatrixComposite& M) +MatrixComposite operator*(double scalar_left, const MatrixComposite& M) { MatrixComposite S; S = M * scalar_left; return S; } -MatrixXd MatrixComposite::matrix(const StateKeys &_struct_rows, const StateKeys &_struct_cols) const +MatrixXd MatrixComposite::matrix(const StateKeys& _struct_rows, const StateKeys& _struct_cols) const { - - std::unordered_map < char, unsigned int> indices_rows; - std::unordered_map < char, unsigned int> indices_cols; - unsigned int rows, cols; + std::unordered_map<char, unsigned int> indices_rows; + std::unordered_map<char, unsigned int> indices_cols; + unsigned int rows, cols; sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols); - MatrixXd M ( MatrixXd::Zero(rows, cols) ); + MatrixXd M(MatrixXd::Zero(rows, cols)); for (const auto& pair_row_rband : (*this)) { - const auto& row = pair_row_rband.first; + const auto& row = pair_row_rband.first; const auto& rband = pair_row_rband.second; for (const auto& pair_col_blk : rband) { const auto& col = pair_col_blk.first; const auto& blk = pair_col_blk.second; - const unsigned int & blk_rows = size_rows_.at(row); - const unsigned int & blk_cols = size_cols_.at(col); + const unsigned int& blk_rows = size_rows_.at(row); + const unsigned int& blk_cols = size_cols_.at(col); assert(blk.rows() == blk_rows && "MatrixComposite block size mismatch! Wrong number of rows."); assert(blk.cols() == blk_cols && "MatrixComposite block size mismatch! Wrong number of cols."); M.block(indices_rows.at(row), indices_cols.at(col), blk_rows, blk_cols) = blk; - } } @@ -349,54 +342,52 @@ MatrixXd MatrixComposite::matrix(const StateKeys &_struct_rows, const StateKeys unsigned int MatrixComposite::rows() const { unsigned int rows = 0; - for (const auto& pair_row_size : this->size_rows_) - rows += pair_row_size.second; + for (const auto& pair_row_size : this->size_rows_) rows += pair_row_size.second; return rows; } unsigned int MatrixComposite::cols() const { unsigned int cols = 0; - for (const auto& pair_col_size : this->size_rows_) - cols += pair_col_size.second; + for (const auto& pair_col_size : this->size_rows_) cols += pair_col_size.second; return cols; } -void MatrixComposite::sizeAndIndices(const StateKeys &_struct_rows, - const StateKeys &_struct_cols, - std::unordered_map<char, unsigned int> &_indices_rows, - std::unordered_map<char, unsigned int> &_indices_cols, - unsigned int &_rows, - unsigned int &_cols) const +void MatrixComposite::sizeAndIndices(const StateKeys& _struct_rows, + const StateKeys& _struct_cols, + std::unordered_map<char, unsigned int>& _indices_rows, + std::unordered_map<char, unsigned int>& _indices_cols, + unsigned int& _rows, + unsigned int& _cols) const { _rows = 0; _cols = 0; for (const auto& row : _struct_rows) { - _indices_rows[row] = _rows; - _rows += size_rows_.at(row); + _indices_rows[row] = _rows; + _rows += size_rows_.at(row); } for (const auto& col : _struct_cols) { - _indices_cols[col] = _cols; - _cols += size_cols_.at(col); + _indices_cols[col] = _cols; + _cols += size_cols_.at(col); } } -MatrixComposite::MatrixComposite (const StateKeys& _row_structure, const StateKeys& _col_structure) +MatrixComposite::MatrixComposite(const StateKeys& _row_structure, const StateKeys& _col_structure) { for (const auto& rkey : _row_structure) - for (const auto& ckey : _col_structure) - this->emplace(rkey, ckey, MatrixXd(0,0)); + for (const auto& ckey : _col_structure) this->emplace(rkey, ckey, MatrixXd(0, 0)); } -MatrixComposite::MatrixComposite (const StateKeys& _row_structure, - const std::list<int>& _row_sizes, - const StateKeys& _col_structure, - const std::list<int>& _col_sizes) +MatrixComposite::MatrixComposite(const StateKeys& _row_structure, + const std::list<int>& _row_sizes, + const StateKeys& _col_structure, + const std::list<int>& _col_sizes) { - assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!"); - assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!"); + assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!"); + assert(_col_structure.size() == _col_sizes.size() && + "Column structure and sizes have different number of elements!"); auto rsize_it = _row_sizes.begin(); for (const auto& rkey : _row_structure) @@ -404,30 +395,31 @@ MatrixComposite::MatrixComposite (const StateKeys& _row_structure, auto csize_it = _col_sizes.begin(); for (const auto& ckey : _col_structure) { - this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized. + this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized. - csize_it ++; + csize_it++; } rsize_it++; } } -MatrixComposite::MatrixComposite (const MatrixXd& _m, - const StateKeys& _row_structure, - const std::list<int>& _row_sizes, - const StateKeys& _col_structure, - const std::list<int>& _col_sizes) +MatrixComposite::MatrixComposite(const MatrixXd& _m, + const StateKeys& _row_structure, + const std::list<int>& _row_sizes, + const StateKeys& _col_structure, + const std::list<int>& _col_sizes) { - assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!"); - assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!"); + assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!"); + assert(_col_structure.size() == _col_sizes.size() && + "Column structure and sizes have different number of elements!"); - SizeEigen rindex = 0, cindex; - auto rsize_it = _row_sizes.begin(); + SizeEigen rindex = 0, cindex; + auto rsize_it = _row_sizes.begin(); for (const auto& rkey : _row_structure) { assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows"); - cindex = 0; + cindex = 0; auto csize_it = _col_sizes.begin(); for (const auto& ckey : _col_structure) @@ -437,7 +429,7 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m, this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it)); cindex += *csize_it; - csize_it ++; + csize_it++; } assert(_m.cols() == cindex && "Provided matrix has too many columns"); @@ -449,13 +441,16 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m, assert(_m.rows() == rindex && "Provided matrix has too many rows"); } -MatrixComposite MatrixComposite::zero (const StateKeys& _row_structure, const std::list<int>& _row_sizes, - const StateKeys& _col_structure, const std::list<int>& _col_sizes) +MatrixComposite MatrixComposite::zero(const StateKeys& _row_structure, + const std::list<int>& _row_sizes, + const StateKeys& _col_structure, + const std::list<int>& _col_sizes) { MatrixComposite m; - assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!"); - assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!"); + assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!"); + assert(_col_structure.size() == _col_sizes.size() && + "Column structure and sizes have different number of elements!"); auto rsize_it = _row_sizes.begin(); for (const auto& rkey : _row_structure) @@ -465,34 +460,33 @@ MatrixComposite MatrixComposite::zero (const StateKeys& _row_structure, const st { m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it)); - csize_it ++; + csize_it++; } rsize_it++; } return m; } -MatrixComposite MatrixComposite::identity (const StateKeys& _structure, const std::list<int>& _sizes) +MatrixComposite MatrixComposite::identity(const StateKeys& _structure, const std::list<int>& _sizes) { MatrixComposite m; - assert (_structure.size() == _sizes.size() && "Structure and sizes have different number of elements!"); + assert(_structure.size() == _sizes.size() && "Structure and sizes have different number of elements!"); auto rsize_it = _sizes.begin(); - auto rkey_it = _structure.begin(); + auto rkey_it = _structure.begin(); while (rkey_it != _structure.end()) { + const auto& rkey = *rkey_it; + const auto rsize = *rsize_it; - const auto& rkey = *rkey_it; - const auto rsize = *rsize_it; - - m.emplace(rkey, rkey, MatrixXd::Identity(rsize,rsize)); // diagonal block + m.emplace(rkey, rkey, MatrixXd::Identity(rsize, rsize)); // diagonal block auto csize_it = rsize_it; - auto ckey_it = rkey_it; + auto ckey_it = rkey_it; - csize_it ++; + csize_it++; ckey_it++; while (ckey_it != _structure.end()) @@ -500,20 +494,20 @@ MatrixComposite MatrixComposite::identity (const StateKeys& _structure, const st const auto& ckey = *ckey_it; const auto& csize = *csize_it; - m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block - m.emplace(ckey, rkey, MatrixXd::Zero(csize, rsize)); // symmetric block + m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block + m.emplace(ckey, rkey, MatrixXd::Zero(csize, rsize)); // symmetric block - csize_it ++; + csize_it++; ckey_it++; } - rsize_it ++; + rsize_it++; rkey_it++; } return m; } -bool MatrixComposite::check ( ) const +bool MatrixComposite::check() const { bool size_ok = true; @@ -521,7 +515,7 @@ bool MatrixComposite::check ( ) const for (const auto& pair_rkey_row : *this) { const auto& rkey = pair_rkey_row.first; - const auto& row = pair_rkey_row.second; + const auto& row = pair_rkey_row.second; for (const auto& pair_ckey_blk : row) { const auto& ckey = pair_ckey_blk.first; @@ -529,7 +523,7 @@ bool MatrixComposite::check ( ) const if (size_rows_.count(rkey) != 0) { - if( size_rows_.at(rkey) != blk.rows()) + if (size_rows_.at(rkey) != blk.rows()) { WOLF_ERROR("row size for row ", rkey, " has wrong size"); size_ok = false; @@ -542,7 +536,7 @@ bool MatrixComposite::check ( ) const } if (size_cols_.count(ckey) != 0) { - if( size_cols_.at(ckey) != blk.cols()) + if (size_cols_.at(ckey) != blk.cols()) { WOLF_ERROR("col size for col ", rkey, " has wrong size"); size_ok = false; @@ -558,15 +552,15 @@ bool MatrixComposite::check ( ) const return size_ok; } -std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M) +std::ostream& operator<<(std::ostream& _os, const MatrixComposite& _M) { - for (const auto &pair_rkey_row : _M) + for (const auto& pair_rkey_row : _M) { const auto rkey = pair_rkey_row.first; - for (const auto &pair_ckey_mat : pair_rkey_row.second) + for (const auto& pair_ckey_mat : pair_rkey_row.second) { - const auto &ckey = pair_ckey_mat.first; + const auto& ckey = pair_ckey_mat.first; _os << "\n[" << rkey << "," << ckey << "]: \n" << pair_ckey_mat.second; } @@ -574,5 +568,4 @@ std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M) return _os; } -} // namespace wolf - +} // namespace wolf diff --git a/src/composite/spec_state_sensor_composite.cpp b/src/composite/spec_state_sensor_composite.cpp index 3f6176af3e33f17bbc555506c272434c957cd95e..5608f4c7213e870b63a7cd7f35023f8ce21cfe6a 100644 --- a/src/composite/spec_state_sensor_composite.cpp +++ b/src/composite/spec_state_sensor_composite.cpp @@ -20,26 +20,22 @@ #include "core/composite/spec_state_sensor_composite.h" #include "core/state_block/factory_state_block.h" -#include "core/common/params_base.h" // toString() +#include "core/common/params_base.h" // toString() namespace wolf { - SpecStateSensor::SpecStateSensor(const std::string& _type, const Eigen::VectorXd& _state, const std::string& _mode, const Eigen::VectorXd& _noise_std, bool _dynamic, - const Eigen::VectorXd& _drift_std) : - SpecState(_type, _state, _mode, _noise_std), - dynamic_(_dynamic), - drift_std_(_drift_std) + const Eigen::VectorXd& _drift_std) + : SpecState(_type, _state, _mode, _noise_std), dynamic_(_dynamic), drift_std_(_drift_std) { check(); } -SpecStateSensor::SpecStateSensor(const YAML::Node& prior_node) : - SpecState(prior_node) +SpecStateSensor::SpecStateSensor(const YAML::Node& prior_node) : SpecState(prior_node) { dynamic_ = prior_node["dynamic"].as<bool>(); @@ -55,7 +51,7 @@ void SpecStateSensor::check() const { SpecState::check(); - auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); // already tried in SpecState::check() + auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); // already tried in SpecState::check() // check drift sigma size if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize()) @@ -65,8 +61,7 @@ void SpecStateSensor::check() const std::string SpecStateSensor::print(const std::string& _spaces) const { - return SpecState::print(_spaces) + - _spaces + "dynamic: " + toString(dynamic_) + "\n" + + return SpecState::print(_spaces) + _spaces + "dynamic: " + toString(dynamic_) + "\n" + (drift_std_.size() > 0 ? _spaces + "drift_std: " + toString(drift_std_) + "\n" : ""); } @@ -79,9 +74,8 @@ std::ostream& operator<<(std::ostream& _os, const wolf::SpecStateSensor& _x) YAML::Node SpecStateSensor::toYaml() const { YAML::Node node = SpecState::toYaml(); - node["dynamic"] = dynamic_; - if (dynamic_ and drift_std_.size() > 0) - node["drift_std"] = drift_std_; + node["dynamic"] = dynamic_; + if (dynamic_ and drift_std_.size() > 0) node["drift_std"] = drift_std_; return node; } diff --git a/src/composite/vector_composite.cpp b/src/composite/vector_composite.cpp index 7156856700259f50287d26a31cef48fe495054a4..68c13b4909330c717bd9c0ac5dead0805e6c2499 100644 --- a/src/composite/vector_composite.cpp +++ b/src/composite/vector_composite.cpp @@ -22,7 +22,6 @@ namespace wolf { - VectorComposite::VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes) { assert(_keys.size() == _sizes.size() && "Keys and _sizes list size mismatch"); diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp index 83031d1965af391d0a9787cb726f56aa8138da44..f879376e8e661072d1944eb6021dafeb630bd62b 100644 --- a/src/factor/factor_analytic.cpp +++ b/src/factor/factor_analytic.cpp @@ -27,7 +27,7 @@ FactorAnalytic::FactorAnalytic(const std::string& _type, const FactorTopology& _top, const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _measurement_sqrt_information_upper, - const NodeStateBlocksPtrList& _node_state_block_list, + const NodeStateBlocksPtrList& _node_state_block_list, const ProcessorBasePtr& _processor_ptr, const std::vector<StateBlockPtr>& _state_ptrs, bool _apply_loss_function, diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index baa1665ccf191e6f134996c968e6bc2c6453fea6..e227b4e68590a66e78d42262a65e2cb67f7ee743 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -194,9 +194,7 @@ void FeatureBase::print(int _depth, if (c) c->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + " "); } -CheckLog FeatureBase::localCheck(bool _verbose, - std::ostream& _stream, - std::string _tabs) const +CheckLog FeatureBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const { CheckLog log; std::stringstream inconsistency_explanation; @@ -234,10 +232,7 @@ CheckLog FeatureBase::localCheck(bool _verbose, return log; } -bool FeatureBase::check(CheckLog& _log, - bool _verbose, - std::ostream& _stream, - std::string _tabs) const +bool FeatureBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const { auto local_log = localCheck(_verbose, _stream, _tabs); _log.compose(local_log); diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp index 4c97a4d33cd91f35ab79643446f36b7bb26e9e46..5703bb078b2809d3fe79402e9a1c98ccdfc89eb5 100644 --- a/src/feature/feature_diff_drive.cpp +++ b/src/feature/feature_diff_drive.cpp @@ -22,16 +22,15 @@ namespace wolf { - FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated, const Eigen::MatrixXd& _delta_preintegrated_covariance, const Eigen::VectorXd& _diff_drive_params, - const Eigen::MatrixXd& _jacobian_diff_drive_params) : - FeatureMotion("FeatureDiffDrive", - _delta_preintegrated, - _delta_preintegrated_covariance, - _diff_drive_params, - _jacobian_diff_drive_params) + const Eigen::MatrixXd& _jacobian_diff_drive_params) + : FeatureMotion("FeatureDiffDrive", + _delta_preintegrated, + _delta_preintegrated_covariance, + _diff_drive_params, + _jacobian_diff_drive_params) { // } diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp index c80cc7c93c98c07541100d5bd5a1a45ad82bc2c2..c677ad335e3cf0a8a48327a69acc82b836b326e0 100644 --- a/src/feature/feature_motion.cpp +++ b/src/feature/feature_motion.cpp @@ -22,15 +22,14 @@ namespace wolf { - FeatureMotion::FeatureMotion(const std::string& _type, - const VectorXd& _delta_preint, - const MatrixXd _delta_preint_cov, - const VectorXd& _calib_preint, - const MatrixXd& _jacobian_calib) : - FeatureBase(_type, _delta_preint, _delta_preint_cov), - calib_preint_(_calib_preint), - jacobian_calib_(_jacobian_calib) + const VectorXd& _delta_preint, + const MatrixXd _delta_preint_cov, + const VectorXd& _calib_preint, + const MatrixXd& _jacobian_calib) + : FeatureBase(_type, _delta_preint, _delta_preint_cov), + calib_preint_(_calib_preint), + jacobian_calib_(_jacobian_calib) { // } diff --git a/src/feature/feature_odom_2d.cpp b/src/feature/feature_odom_2d.cpp index f07bb4e6f7cb2ecc2187a86a2e073c1eec1bd94a..e2f68e89ced8518171df2d92e93da41619446496 100644 --- a/src/feature/feature_odom_2d.cpp +++ b/src/feature/feature_odom_2d.cpp @@ -20,12 +20,13 @@ #include "core/feature/feature_odom_2d.h" -namespace wolf { - -FeatureOdom2d::FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) : - FeatureBase("FeatureOdom2d", _measurement, _meas_covariance) +namespace wolf +{ +FeatureOdom2d::FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) + : FeatureBase("FeatureOdom2d", _measurement, _meas_covariance) { - //std::cout << "New FeatureOdom2d: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl; + // std::cout << "New FeatureOdom2d: measurement " << _measurement.transpose() << std::endl << "covariance" << + // std::endl << _meas_covariance << std::endl; } FeatureOdom2d::~FeatureOdom2d() @@ -33,4 +34,4 @@ FeatureOdom2d::~FeatureOdom2d() // } -} // namespace wolf +} // namespace wolf diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp index b53ed99b86e118d7a82c8e798805e6c8090a8709..443e08780af75c82dd6d6297f07d6858b29b5c4f 100644 --- a/src/feature/feature_pose.cpp +++ b/src/feature/feature_pose.cpp @@ -20,10 +20,10 @@ #include "core/feature/feature_pose.h" -namespace wolf { - -FeaturePose::FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) : - FeatureBase("FeaturePose", _measurement, _meas_covariance) +namespace wolf +{ +FeaturePose::FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) + : FeatureBase("FeaturePose", _measurement, _meas_covariance) { // } @@ -33,4 +33,4 @@ FeaturePose::~FeaturePose() // } -} // namespace wolf +} // namespace wolf diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index c60c5c9b60cfe52515cb39f10d23818568d8ad3f..75eed4795ead83cc1555522a56e66c4b8c9f6ef1 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -29,7 +29,6 @@ namespace wolf { - unsigned int FrameBase::frame_id_count_ = 0; FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp index f41685c7dbcfa128ebbb5b803c984bdbfb277339..d4a8a45ed943bd61c8ab2d297db6d22293a06949 100644 --- a/src/hardware/hardware_base.cpp +++ b/src/hardware/hardware_base.cpp @@ -23,7 +23,6 @@ namespace wolf { - HardwareBase::HardwareBase() : NodeBase("HARDWARE", "HardwareBase") { // std::cout << "constructed H" << std::endl; diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 037fd2c82e6c33263c6352f7476268b7522b906c..db392e44576e56e9fb0cdfc70f4fc67355350106 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -147,9 +147,7 @@ void LandmarkBase::print(int _depth, printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs); } -CheckLog LandmarkBase::localCheck(bool _verbose, - std::ostream& _stream, - std::string _tabs) const +CheckLog LandmarkBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const { CheckLog log; std::stringstream inconsistency_explanation; @@ -163,8 +161,8 @@ CheckLog LandmarkBase::localCheck(bool _verbose, // check problem and map pointers auto map_ptr = getMap(); - inconsistency_explanation << "Landmarks' problem ptr " << getProblem() - << " different from Map's problem ptr " << map_ptr->getProblem() << "\n"; + inconsistency_explanation << "Landmarks' problem ptr " << getProblem() << " different from Map's problem ptr " + << map_ptr->getProblem() << "\n"; log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation); @@ -180,10 +178,7 @@ CheckLog LandmarkBase::localCheck(bool _verbose, return log; } -bool LandmarkBase::check(CheckLog& _log, - bool _verbose, - std::ostream& _stream, - std::string _tabs) const +bool LandmarkBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const { auto local_log = localCheck(_verbose, _stream, _tabs); _log.compose(local_log); @@ -194,4 +189,4 @@ bool LandmarkBase::check(CheckLog& _log, // Register landmark creator WOLF_REGISTER_LANDMARK(LandmarkBase); -} // namespace wolf +} // namespace wolf diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp index fe02ce3987987ec5fcf6eb1109366d9f8272a4e6..7b757f95c0e2dff5ccc0620302a5591d2a8900dd 100644 --- a/src/map/map_base.cpp +++ b/src/map/map_base.cpp @@ -35,7 +35,6 @@ namespace wolf { - MapBase::MapBase() : NodeBase("MAP", "Base") { // std::cout << "constructed M"<< std::endl; diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 964aafcb086c9a1849bb6de1bea4d14f6b266a98..d06e06e27a39e153fba4d521fcde6c65a9ad489b 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -376,7 +376,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_ } ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, - SensorBasePtr _corresponding_sensor, + SensorBasePtr _corresponding_sensor, const std::string& _params_yaml_filename, const std::vector<std::string>& _folders_schema) { diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp index 30f3958235120c1729f79aaabb312fdff2813831..f5dde074f7c630669f0b87504314efcc80ae8de6 100644 --- a/src/processor/motion_buffer.cpp +++ b/src/processor/motion_buffer.cpp @@ -21,51 +21,44 @@ #include "core/processor/motion_buffer.h" namespace wolf { - Motion::Motion(const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _delta, - const MatrixXd& _delta_cov, - const VectorXd& _delta_integr, - const MatrixXd& _delta_integr_cov, - const MatrixXd& _jac_delta, - const MatrixXd& _jac_delta_int, - const MatrixXd& _jac_calib) : - data_size_(_data.size()), - delta_size_(_delta.size()), - cov_size_(_delta_cov.cols()), - calib_size_(_jac_calib.cols()), - ts_(_ts), - data_(_data), - data_cov_(_data_cov), - delta_(_delta), - delta_cov_(_delta_cov), - delta_integr_(_delta_integr), - delta_integr_cov_(_delta_integr_cov), - jacobian_delta_(_jac_delta), - jacobian_delta_integr_(_jac_delta_int), - jacobian_calib_(_jac_calib) + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _delta, + const MatrixXd& _delta_cov, + const VectorXd& _delta_integr, + const MatrixXd& _delta_integr_cov, + const MatrixXd& _jac_delta, + const MatrixXd& _jac_delta_int, + const MatrixXd& _jac_calib) + : data_size_(_data.size()), + delta_size_(_delta.size()), + cov_size_(_delta_cov.cols()), + calib_size_(_jac_calib.cols()), + ts_(_ts), + data_(_data), + data_cov_(_data_cov), + delta_(_delta), + delta_cov_(_delta_cov), + delta_integr_(_delta_integr), + delta_integr_cov_(_delta_integr_cov), + jacobian_delta_(_jac_delta), + jacobian_delta_integr_(_jac_delta_int), + jacobian_calib_(_jac_calib) { } Motion::Motion(const TimeStamp& _ts, - SizeEigen _data_size, - SizeEigen _delta_size, - SizeEigen _cov_size, - SizeEigen _calib_size) : - data_size_(_data_size), - delta_size_(_delta_size), - cov_size_(_cov_size), - calib_size_(_calib_size), - ts_(_ts) + SizeEigen _data_size, + SizeEigen _delta_size, + SizeEigen _cov_size, + SizeEigen _calib_size) + : data_size_(_data_size), delta_size_(_delta_size), cov_size_(_cov_size), calib_size_(_calib_size), ts_(_ts) { resize(_data_size, _delta_size, _cov_size, _calib_size); } -Motion::~Motion() -{ -} +Motion::~Motion() {} void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s) { @@ -89,11 +82,8 @@ MotionBuffer::MotionBuffer() const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const { - //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds"); - auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) - { - return m.ts_ <= _ts; - }); + // assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds"); + auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; }); if (previous == this->rend()) // The time stamp is older than the buffer's oldest data. // We could do something here, and throw an error or something, but by now we'll return the first valid data @@ -104,11 +94,8 @@ const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const { - //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds"); - auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) - { - return m.ts_ <= _ts; - }); + // assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds"); + auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; }); if (previous == this->rend()) // The time stamp is older than the buffer's oldest data. // We could do something here, but by now we'll return the last valid data @@ -120,12 +107,9 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts) { assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller or equal than the buffer's first tick"); - assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater or equal than the buffer's last tick"); + assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater or equal than the buffer's last tick"); - auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) - { - return m.ts_ <= _ts; - }); + auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; }); if (previous == this->rend()) { // The time stamp is more recent than the buffer's most recent data: @@ -135,14 +119,10 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before else { // Transfer part of the buffer - _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(), - *this, - this->begin(), - (previous--).base()); + _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(), *this, this->begin(), (previous--).base()); } } - void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs, bool show_covs) { using std::cout; @@ -151,44 +131,38 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b if (!show_data && !show_delta && !show_delta_int && !show_jacs && !show_covs) { cout << "Buffer size: " << this->size() << " ; time stamps: <"; - for (Motion mot : *this) - cout << " " << mot.ts_; + for (Motion mot : *this) cout << " " << mot.ts_; cout << " >" << endl; } else { - print(0,0,0,0); + print(0, 0, 0, 0); for (Motion mot : *this) { cout << "-- Motion (" << mot.ts_ << ")" << endl; if (show_data) { cout << " data: " << mot.data_.transpose() << endl; - if (show_covs) - cout << " data cov: \n" << mot.data_cov_ << endl; + if (show_covs) cout << " data cov: \n" << mot.data_cov_ << endl; } if (show_delta) { cout << " delta: " << mot.delta_.transpose() << endl; - if (show_covs) - cout << " delta cov: \n" << mot.delta_cov_ << endl; + if (show_covs) cout << " delta cov: \n" << mot.delta_cov_ << endl; } if (show_delta_int) { cout << " delta integrated: " << mot.delta_integr_.transpose() << endl; - if (show_covs) - cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl; + if (show_covs) cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl; } if (show_jacs) { cout << " Jac delta: \n" << mot.jacobian_delta_ << endl; cout << " Jac delta integr: \n" << mot.jacobian_delta_integr_ << endl; cout << " Jac calib: \n" << mot.jacobian_calib_ << endl; - } } } } -} - +} // namespace wolf diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp index 3726ad2c0a6d81396ecf96e27d7c98a531ebc71a..0ad0109bc5d29740f5f18bbaea0510c32d869377 100644 --- a/src/processor/motion_provider.cpp +++ b/src/processor/motion_provider.cpp @@ -25,7 +25,9 @@ using namespace wolf; void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr) { - WOLF_DEBUG_COND(not isStateGetter(), "MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor"); + WOLF_DEBUG_COND( + not isStateGetter(), + "MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor"); if (isStateGetter()) { _prb_ptr->addMotionProvider(_motion_ptr); @@ -38,14 +40,12 @@ void MotionProvider::setOdometry(const VectorComposite& _odom) std::lock_guard<std::mutex> lock(mut_odometry_); assert(_odom.has(getStateKeys()) && "MotionProvider::setOdometry(): any key missing in _odom."); - for (auto key : getStateKeys()) - odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_types_ + for (auto key : getStateKeys()) odometry_[key] = _odom.at(key); // overwrite/insert only keys of the state_types_ } -wolf::VectorComposite MotionProvider::getOdometry ( ) const +wolf::VectorComposite MotionProvider::getOdometry() const { std::lock_guard<std::mutex> lock(mut_odometry_); return odometry_; } - diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 1d86e6f81d7a2204749e3c8c7de7a4c69ac96320..44f3cc0f160ab30f21f5300a8d4ddf7024c606de 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -197,8 +197,8 @@ CheckLog ProcessorBase::localCheck(bool _verbose, std::ostream& _stream, std::st << " is different from Sensor problem pointer " << sen_ptr->getProblem() << "\n"; log.assertTrue((getProblem() == sen_ptr->getProblem()), inconsistency_explanation); - inconsistency_explanation << "Prc" << id() << " @ " << this << " ---> Sen" << sen_ptr->id() << " @ " - << sen_ptr << " -X-> Prc" << id(); + inconsistency_explanation << "Prc" << id() << " @ " << this << " ---> Sen" << sen_ptr->id() << " @ " << sen_ptr + << " -X-> Prc" << id(); auto sen_prc_list = sen_ptr->getProcessorList(); auto sen_has_prc = std::find(sen_prc_list.begin(), sen_prc_list.end(), shared_from_this()); log.assertTrue(sen_has_prc != sen_prc_list.end(), inconsistency_explanation); diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index 8b95f0be06e0a4473815756736d3acc16b95fbd0..b9d091017be0199ae130dff522c2625cbdc9c9cd 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -28,10 +28,9 @@ namespace wolf { - ProcessorDiffDrive::ProcessorDiffDrive(const YAML::Node& _params) : ProcessorOdom2d(_params), - radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor). + radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor). { setType("ProcessorDiffDrive"); // overwrite odom2d setting calib_size_ = 3; // overwrite odom2d setting diff --git a/src/processor/processor_fixed_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp index 5334f5ecbd8a313a5f805d8dac0def15128d5e89..582b6e73f54152dc4532687d6870d8362c8530de 100644 --- a/src/processor/processor_fixed_wing_model.cpp +++ b/src/processor/processor_fixed_wing_model.cpp @@ -26,42 +26,38 @@ namespace wolf { - -ProcessorFixedWingModel::ProcessorFixedWingModel(const YAML::Node& _params) : - ProcessorBase("ProcessorFixedWingModel", 3, _params), - velocity_local_(_params["velocity_local"].as<Eigen::Vector3d>()), - angle_stdev_(_params["angle_stdev"].as<double>()), - min_vel_norm_(_params["min_vel_norm"].as<double>()) +ProcessorFixedWingModel::ProcessorFixedWingModel(const YAML::Node& _params) + : ProcessorBase("ProcessorFixedWingModel", 3, _params), + velocity_local_(_params["velocity_local"].as<Eigen::Vector3d>()), + angle_stdev_(_params["angle_stdev"].as<double>()), + min_vel_norm_(_params["min_vel_norm"].as<double>()) { - assert(std::abs(velocity_local_.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixedWingModel: 'velocity_local' must be normalized"); + assert(std::abs(velocity_local_.norm() - 1.0) < wolf::Constants::EPS && + "ParamsProcessorFixedWingModel: 'velocity_local' must be normalized"); } -ProcessorFixedWingModel::~ProcessorFixedWingModel() -{ -} +ProcessorFixedWingModel::~ProcessorFixedWingModel() {} void ProcessorFixedWingModel::configure(SensorBasePtr _sensor) { - assert(_sensor->getProblem()->getFrameKeys().find('V') != std::string::npos && "Processor only works with problems with V"); + assert(_sensor->getProblem()->getFrameKeys().find('V') != std::string::npos && + "Processor only works with problems with V"); } void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr) { - if (_keyframe_ptr->getV()->isFixed()) - return; + if (_keyframe_ptr->getV()->isFixed()) return; - if (not _keyframe_ptr->getFactorsOf(shared_from_this()).empty()) - return; + if (not _keyframe_ptr->getFactorsOf(shared_from_this()).empty()) return; // emplace capture - auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase", - _keyframe_ptr->getTimeStamp(), getSensor()); - + auto cap = + CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase", _keyframe_ptr->getTimeStamp(), getSensor()); + // emplace feature - auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase", - velocity_local_, - Eigen::Matrix1d(angle_stdev_ * angle_stdev_)); - + auto fea = FeatureBase::emplace<FeatureBase>( + cap, "FeatureBase", velocity_local_, Eigen::Matrix1d(angle_stdev_ * angle_stdev_)); + // emplace factor auto fac = FactorBase::emplace<FactorVelocityLocalDirection3d>(fea, fea->getMeasurement(), @@ -75,5 +71,4 @@ void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr) // Register in the FactoryProcessor WOLF_REGISTER_PROCESSOR(ProcessorFixedWingModel); -} // namespace wolf - +} // namespace wolf diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp index 84850d4ac9f38f0671f0842e26ada002381042f0..53021d8cc49b9a08bd29e9b0f4c07559dc56e01b 100644 --- a/src/processor/processor_landmark_external.cpp +++ b/src/processor/processor_landmark_external.cpp @@ -36,7 +36,6 @@ using namespace Eigen; namespace wolf { - void ProcessorLandmarkExternal::preProcess() { new_features_incoming_.clear(); diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp index 85d190a13c0fdddc6a1fe2a7bb4c50504fcdae4e..90aaaa8970fbac2dcf6800509e978c6c29a5b8b7 100644 --- a/src/processor/processor_loop_closure.cpp +++ b/src/processor/processor_loop_closure.cpp @@ -22,7 +22,6 @@ namespace wolf { - ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params) : ProcessorBase(_type, _dim, _params) { diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 746279a02bd6b1b198aeee55f3908faa6b93f210..2da24c1143bbdf130354866e9bb954e715adf147 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -23,7 +23,6 @@ namespace wolf { - ProcessorMotion::ProcessorMotion(const std::string& _type, TypeComposite _state_types, int _dim, @@ -345,7 +344,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // will be emplaced capture_existing->getFeatureList().back()->remove(); // factor is removed automatically - assert(capture_existing->getFeatureList().empty()); // there was only one feature! + assert(capture_existing->getFeatureList().empty()); // there was only one feature! emplaceFeaturesAndFactors(capture_for_keyframe_callback, capture_existing); } diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index 8963cda0fc2c7fc4cd228f469f2d7a959d7808d3..0a8117a34db7a8da9067eb88d3c862b0fb213394 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -26,7 +26,6 @@ namespace wolf { - ProcessorOdom2d::ProcessorOdom2d(const YAML::Node& _params) : ProcessorMotion("ProcessorOdom2d", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, 2, 3, 3, 3, 2, 0, _params) { diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp index 42795eb4c4fe21d32a51ee36c107e6c56e266ad2..28900f34f84959c6ec29c50faa974f5d20f8589d 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/src/processor/processor_odom_3d.cpp @@ -24,7 +24,6 @@ namespace wolf { - ProcessorOdom3d::ProcessorOdom3d(const YAML::Node& _params) : ProcessorMotion("ProcessorOdom3d", {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, 3, 7, 7, 6, 6, 0, _params) { diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 46d93f01ea714619b9328629b1620ce68b329cb8..bc093a4e338c9b73f2eb2313ce78c286c1d92356 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -26,7 +26,6 @@ namespace wolf { - ProcessorTracker::ProcessorTracker(const std::string& _type, const StateKeys& _state_keys, int _dim, @@ -124,8 +123,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) } case SECOND_TIME_WITH_KEYFRAME: { // No-break case only for debug. Next case will be executed too. - FrameBasePtr keyframe_from_callback = - buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance()); + FrameBasePtr keyframe_from_callback = buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance()); // This received KF is discarded since it is most likely the same KF we createed in FIRST_TIME, ... // ... only that in FIRST_TIME we checked for incominig, and now we checked for last. // Such KF however should have been removed from the buffer of keyframes with the call to @@ -174,8 +172,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) break; } case RUNNING_WITH_KEYFRAME: { - FrameBasePtr keyframe_from_callback = - buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance()); + FrameBasePtr keyframe_from_callback = buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance()); buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp()); WOLF_DEBUG("PT ", diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index bd5dc55b486876e1b59128bdff904133e180f546..5abd3844ff71aacf2f64af0d8271b24f32c63304 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -22,7 +22,6 @@ namespace wolf { - ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type, const StateKeys& _structure, int _dim, diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index c27f201c74fcce58d521679d1add93e3ff2956c2..e6703dec0cd288e7a3547fd4c39ca344f3a6123d 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -25,7 +25,6 @@ namespace wolf { - ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type, const StateKeys& _structure, const YAML::Node& _params) diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp index e57e0adfac75d6fdf000b7ba02be551694893aa3..37d32b7dc38c3e2cac981957b5b5d39900721238 100644 --- a/src/processor/track_matrix.cpp +++ b/src/processor/track_matrix.cpp @@ -22,7 +22,6 @@ namespace wolf { - SizeStd TrackMatrix::track_id_count_ = 0; TrackMatrix::TrackMatrix() @@ -39,9 +38,8 @@ TrackConst TrackMatrix::track(const SizeStd& _track_id) const { TrackConst track_const; if (tracks_.count(_track_id) > 0) - for (auto&& pair : tracks_.at(_track_id)) - track_const[pair.first]=pair.second; - + for (auto&& pair : tracks_.at(_track_id)) track_const[pair.first] = pair.second; + return track_const; } @@ -55,21 +53,16 @@ Track TrackMatrix::track(const SizeStd& _track_id) SnapshotConst TrackMatrix::snapshot(CaptureBaseConstPtr _capture) const { - const auto& it = std::find_if(snapshots_.cbegin(), - snapshots_.cend(), - [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair) - { - return pair.first == _capture; - } - ); + const auto& it = std::find_if( + snapshots_.cbegin(), snapshots_.cend(), [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair) { + return pair.first == _capture; + }); - if (it == snapshots_.cend()) - return SnapshotConst(); + if (it == snapshots_.cend()) return SnapshotConst(); SnapshotConst snapshot_const; - for (auto&& pair : it->second) - snapshot_const[pair.first]=pair.second; - + for (auto&& pair : it->second) snapshot_const[pair.first] = pair.second; + return snapshot_const; } @@ -83,21 +76,22 @@ Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture) void TrackMatrix::newTrack(FeatureBasePtr _ftr) { - track_id_count_ ++; + track_id_count_++; add(track_id_count_, _ftr); } void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr) { - assert(( tracks_.count(_track_id) != 0 || _track_id == track_id_count_) && "Provided track ID does not exist. Use newTrack() instead."); - assert( _ftr->getCapture() != nullptr && "adding a feature not linked to any capture"); + assert((tracks_.count(_track_id) != 0 || _track_id == track_id_count_) && + "Provided track ID does not exist. Use newTrack() instead."); + assert(_ftr->getCapture() != nullptr && "adding a feature not linked to any capture"); _ftr->setTrackId(_track_id); tracks_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr); - snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present + snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present } -void TrackMatrix::add(const FeatureBasePtr& _ftr_existing,const FeatureBasePtr& _ftr_new) +void TrackMatrix::add(const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new) { add(_ftr_existing->trackId(), _ftr_new); } @@ -111,8 +105,7 @@ void TrackMatrix::remove(const SizeStd& _track_id) { CaptureBasePtr cap = pair_time_ftr.second->getCapture(); snapshots_.at(cap).erase(_track_id); - if (snapshots_.at(cap).empty()) - snapshots_.erase(cap); + if (snapshots_.at(cap).empty()) snapshots_.erase(cap); } // Remove track @@ -130,8 +123,7 @@ void TrackMatrix::remove(CaptureBasePtr _cap) { SizeStd trk_id = pair_trkid_ftr.first; tracks_.at(trk_id).erase(ts); - if (tracks_.at(trk_id).empty()) - tracks_.erase(trk_id); + if (tracks_.at(trk_id).empty()) tracks_.erase(trk_id); } // remove snapshot @@ -146,17 +138,15 @@ void TrackMatrix::remove(FeatureBasePtr _ftr) { if (auto cap = _ftr->getCapture()) { - if(tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp())) + if (tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp())) { - tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp()); - if (tracks_.at(_ftr->trackId()).empty()) - tracks_.erase(_ftr->trackId()); + tracks_.at(_ftr->trackId()).erase(cap->getTimeStamp()); + if (tracks_.at(_ftr->trackId()).empty()) tracks_.erase(_ftr->trackId()); } - if(snapshots_.count(cap) && snapshots_.at(cap).count(_ftr->trackId())) + if (snapshots_.count(cap) && snapshots_.at(cap).count(_ftr->trackId())) { - snapshots_ .at(cap).erase(_ftr->trackId()); - if (snapshots_.at(cap).empty()) - snapshots_.erase(cap); + snapshots_.at(cap).erase(_ftr->trackId()); + if (snapshots_.at(cap).empty()) snapshots_.erase(cap); } } } @@ -210,8 +200,7 @@ vector<FeatureBaseConstPtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) if (tracks_.count(_track_id)) { vec.reserve(trackSize(_track_id)); - for (auto const& pair_time_ftr : tracks_.at(_track_id)) - vec.push_back(pair_time_ftr.second); + for (auto const& pair_time_ftr : tracks_.at(_track_id)) vec.push_back(pair_time_ftr.second); } return vec; } @@ -222,8 +211,7 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) if (tracks_.count(_track_id)) { vec.reserve(trackSize(_track_id)); - for (auto const& pair_time_ftr : tracks_.at(_track_id)) - vec.push_back(pair_time_ftr.second); + for (auto const& pair_time_ftr : tracks_.at(_track_id)) vec.push_back(pair_time_ftr.second); } return vec; } @@ -234,8 +222,7 @@ FeatureBaseConstPtrList TrackMatrix::snapshotAsList(CaptureBaseConstPtr _cap) co FeatureBaseConstPtrList lst; - for (auto const& pair_trkid_ftr : snapshot_const) - lst.push_back(pair_trkid_ftr.second); + for (auto const& pair_trkid_ftr : snapshot_const) lst.push_back(pair_trkid_ftr.second); return lst; } @@ -244,8 +231,7 @@ FeatureBasePtrList TrackMatrix::snapshotAsList(CaptureBasePtr _cap) { FeatureBasePtrList lst; if (snapshots_.count(_cap)) - for (auto const& pair_trkid_ftr : snapshots_.at(_cap)) - lst.push_back(pair_trkid_ftr.second); + for (auto const& pair_trkid_ftr : snapshots_.at(_cap)) lst.push_back(pair_trkid_ftr.second); return lst; } @@ -253,8 +239,8 @@ TrackMatchesConst TrackMatrix::matches(CaptureBaseConstPtr _cap_1, CaptureBaseCo { TrackMatchesConst pairs; - auto s_1 = snapshot(_cap_1); - auto s_2 = snapshot(_cap_2); + auto s_1 = snapshot(_cap_1); + auto s_2 = snapshot(_cap_2); auto s_short = s_1; auto s_long = s_2; if (s_1.size() > s_2.size()) @@ -263,7 +249,7 @@ TrackMatchesConst TrackMatrix::matches(CaptureBaseConstPtr _cap_1, CaptureBaseCo s_short = s_2; } - for (auto const & pair_trkid_ftr : s_short) + for (auto const& pair_trkid_ftr : s_short) { SizeStd trk_id = pair_trkid_ftr.first; if (s_long.count(trk_id)) @@ -277,8 +263,8 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) { TrackMatches pairs; - auto s_1 = snapshot(_cap_1); - auto s_2 = snapshot(_cap_2); + auto s_1 = snapshot(_cap_1); + auto s_2 = snapshot(_cap_2); auto s_short = s_1; auto s_long = s_2; if (s_1.size() > s_2.size()) @@ -287,11 +273,10 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) s_short = s_2; } - for (auto const & pair_trkid_ftr : s_short) + for (auto const& pair_trkid_ftr : s_short) { SizeStd trk_id = pair_trkid_ftr.first; - if (s_long.count(trk_id)) - pairs[trk_id] = pair<FeatureBasePtr, FeatureBasePtr>(s_1.at(trk_id), s_2.at(trk_id)); + if (s_long.count(trk_id)) pairs[trk_id] = pair<FeatureBasePtr, FeatureBasePtr>(s_1.at(trk_id), s_2.at(trk_id)); } return pairs; @@ -333,7 +318,8 @@ TrackConst TrackMatrix::trackAtKeyframes(const SizeStd& _track_id) const { auto& ts = pair_ts_ftr.first; auto& ftr = pair_ts_ftr.second; - if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem()) + if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && + ftr->getCapture()->getFrame()->getProblem()) track_kf[ts] = ftr; } return track_kf; @@ -352,7 +338,8 @@ Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id) { auto& ts = pair_ts_ftr.first; auto& ftr = pair_ts_ftr.second; - if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem()) + if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && + ftr->getCapture()->getFrame()->getProblem()) track_kf[ts] = ftr; } return track_kf; @@ -364,25 +351,20 @@ Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id) list<SizeStd> TrackMatrix::trackIds(CaptureBaseConstPtr _capture) const { list<SizeStd> track_ids; - + if (not _capture) - for (auto track_pair : tracks_) - track_ids.push_back(track_pair.first); - else + for (auto track_pair : tracks_) track_ids.push_back(track_pair.first); + else { - auto it = std::find_if(snapshots_.cbegin(), - snapshots_.cend(), - [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair) - { - return pair.first == _capture; - } - ); + auto it = std::find_if( + snapshots_.cbegin(), snapshots_.cend(), [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair) { + return pair.first == _capture; + }); if (it != snapshots_.cend()) - for (auto track_pair : it->second) - track_ids.push_back(track_pair.first); + for (auto track_pair : it->second) track_ids.push_back(track_pair.first); } return track_ids; } -} +} // namespace wolf diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp index 19ff50d07b463ea90b4416dad3e4cef5ef5fb070..6956f98185b9240a0478ecd31e4f155ab8762b0a 100644 --- a/src/sensor/sensor_diff_drive.cpp +++ b/src/sensor/sensor_diff_drive.cpp @@ -24,7 +24,6 @@ namespace wolf { - SensorDiffDrive::SensorDiffDrive(const YAML::Node& _params) : SensorBase("SensorDiffDrive", 2, _params), ticks_per_wheel_revolution_(_params["ticks_per_wheel_revolution"].as<double>()), diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp index b4239f9ddc62030d1a44d2f60c1920a827fb78c0..6cd04a1515086d849d4b195ef5e72047c82ec3d9 100644 --- a/src/sensor/sensor_motion_model.cpp +++ b/src/sensor/sensor_motion_model.cpp @@ -22,7 +22,6 @@ namespace wolf { - SensorMotionModel::SensorMotionModel(const YAML::Node& _params) : SensorBase("SensorMotionModel", -1, _params) {} SensorMotionModel::~SensorMotionModel() {} diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index d0b295cc6e790073589fe7f98b69018b4517dbc4..cff673c7b5437a1e77207fa1ccf6ac642d32f67e 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -23,54 +23,47 @@ #include "core/map/map_base.h" #include "core/landmark/landmark_base.h" -namespace wolf { - -SolverManager::SolverManager(const ProblemPtr& _problem) : - SolverManager(_problem, std::make_shared<ParamsSolver>()) -{ -} - -SolverManager::SolverManager(const ProblemPtr& _problem, - const ParamsSolverPtr& _params) : - n_solve_(0), - n_cov_(0), - acc_duration_total_(0), - acc_duration_solver_(0), - acc_duration_update_(0), - acc_duration_state_(0), - acc_duration_cov_(0), - max_duration_total_(0), - max_duration_solver_(0), - max_duration_update_(0), - max_duration_state_(0), - max_duration_cov_(0), - wolf_problem_(_problem), - params_(_params) +namespace wolf +{ +SolverManager::SolverManager(const ProblemPtr& _problem) : SolverManager(_problem, std::make_shared<ParamsSolver>()) {} + +SolverManager::SolverManager(const ProblemPtr& _problem, const ParamsSolverPtr& _params) + : n_solve_(0), + n_cov_(0), + acc_duration_total_(0), + acc_duration_solver_(0), + acc_duration_update_(0), + acc_duration_state_(0), + acc_duration_cov_(0), + max_duration_total_(0), + max_duration_solver_(0), + max_duration_update_(0), + max_duration_state_(0), + max_duration_cov_(0), + wolf_problem_(_problem), + params_(_params) { assert(_problem != nullptr && "Passed a nullptr ProblemPtr."); } -SolverManager::~SolverManager() -{ -} +SolverManager::~SolverManager() {} void SolverManager::update() { // lock mutex to prevent Problem::transform() during this update() - std::lock_guard<std::mutex> lock_transform (wolf_problem_->mut_transform_); + std::lock_guard<std::mutex> lock_transform(wolf_problem_->mut_transform_); // Consume notification maps - std::map<StateBlockPtr,Notification> sb_notification_map; - std::map<FactorBasePtr,Notification> fac_notification_map; + std::map<StateBlockPtr, Notification> sb_notification_map; + std::map<FactorBasePtr, Notification> fac_notification_map; wolf_problem_->consumeNotifications(sb_notification_map, fac_notification_map); - #ifdef _WOLF_DEBUG - assert(check("before update()")); - #endif +#ifdef _WOLF_DEBUG + assert(check("before update()")); +#endif // REMOVE FACTORS - for (auto fac_notification_it = fac_notification_map.begin(); - fac_notification_it != fac_notification_map.end(); + for (auto fac_notification_it = fac_notification_map.begin(); fac_notification_it != fac_notification_map.end(); /* nothing, next is handled within the for */) { if (fac_notification_it->second == REMOVE) @@ -83,11 +76,10 @@ void SolverManager::update() } // ADD/REMOVE STATE BLOCS - while ( !sb_notification_map.empty() ) + while (!sb_notification_map.empty()) { // add - if (sb_notification_map.begin()->second == ADD) - addStateBlock(sb_notification_map.begin()->first); + if (sb_notification_map.begin()->second == ADD) addStateBlock(sb_notification_map.begin()->first); // remove else @@ -100,7 +92,8 @@ void SolverManager::update() // ADD FACTORS while (!fac_notification_map.empty()) { - assert(fac_notification_map.begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD"); + assert(fac_notification_map.begin()->second == ADD && + "unexpected factor notification value after all REMOVE have been processed, this should be ADD"); // add factor addFactor(fac_notification_map.begin()->first); @@ -118,31 +111,31 @@ void SolverManager::update() // Check for "floating" state blocks (not involved in any factor -> not observable problem) if (state_blocks_2_factors_.at(state_ptr).empty()) { - WOLF_DEBUG("SolverManager::update(): StateBlock ", state_ptr, " is 'Floating' (not involved in any factor). Storing it apart."); + WOLF_DEBUG("SolverManager::update(): StateBlock ", + state_ptr, + " is 'Floating' (not involved in any factor). Storing it apart."); new_floating_state_blocks.insert(state_ptr); continue; } // state update - if (state_ptr->stateUpdated()) - updateStateBlockState(state_ptr); + if (state_ptr->stateUpdated()) updateStateBlockState(state_ptr); // fix update - if (state_ptr->fixUpdated()) - updateStateBlockStatus(state_ptr); + if (state_ptr->fixUpdated()) updateStateBlockStatus(state_ptr); // local parameterization update - if (state_ptr->localParamUpdated()) - updateStateBlockLocalParametrization(state_ptr); + if (state_ptr->localParamUpdated()) updateStateBlockLocalParametrization(state_ptr); } // REMOVE NEWLY DETECTED "floating" STATE BLOCKS (will be added next update() call) for (auto state_ptr : new_floating_state_blocks) { removeStateBlock(state_ptr); - floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()! + floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()! } - // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be set in addStateBlock) + // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be + // set in addStateBlock) for (auto state_ptr : floating_state_blocks_) { state_ptr->resetStateUpdated(); @@ -152,8 +145,8 @@ void SolverManager::update() wolf_problem_->resetTransformed(); #ifdef _WOLF_DEBUG - assert(check("after update()")); - #endif + assert(check("after update()")); +#endif } wolf::ProblemPtr SolverManager::getProblem() @@ -174,16 +167,18 @@ std::string SolverManager::solve(const ReportVerbosity report_level) // update problem auto start_update = std::chrono::high_resolution_clock::now(); update(); - auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_update); + auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>( + std::chrono::high_resolution_clock::now() - start_update); acc_duration_update_ += duration_update; - max_duration_update_ = std::max(max_duration_update_,duration_update); + max_duration_update_ = std::max(max_duration_update_, duration_update); // call derived solver - auto start_solver = std::chrono::high_resolution_clock::now(); - std::string report = solveDerived(report_level); - auto duration_solver = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_solver); + auto start_solver = std::chrono::high_resolution_clock::now(); + std::string report = solveDerived(report_level); + auto duration_solver = std::chrono::duration_cast<std::chrono::microseconds>( + std::chrono::high_resolution_clock::now() - start_solver); acc_duration_solver_ += duration_solver; - max_duration_solver_ = std::max(max_duration_solver_,duration_solver); + max_duration_solver_ = std::max(max_duration_solver_, duration_solver); // update StateBlocks with optimized state value. auto start_state = std::chrono::high_resolution_clock::now(); @@ -201,13 +196,15 @@ std::string SolverManager::solve(const ReportVerbosity report_level) stateblock_statevector.first->transform(wolf_problem_->getTransformation()); } } - auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state); + auto duration_state = + std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state); acc_duration_state_ += duration_state; - max_duration_state_ = std::max(max_duration_state_,duration_state); + max_duration_state_ = std::max(max_duration_state_, duration_state); - auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total); + auto duration_total = + std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total); acc_duration_total_ += duration_total; - max_duration_total_ = std::max(max_duration_total_,duration_total); + max_duration_total_ = std::max(max_duration_total_, duration_total); return report; } @@ -224,9 +221,10 @@ bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks auto ret = computeCovariancesDerived(blocks); - auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov); + auto duration_cov = + std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov); acc_duration_cov_ += duration_cov; - max_duration_cov_ = std::max(max_duration_cov_,duration_cov); + max_duration_cov_ = std::max(max_duration_cov_, duration_cov); return ret; } @@ -238,9 +236,10 @@ bool SolverManager::computeCovariances(const std::vector<StateBlockPtr>& st_list auto ret = computeCovariancesDerived(st_list); - auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov); + auto duration_cov = + std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov); acc_duration_cov_ += duration_cov; - max_duration_cov_ = std::max(max_duration_cov_,duration_cov); + max_duration_cov_ = std::max(max_duration_cov_, duration_cov); return ret; } @@ -267,13 +266,21 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr) // Check if it was stored as a 'floating' state block if (floating_state_blocks_.count(st) == 1) { - WOLF_DEBUG("SolverManager::addFactor(): Factor ", fac_ptr->id(), " involves state block ", st, " stored as 'floating'. Adding the state block to the solver."); - floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()! + WOLF_DEBUG("SolverManager::addFactor(): Factor ", + fac_ptr->id(), + " involves state block ", + st, + " stored as 'floating'. Adding the state block to the solver."); + floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()! addStateBlock(st); } else { - WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later."); + WOLF_WARN("SolverManager::addFactor(): Factor ", + fac_ptr->id(), + " is notified to ADD but the involved state block ", + st, + " is not. Skipping, will be added later."); // put back notification in problem (will be added next update() call) and do nothing wolf_problem_->notifyFactor(fac_ptr, ADD); return; @@ -287,7 +294,8 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr) for (auto st : fac_ptr->getStateBlockPtrVector()) { assert(state_blocks_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block"); - assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block"); + assert(state_blocks_2_factors_.count(st) != 0 && + "SolverManager::addFactor before adding any involved state block"); state_blocks_2_factors_.at(st).push_back(fac_ptr); } @@ -314,7 +322,8 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr) for (auto st : fac_ptr->getStateBlockPtrVector()) { assert(state_blocks_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block"); - assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block"); + assert(state_blocks_2_factors_.count(st) != 0 && + "SolverManager::removeFactor missing any involved state block"); state_blocks_2_factors_.at(st).remove(fac_ptr); } } @@ -336,7 +345,8 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr) assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added"); assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added"); - assert(state_ptr->isValid() && "SolverManager::addStateBlock state block state not valid (local parameterization)"); + assert(state_ptr->isValid() && + "SolverManager::addStateBlock state block state not valid (local parameterization)"); // stateblock maps state_blocks_.emplace(state_ptr, state_ptr->getState()); @@ -345,7 +355,8 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr) // derived addStateBlockDerived(state_ptr); - // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags + // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of + // those things -> reset flags state_ptr->resetStateUpdated(); state_ptr->resetFixUpdated(); state_ptr->resetLocalParamUpdated(); @@ -378,15 +389,19 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr) */ if (!state_blocks_2_factors_.at(state_ptr).empty()) { - WOLF_WARN("SolverManager::removeStateBlock(): StateBlock ", state_ptr, " is notified to REMOVE but ", state_blocks_2_factors_.at(state_ptr).size(), " involved factors still not removed. Skipping, will be removed later."); + WOLF_WARN("SolverManager::removeStateBlock(): StateBlock ", + state_ptr, + " is notified to REMOVE but ", + state_blocks_2_factors_.at(state_ptr).size(), + " involved factors still not removed. Skipping, will be removed later."); // put back notification in problem (will be removed next update() call) and do nothing - for (auto fac : state_blocks_2_factors_.at(state_ptr)) - WOLF_INFO(fac->id()); + for (auto fac : state_blocks_2_factors_.at(state_ptr)) WOLF_INFO(fac->id()); wolf_problem_->notifyStateBlock(state_ptr, REMOVE); return; } - assert(state_blocks_2_factors_.at(state_ptr).empty() && "SolverManager::removeStateBlock removing state block before removing all factors involved"); + assert(state_blocks_2_factors_.at(state_ptr).empty() && + "SolverManager::removeStateBlock removing state block before removing all factors involved"); // derived removeStateBlockDerived(state_ptr); @@ -409,11 +424,12 @@ void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr) { assert(state_ptr && "SolverManager::updateStateBlockState null state block"); assert(state_blocks_.count(state_ptr) == 1 && "SolverManager::updateStateBlockState unregistered state block"); - assert(state_ptr->isValid() && "SolverManager::updateStateBlockState state block state not valid (local parameterization)"); + assert(state_ptr->isValid() && + "SolverManager::updateStateBlockState state block state not valid (local parameterization)"); assert(state_ptr->getState().size() == getAssociatedMemBlock(state_ptr).size()); Eigen::VectorXd new_state = state_ptr->getState(); - std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr)); + std::copy(new_state.data(), new_state.data() + new_state.size(), getAssociatedMemBlockPtr(state_ptr)); // reset flag state_ptr->resetStateUpdated(); } @@ -470,7 +486,8 @@ SolverManager::ReportVerbosity SolverManager::getVerbosity() const bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const { - return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr)); + return isStateBlockFloating(state_ptr) or + (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr)); } bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const @@ -485,33 +502,28 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const bool SolverManager::isStateBlockFixed(const StateBlockPtr& st) { - if (!isStateBlockRegistered(st)) - return false; + if (!isStateBlockRegistered(st)) return false; - if (isStateBlockFloating(st)) - return st->isFixed(); + if (isStateBlockFloating(st)) return st->isFixed(); return isStateBlockFixedDerived(st); } -bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) +bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) { - if (!isStateBlockRegistered(st)) - return false; + if (!isStateBlockRegistered(st)) return false; - if (isStateBlockFloating(st)) - return st->getLocalParametrization() == local_param; + if (isStateBlockFloating(st)) return st->getLocalParametrization() == local_param; return hasThisLocalParametrizationDerived(st, local_param); }; bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const { - if (!isStateBlockRegistered(st)) - return false; + if (!isStateBlockRegistered(st)) return false; - if (isStateBlockFloating(st)) - return st->hasLocalParametrization(); + if (isStateBlockFloating(st)) return st->hasLocalParametrization(); return hasLocalParametrizationDerived(st); }; @@ -553,7 +565,9 @@ bool SolverManager::check(std::string prefix) const // state blocks if (state_blocks_.size() != state_blocks_2_factors_.size()) { - WOLF_ERROR("SolverManager::check: mismatching number of state blocks (state_blocks_, state_blocks_2_factors_) - in ", prefix); + WOLF_ERROR( + "SolverManager::check: mismatching number of state blocks (state_blocks_, state_blocks_2_factors_) - in ", + prefix); ok = false; } auto sb_vec_it = state_blocks_.begin(); @@ -563,14 +577,22 @@ bool SolverManager::check(std::string prefix) const // same state block in both maps if (sb_vec_it->first != sb_fac_it->first) { - WOLF_ERROR("SolverManager::check: mismatching state blocks ", sb_vec_it->first, " and ", sb_fac_it->first, " - in ", prefix); + WOLF_ERROR("SolverManager::check: mismatching state blocks ", + sb_vec_it->first, + " and ", + sb_fac_it->first, + " - in ", + prefix); ok = false; } // no factors involving state block if (sb_fac_it->second.empty()) { - WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is in state_blocks_ but not involved in any factor - in ", prefix); + WOLF_ERROR("SolverManager::check: state block ", + sb_fac_it->first, + " is in state_blocks_ but not involved in any factor - in ", + prefix); ok = false; } else @@ -580,7 +602,12 @@ bool SolverManager::check(std::string prefix) const { if (factors_.count(fac) == 0) { - WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix); + WOLF_ERROR("SolverManager::check: factor ", + fac->id(), + " (involved in sb ", + sb_fac_it->first, + ") missing in factors_ map - in ", + prefix); ok = false; } } @@ -589,7 +616,10 @@ bool SolverManager::check(std::string prefix) const // can't be in both state_blocks_ and floating_state_blocks_ if (floating_state_blocks_.count(sb_fac_it->first) == 1) { - WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is both in state_blocks_ and floating_state_blocks_ - in ", prefix); + WOLF_ERROR("SolverManager::check: state block ", + sb_fac_it->first, + " is both in state_blocks_ and floating_state_blocks_ - in ", + prefix); ok = false; } @@ -605,7 +635,12 @@ bool SolverManager::check(std::string prefix) const { if (state_blocks_.count(sb) == 0) { - WOLF_ERROR("SolverManager::check: state block ", sb, " inolved in factor ", fac->id(), " missing in state_blocks_ map - in ", prefix); + WOLF_ERROR("SolverManager::check: state block ", + sb, + " inolved in factor ", + fac->id(), + " missing in state_blocks_ map - in ", + prefix); ok = false; } } @@ -619,7 +654,10 @@ bool SolverManager::check(std::string prefix) const for (auto sb : state_blocks_) if (!isStateBlockRegisteredDerived(sb.first)) { - WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix); + WOLF_ERROR("SolverManager::check: state block ", + sb.first, + " is in state_blocks_ but not registered in derived solver - in ", + prefix); ok = false; } @@ -627,19 +665,32 @@ bool SolverManager::check(std::string prefix) const for (auto fac : factors_) if (!isFactorRegisteredDerived(fac)) { - WOLF_ERROR("SolverManager::check: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix); + WOLF_ERROR("SolverManager::check: factor ", + fac->id(), + " is in factors_ but not registered in derived solver - in ", + prefix); ok = false; } // numbers if (numStateBlocksDerived() != state_blocks_.size()) { - WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix); + WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", + numStateBlocksDerived(), + " DIFFERENT THAN state_blocks_.size() = ", + state_blocks_.size(), + " - in ", + prefix); ok = false; } if (numFactorsDerived() != numFactors()) { - WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix); + WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", + numFactorsDerived(), + " DIFFERENT THAN numFactors() = ", + numFactors(), + " - in ", + prefix); ok = false; } @@ -648,27 +699,30 @@ bool SolverManager::check(std::string prefix) const void SolverManager::printProfiling(std::ostream& _stream) const { - _stream <<"\nSolverManager:" - <<"\nTotal values:" - << "\n\tSolving state: " << 1e-6*acc_duration_total_.count() << " s - executions: " << n_solve_ - << "\n\t\tUpdate problem: " << 1e-6*acc_duration_update_.count() << " s" << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)" - << "\n\t\tSolver: " << 1e-6*acc_duration_solver_.count() << " s" << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)" - << "\n\t\tUpdate state: " << 1e-6*acc_duration_state_.count() << " s" << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)" - << "\n\tSolving covariance: " << 1e-6*acc_duration_cov_.count() << " s - executions: " << n_cov_ - <<"\nAverage:" - << "\n\tSolving state: " << 1e-3*acc_duration_total_.count() / n_solve_ << " ms" - << "\n\t\tUpdate problem: " << 1e-3*acc_duration_update_.count() / n_solve_ << " ms" - << "\n\t\tSolver: " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms" - << "\n\t\tUpdate state: " << 1e-3*acc_duration_state_.count() / n_solve_ << " ms" - << "\n\tSolving covariance: " << 1e-3*acc_duration_cov_.count() / n_cov_ << " ms" - <<"\nMax values:" - << "\n\tSolving state: " << 1e-3*max_duration_total_.count() << " ms" - << "\n\t\tUpdate problem: " << 1e-3*max_duration_update_.count() << " ms" - << "\n\t\tSolver: " << 1e-3*max_duration_solver_.count() << " ms" - << "\n\t\tUpdate state: " << 1e-3*max_duration_state_.count() << " ms" - << "\n\tSolving covariance: " << 1e-3*max_duration_cov_.count() << " ms" << std::endl; + _stream << "\nSolverManager:" + << "\nTotal values:" + << "\n\tSolving state: " << 1e-6 * acc_duration_total_.count() << " s - executions: " << n_solve_ + << "\n\t\tUpdate problem: " << 1e-6 * acc_duration_update_.count() << " s" + << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)" + << "\n\t\tSolver: " << 1e-6 * acc_duration_solver_.count() << " s" + << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)" + << "\n\t\tUpdate state: " << 1e-6 * acc_duration_state_.count() << " s" + << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)" + << "\n\tSolving covariance: " << 1e-6 * acc_duration_cov_.count() << " s - executions: " << n_cov_ + << "\nAverage:" + << "\n\tSolving state: " << 1e-3 * acc_duration_total_.count() / n_solve_ << " ms" + << "\n\t\tUpdate problem: " << 1e-3 * acc_duration_update_.count() / n_solve_ << " ms" + << "\n\t\tSolver: " << 1e-3 * acc_duration_solver_.count() / n_solve_ << " ms" + << "\n\t\tUpdate state: " << 1e-3 * acc_duration_state_.count() / n_solve_ << " ms" + << "\n\tSolving covariance: " << 1e-3 * acc_duration_cov_.count() / n_cov_ << " ms" + << "\nMax values:" + << "\n\tSolving state: " << 1e-3 * max_duration_total_.count() << " ms" + << "\n\t\tUpdate problem: " << 1e-3 * max_duration_update_.count() << " ms" + << "\n\t\tSolver: " << 1e-3 * max_duration_solver_.count() << " ms" + << "\n\t\tUpdate state: " << 1e-3 * max_duration_state_.count() << " ms" + << "\n\tSolving covariance: " << 1e-3 * max_duration_cov_.count() << " ms" << std::endl; printProfilingDerived(_stream); } -} // namespace wolf +} // namespace wolf diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp index 4b2a57342976d31e547ace486092392b96f503b9..56c3bc896d4a7229defcff172a38242239c141bd 100644 --- a/src/solver_suitesparse/solver_manager.cpp +++ b/src/solver_suitesparse/solver_manager.cpp @@ -20,73 +20,66 @@ #include "core/ceres_wrapper/solver_ceres.h" -SolverManager::SolverManager() -{ - -} +SolverManager::SolverManager() {} SolverManager::~SolverManager() { - removeAllStateUnits(); + removeAllStateUnits(); } -void SolverManager::solve() -{ - -} +void SolverManager::solve() {} -//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr) +// void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr) //{ //} void SolverManager::update(const WolfProblemPtr _problem_ptr) { - // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN - if (_problem_ptr->isReallocated()) - { - // todo: reallocate x - } - else - { - // ADD/UPDATE STATE BLOKS - for(auto state_block : _problem_ptr->getStateList()) - { - if (state_block->getPendingStatus() == ADD_PENDING) - addStateUnit(state_block); - - else if(state_block->getPendingStatus() == UPDATE_PENDING) - updateStateUnitStatus(state_block); - } - //std::cout << "state units updated!" << std::endl; - - // REMOVE STATE UNITS - while (!_problem_ptr->getRemovedStateList().empty()) - { - // TODO: remove state unit - //_problem_ptr->getRemovedStateList().pop_front(); - } - //std::cout << "state units removed!" << std::endl; - - // ADD CONSTRAINTS - FactorBasePtrList fac_list; - _problem_ptr->getTrajectory()->getFactorList(fac_list); - //std::cout << "fac_list.size() = " << fac_list.size() << std::endl; - for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++) - if ((*fac_it)->getPendingStatus() == ADD_PENDING) - addFactor(*fac_it); - - //std::cout << "factors updated!" << std::endl; - } + // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN + if (_problem_ptr->isReallocated()) + { + // todo: reallocate x + } + else + { + // ADD/UPDATE STATE BLOKS + for (auto state_block : _problem_ptr->getStateList()) + { + if (state_block->getPendingStatus() == ADD_PENDING) + addStateUnit(state_block); + + else if (state_block->getPendingStatus() == UPDATE_PENDING) + updateStateUnitStatus(state_block); + } + // std::cout << "state units updated!" << std::endl; + + // REMOVE STATE UNITS + while (!_problem_ptr->getRemovedStateList().empty()) + { + // TODO: remove state unit + //_problem_ptr->getRemovedStateList().pop_front(); + } + // std::cout << "state units removed!" << std::endl; + + // ADD CONSTRAINTS + FactorBasePtrList fac_list; + _problem_ptr->getTrajectory()->getFactorList(fac_list); + // std::cout << "fac_list.size() = " << fac_list.size() << std::endl; + for (auto fac_it = fac_list.begin(); fac_it != fac_list.end(); fac_it++) + if ((*fac_it)->getPendingStatus() == ADD_PENDING) addFactor(*fac_it); + + // std::cout << "factors updated!" << std::endl; + } } void SolverManager::addFactor(FactorBasePtr _corr_ptr) { - //TODO MatrixXd J; Vector e; + // TODO MatrixXd J; Vector e; // getResidualsAndJacobian(_corr_ptr, J, e); // solverQR->addFactor(_corr_ptr, J, e); -// factor_map_[_corr_ptr->id()] = blockIdx; - _corr_ptr->setPendingStatus(NOT_PENDING); + // factor_map_[_corr_ptr->id()] = blockIdx; + _corr_ptr->setPendingStatus(NOT_PENDING); } void SolverManager::removeFactor(const unsigned int& _corr_idx) @@ -96,170 +89,160 @@ void SolverManager::removeFactor(const unsigned int& _corr_idx) void SolverManager::addStateUnit(StateBlockPtr _st_ptr) { - //std::cout << "Adding State Unit " << _st_ptr->id() << std::endl; - //_st_ptr->print(); - - switch (_st_ptr->getStateType()) - { - case ST_COMPLEX_ANGLE: - { - // TODO - //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl; - //ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization); - break; - } - case ST_THETA: - { - //std::cout << "No Local Parametrization to be added" << std::endl; - ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr); - break; - } - case ST_POINT_1d: - { - //std::cout << "No Local Parametrization to be added" << std::endl; - ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1d*)_st_ptr)->BLOCK_SIZE, nullptr); - break; - } - case ST_VECTOR: - { - //std::cout << "No Local Parametrization to be added" << std::endl; - ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr); - break; - } - case ST_POINT_3d: - { - //std::cout << "No Local Parametrization to be added" << std::endl; - ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr); - break; - } - default: - std::cout << "Unknown Local Parametrization type!" << std::endl; - } - if (_st_ptr->isFixed()) - updateStateUnitStatus(_st_ptr); - - _st_ptr->setPendingStatus(NOT_PENDING); + // std::cout << "Adding State Unit " << _st_ptr->id() << std::endl; + //_st_ptr->print(); + + switch (_st_ptr->getStateType()) + { + case ST_COMPLEX_ANGLE: { + // TODO + // std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl; + // ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new + // ComplexAngleParameterization); + break; + } + case ST_THETA: { + // std::cout << "No Local Parametrization to be added" << std::endl; + ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr); + break; + } + case ST_POINT_1d: { + // std::cout << "No Local Parametrization to be added" << std::endl; + ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1d*)_st_ptr)->BLOCK_SIZE, nullptr); + break; + } + case ST_VECTOR: { + // std::cout << "No Local Parametrization to be added" << std::endl; + ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr); + break; + } + case ST_POINT_3d: { + // std::cout << "No Local Parametrization to be added" << std::endl; + ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr); + break; + } + default: + std::cout << "Unknown Local Parametrization type!" << std::endl; + } + if (_st_ptr->isFixed()) updateStateUnitStatus(_st_ptr); + + _st_ptr->setPendingStatus(NOT_PENDING); } void SolverManager::removeAllStateUnits() { - std::vector<double*> parameter_blocks; + std::vector<double*> parameter_blocks; - ceres_problem_->GetParameterBlocks(¶meter_blocks); + ceres_problem_->GetParameterBlocks(¶meter_blocks); - for (unsigned int i = 0; i< parameter_blocks.size(); i++) - ceres_problem_->RemoveParameterBlock(parameter_blocks[i]); + for (unsigned int i = 0; i < parameter_blocks.size(); i++) + ceres_problem_->RemoveParameterBlock(parameter_blocks[i]); } void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr) { // TODO -// if (!_st_ptr->isFixed()) -// ceres_problem_->SetParameterBlockVariable(_st_ptr->get()); -// else if (_st_ptr->isFixed()) -// ceres_problem_->SetParameterBlockConstant(_st_ptr->get()); -// else -// printf("\nERROR: Update state unit status with unknown status"); -// -// _st_ptr->setPendingStatus(NOT_PENDING); + // if (!_st_ptr->isFixed()) + // ceres_problem_->SetParameterBlockVariable(_st_ptr->get()); + // else if (_st_ptr->isFixed()) + // ceres_problem_->SetParameterBlockConstant(_st_ptr->get()); + // else + // printf("\nERROR: Update state unit status with unknown status"); + // + // _st_ptr->setPendingStatus(NOT_PENDING); } ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr) { - //std::cout << "adding ctr " << _corrPtr->id() << std::endl; - //_corrPtr->print(); - - switch (_corrPtr->getFactorType()) - { - case FAC_GPS_FIX_2d: - { - FactorGPS2d* specific_ptr = (FactorGPS2d*)(_corrPtr); - return new ceres::AutoDiffCostFunction<FactorGPS2d, - specific_ptr->residualSize, - specific_ptr->block0Size, - specific_ptr->block1Size, - specific_ptr->block2Size, - specific_ptr->block3Size, - specific_ptr->block4Size, - specific_ptr->block5Size, - specific_ptr->block6Size, - specific_ptr->block7Size, - specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - case FAC_ODOM_2d_COMPLEX_ANGLE: - { - FactorOdom2dComplexAngle* specific_ptr = (FactorOdom2dComplexAngle*)(_corrPtr); - return new ceres::AutoDiffCostFunction<FactorOdom2dComplexAngle, - specific_ptr->residualSize, - specific_ptr->block0Size, - specific_ptr->block1Size, - specific_ptr->block2Size, - specific_ptr->block3Size, - specific_ptr->block4Size, - specific_ptr->block5Size, - specific_ptr->block6Size, - specific_ptr->block7Size, - specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - case FAC_ODOM_2d: - { - FactorOdom2d* specific_ptr = (FactorOdom2d*)(_corrPtr); - return new ceres::AutoDiffCostFunction<FactorOdom2d, - specific_ptr->residualSize, - specific_ptr->block0Size, - specific_ptr->block1Size, - specific_ptr->block2Size, - specific_ptr->block3Size, - specific_ptr->block4Size, - specific_ptr->block5Size, - specific_ptr->block6Size, - specific_ptr->block7Size, - specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - case FAC_CORNER_2d: - { - FactorCorner2d* specific_ptr = (FactorCorner2d*)(_corrPtr); - return new ceres::AutoDiffCostFunction<FactorCorner2d, - specific_ptr->residualSize, - specific_ptr->block0Size, - specific_ptr->block1Size, - specific_ptr->block2Size, - specific_ptr->block3Size, - specific_ptr->block4Size, - specific_ptr->block5Size, - specific_ptr->block6Size, - specific_ptr->block7Size, - specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - case FAC_IMU: - { - FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr); - return new ceres::AutoDiffCostFunction<FactorIMU, - specific_ptr->residualSize, - specific_ptr->block0Size, - specific_ptr->block1Size, - specific_ptr->block2Size, - specific_ptr->block3Size, - specific_ptr->block4Size, - specific_ptr->block5Size, - specific_ptr->block6Size, - specific_ptr->block7Size, - specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - default: - std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl; - - return nullptr; - } + // std::cout << "adding ctr " << _corrPtr->id() << std::endl; + //_corrPtr->print(); + + switch (_corrPtr->getFactorType()) + { + case FAC_GPS_FIX_2d: { + FactorGPS2d* specific_ptr = (FactorGPS2d*)(_corrPtr); + return new ceres::AutoDiffCostFunction<FactorGPS2d, + specific_ptr->residualSize, + specific_ptr->block0Size, + specific_ptr->block1Size, + specific_ptr->block2Size, + specific_ptr->block3Size, + specific_ptr->block4Size, + specific_ptr->block5Size, + specific_ptr->block6Size, + specific_ptr->block7Size, + specific_ptr->block8Size, + specific_ptr->block9Size>(specific_ptr); + break; + } + case FAC_ODOM_2d_COMPLEX_ANGLE: { + FactorOdom2dComplexAngle* specific_ptr = (FactorOdom2dComplexAngle*)(_corrPtr); + return new ceres::AutoDiffCostFunction<FactorOdom2dComplexAngle, + specific_ptr->residualSize, + specific_ptr->block0Size, + specific_ptr->block1Size, + specific_ptr->block2Size, + specific_ptr->block3Size, + specific_ptr->block4Size, + specific_ptr->block5Size, + specific_ptr->block6Size, + specific_ptr->block7Size, + specific_ptr->block8Size, + specific_ptr->block9Size>(specific_ptr); + break; + } + case FAC_ODOM_2d: { + FactorOdom2d* specific_ptr = (FactorOdom2d*)(_corrPtr); + return new ceres::AutoDiffCostFunction<FactorOdom2d, + specific_ptr->residualSize, + specific_ptr->block0Size, + specific_ptr->block1Size, + specific_ptr->block2Size, + specific_ptr->block3Size, + specific_ptr->block4Size, + specific_ptr->block5Size, + specific_ptr->block6Size, + specific_ptr->block7Size, + specific_ptr->block8Size, + specific_ptr->block9Size>(specific_ptr); + break; + } + case FAC_CORNER_2d: { + FactorCorner2d* specific_ptr = (FactorCorner2d*)(_corrPtr); + return new ceres::AutoDiffCostFunction<FactorCorner2d, + specific_ptr->residualSize, + specific_ptr->block0Size, + specific_ptr->block1Size, + specific_ptr->block2Size, + specific_ptr->block3Size, + specific_ptr->block4Size, + specific_ptr->block5Size, + specific_ptr->block6Size, + specific_ptr->block7Size, + specific_ptr->block8Size, + specific_ptr->block9Size>(specific_ptr); + break; + } + case FAC_IMU: { + FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr); + return new ceres::AutoDiffCostFunction<FactorIMU, + specific_ptr->residualSize, + specific_ptr->block0Size, + specific_ptr->block1Size, + specific_ptr->block2Size, + specific_ptr->block3Size, + specific_ptr->block4Size, + specific_ptr->block5Size, + specific_ptr->block6Size, + specific_ptr->block7Size, + specific_ptr->block8Size, + specific_ptr->block9Size>(specific_ptr); + break; + } + default: + std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl; + + return nullptr; + } } diff --git a/src/state_block/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp index 9c5b8b92ca5d10e56779482fa3a2a1cff52819ba..ac00ac4943755ce6d8ba960631936945a85d8445 100644 --- a/src/state_block/local_parametrization_base.cpp +++ b/src/state_block/local_parametrization_base.cpp @@ -20,17 +20,15 @@ #include "core/state_block/local_parametrization_base.h" -namespace wolf { - -LocalParametrizationBase::LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size) : - global_size_(_global_size), local_size_(_local_size) +namespace wolf { -} - -LocalParametrizationBase::~LocalParametrizationBase() +LocalParametrizationBase::LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size) + : global_size_(_global_size), local_size_(_local_size) { } +LocalParametrizationBase::~LocalParametrizationBase() {} + unsigned int LocalParametrizationBase::getLocalSize() const { return local_size_; @@ -41,15 +39,15 @@ unsigned int LocalParametrizationBase::getGlobalSize() const return global_size_; } -} // namespace wolf +} // namespace wolf bool wolf::LocalParametrizationBase::plus(const Eigen::VectorXd &_x, const Eigen::VectorXd &_delta, - Eigen::VectorXd &_x_plus_delta) const + Eigen::VectorXd & _x_plus_delta) const { Eigen::Map<const Eigen::VectorXd> x(_x.data(), _x.size()); Eigen::Map<const Eigen::VectorXd> delta(_delta.data(), _delta.size()); - Eigen::Map<Eigen::VectorXd> x_plus_delta(_x_plus_delta.data(), _x_plus_delta.size()); + Eigen::Map<Eigen::VectorXd> x_plus_delta(_x_plus_delta.data(), _x_plus_delta.size()); return plus(x, delta, x_plus_delta); } diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp index 30b9eb046bce356967437d2fb7f314354ad50648..cc501ff09db5c0ef5a151ce7ce85eb7cf04b07d6 100644 --- a/src/state_block/local_parametrization_homogeneous.cpp +++ b/src/state_block/local_parametrization_homogeneous.cpp @@ -20,12 +20,11 @@ #include "core/state_block/local_parametrization_homogeneous.h" #include "iostream" -#include "core/math/rotations.h" // we use quaternion algebra here +#include "core/math/rotations.h" // we use quaternion algebra here -namespace wolf { - -LocalParametrizationHomogeneous::LocalParametrizationHomogeneous() : - LocalParametrizationBase(4, 3) +namespace wolf +{ +LocalParametrizationHomogeneous::LocalParametrizationHomogeneous() : LocalParametrizationBase(4, 3) { // } @@ -37,7 +36,7 @@ LocalParametrizationHomogeneous::~LocalParametrizationHomogeneous() bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta, - Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const + Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const { assert(_h.size() == global_size_ && "Wrong size of input homogeneous point."); assert(_delta.size() == local_size_ && "Wrong size of input delta."); @@ -45,28 +44,25 @@ bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXd>& _h using namespace Eigen; - _h_plus_delta = ( exp_q(_delta) * Quaterniond(_h.data()) ).coeffs(); + _h_plus_delta = (exp_q(_delta) * Quaterniond(_h.data())).coeffs(); return true; } bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, - Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const { assert(_h.size() == global_size_ && "Wrong size of input quaternion."); assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix."); - Eigen::Vector4d hh = _h/2; - _jacobian << hh(3), hh(2), -hh(1), - -hh(2), hh(3), hh(0), - hh(1), -hh(0), hh(3), - -hh(0), -hh(1), -hh(2) ; + Eigen::Vector4d hh = _h / 2; + _jacobian << hh(3), hh(2), -hh(1), -hh(2), hh(3), hh(0), hh(1), -hh(0), hh(3), -hh(0), -hh(1), -hh(2); return true; } bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _h1, Eigen::Map<const Eigen::VectorXd>& _h2, - Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) + Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) { using Eigen::Quaterniond; _h2_minus_h1 = log_q(Quaterniond(_h2.data()) * Quaterniond(_h1.data()).conjugate()); @@ -77,5 +73,4 @@ bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state, dou { return _state.size() == global_size_; } -} // namespace wolf - +} // namespace wolf diff --git a/src/state_block/local_parametrization_quaternion.cpp b/src/state_block/local_parametrization_quaternion.cpp index ae2a948f62a68cc9b541d3e4236ca76436f782cf..4cf32c40b671b5b8d46aadbfbba980d0a39e70b8 100644 --- a/src/state_block/local_parametrization_quaternion.cpp +++ b/src/state_block/local_parametrization_quaternion.cpp @@ -22,16 +22,15 @@ #include "core/math/rotations.h" #include <iostream> -namespace wolf { - +namespace wolf +{ ////////// LOCAL PERTURBATION ////////////// template <> bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q, Eigen::Map<const Eigen::VectorXd>& _delta_theta, - Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const + Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const { - assert(_q.size() == global_size_ && "Wrong size of input quaternion."); assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta."); assert(_q_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion."); @@ -40,23 +39,20 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::Vect using namespace Eigen; - _q_plus_delta_theta = ( Quaterniond(_q.data()) * exp_q(_delta_theta) ).coeffs(); + _q_plus_delta_theta = (Quaterniond(_q.data()) * exp_q(_delta_theta)).coeffs(); return true; } template <> bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const { assert(_q.size() == global_size_ && "Wrong size of input quaternion."); assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix."); - Eigen::Vector4d qq = _q/2; - _jacobian << qq(3), -qq(2), qq(1), - qq(2), qq(3), -qq(0), - -qq(1), qq(0), qq(3), - -qq(0), -qq(1), -qq(2) ; + Eigen::Vector4d qq = _q / 2; + _jacobian << qq(3), -qq(2), qq(1), qq(2), qq(3), -qq(0), -qq(1), qq(0), qq(3), -qq(0), -qq(1), -qq(2); return true; } @@ -64,7 +60,7 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const template <> bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1, Eigen::Map<const Eigen::VectorXd>& _q2, - Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) + Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) { assert(_q1.size() == global_size_ && "Wrong size of input quaternion."); assert(_q2.size() == global_size_ && "Wrong size of input quaternion."); @@ -81,9 +77,8 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::Vec template <> bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q, Eigen::Map<const Eigen::VectorXd>& _delta_theta, - Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const + Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const { - assert(_q.size() == global_size_ && "Wrong size of input quaternion."); assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta."); assert(_q_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion."); @@ -92,23 +87,20 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::Vec using namespace Eigen; - _q_plus_delta_theta = ( exp_q(_delta_theta) * Quaterniond(_q.data()) ).coeffs(); + _q_plus_delta_theta = (exp_q(_delta_theta) * Quaterniond(_q.data())).coeffs(); return true; } template <> bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const { assert(_q.size() == global_size_ && "Wrong size of input quaternion."); assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix."); - Eigen::Vector4d qq = _q/2; - _jacobian << qq(3), qq(2), -qq(1), - -qq(2), qq(3), qq(0), - qq(1), -qq(0), qq(3), - -qq(0), -qq(1), -qq(2); + Eigen::Vector4d qq = _q / 2; + _jacobian << qq(3), qq(2), -qq(1), -qq(2), qq(3), qq(0), qq(1), -qq(0), qq(3), -qq(0), -qq(1), -qq(2); return true; } @@ -116,7 +108,7 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const template <> bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1, Eigen::Map<const Eigen::VectorXd>& _q2, - Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) + Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) { assert(_q1.size() == global_size_ && "Wrong size of input quaternion."); assert(_q2.size() == global_size_ && "Wrong size of input quaternion."); @@ -128,4 +120,4 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::Ve return true; } -} // namespace wolf +} // namespace wolf diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp index 9765c619e6c146ce3e6a9c4774003b6edfffeaaa..c6642a3925d32b3a775eabb73020a0ca10419d4f 100644 --- a/src/state_block/state_block.cpp +++ b/src/state_block/state_block.cpp @@ -21,26 +21,23 @@ #include "core/state_block/state_block.h" namespace wolf { - void StateBlock::setState(const Eigen::VectorXd& _state, const bool _notify) { assert(_state.size() == state_.size()); { std::lock_guard<std::mutex> lock(mut_state_); - state_ = _state; + state_ = _state; state_size_ = state_.size(); } // Flag - if (_notify) - state_updated_.store(true); + if (_notify) state_updated_.store(true); } void StateBlock::setFixed(bool _fixed) { // Flag - if (fixed_.load() != _fixed) - fix_updated_.store(true); + if (fixed_.load() != _fixed) fix_updated_.store(true); // set fixed_.store(_fixed); @@ -53,7 +50,7 @@ void StateBlock::perturb(double amplitude) this->plus(perturbation); } -void StateBlock::plus(const Eigen::VectorXd &_dv) +void StateBlock::plus(const Eigen::VectorXd& _dv) { if (local_param_ptr_ == nullptr) setState(getState() + _dv); @@ -65,15 +62,16 @@ void StateBlock::plus(const Eigen::VectorXd &_dv) } } -} +} // namespace wolf #include "core/state_block/factory_state_block.h" #include "core/state_block/state_quaternion.h" #include "core/state_block/state_angle.h" #include "core/state_block/state_homogeneous_3d.h" -namespace wolf{ +namespace wolf +{ WOLF_REGISTER_STATEBLOCK(StateQuaternion); WOLF_REGISTER_STATEBLOCK(StateAngle); WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d); -} +} // namespace wolf diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp index e2c3ff10e2a4b8f41aeb35904b35fd710426f5f3..99e8f61cd283ba3771b261ea243e37873dd8bf81 100644 --- a/src/state_block/state_block_derived.cpp +++ b/src/state_block/state_block_derived.cpp @@ -23,7 +23,6 @@ namespace wolf { - WOLF_REGISTER_STATEBLOCK(StatePoint2d); WOLF_REGISTER_STATEBLOCK(StateVector2d); WOLF_REGISTER_STATEBLOCK(StatePoint3d); diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index 5b9d96728e4588b9cd00651895752bfb839277bb..4a1be6f2691a47696529db338230ca64e2466bbc 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -22,13 +22,9 @@ #include "core/frame/frame_base.h" #include <iterator> -namespace wolf { - -TrajectoryBase::TrajectoryBase() : - NodeBase("TRAJECTORY", "TrajectoryBase"), - frame_map_() +namespace wolf { -} +TrajectoryBase::TrajectoryBase() : NodeBase("TRAJECTORY", "TrajectoryBase"), frame_map_() {} TrajectoryBase::~TrajectoryBase() { @@ -38,7 +34,8 @@ TrajectoryBase::~TrajectoryBase() FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) { // add to list - assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one"); + assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && + "Trying to add a keyframe with the same timestamp of an existing one"); frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr); @@ -50,44 +47,39 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr) frame_map_.erase(_frame_ptr->getTimeStamp()); } -void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const +void TrajectoryBase::getFactorList(FactorBaseConstPtrList& _fac_list) const { - for(auto fr_pair: frame_map_) - if (not fr_pair.second->isRemoving()) - fr_pair.second->getFactorList(_fac_list); + for (auto fr_pair : frame_map_) + if (not fr_pair.second->isRemoving()) fr_pair.second->getFactorList(_fac_list); } -void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) +void TrajectoryBase::getFactorList(FactorBasePtrList& _fac_list) { - for(auto fr_pair: frame_map_) - if (not fr_pair.second->isRemoving()) - fr_pair.second->getFactorList(_fac_list); + for (auto fr_pair : frame_map_) + if (not fr_pair.second->isRemoving()) fr_pair.second->getFactorList(_fac_list); } TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) const { - //If frame_map_ is empty then closestFrameToTimeStamp is meaningless - if(frame_map_.empty()) - return TimeStamp::Invalid(); + // If frame_map_ is empty then closestFrameToTimeStamp is meaningless + if (frame_map_.empty()) return TimeStamp::Invalid(); // Lower bound provides the first iterator that does not go before ts (maybe equal or greater) auto lower_bound = frame_map_.lower_bound(_ts); // If first frame does not go before ts, it is the closest one - if ( lower_bound == frame_map_.begin()) - return frame_map_.begin()->first; + if (lower_bound == frame_map_.begin()) return frame_map_.begin()->first; // If last frame goes before ts, it is the closest one as well - if ( lower_bound == frame_map_.end()) - return frame_map_.rbegin()->first; + if (lower_bound == frame_map_.end()) return frame_map_.rbegin()->first; // Otherwise we have to compare lb and its previous auto upper_bound = lower_bound; - lower_bound = std::prev(lower_bound); - + lower_bound = std::prev(lower_bound); + auto lb_dist = fabs(lower_bound->first - _ts); auto ub_dist = fabs(upper_bound->first - _ts); - if(lb_dist < ub_dist) + if (lb_dist < ub_dist) return lower_bound->first; else return upper_bound->first; @@ -102,8 +94,7 @@ FrameBaseConstPtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0); - if (closest_ts.ok()) - return frame_map_.at(closest_ts); + if (closest_ts.ok()) return frame_map_.at(closest_ts); return nullptr; } @@ -114,8 +105,7 @@ FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0); - if (closest_ts.ok()) - return frame_map_.at(closest_ts); + if (closest_ts.ok()) return frame_map_.at(closest_ts); return nullptr; } @@ -124,57 +114,59 @@ FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame, const u { auto frame_it = frame_map_.find(_frame->getTimeStamp()); WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!"); - - if (frame_it == frame_map_.end() or - std::distance(frame_it, frame_map_.end()) <= i) - return nullptr; - return std::next(frame_it,i)->second; + if (frame_it == frame_map_.end() or std::distance(frame_it, frame_map_.end()) <= i) return nullptr; + + return std::next(frame_it, i)->second; } FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame, const unsigned int& i) { auto frame_it = frame_map_.find(_frame->getTimeStamp()); WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!"); - - if (frame_it == frame_map_.end() or - std::distance(frame_it, frame_map_.end()) <= i) - return nullptr; - return std::next(frame_it,i)->second; + if (frame_it == frame_map_.end() or std::distance(frame_it, frame_map_.end()) <= i) return nullptr; + + return std::next(frame_it, i)->second; } FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame, const unsigned int& i) const { auto frame_it = frame_map_.find(_frame->getTimeStamp()); WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!"); - - if (frame_it == frame_map_.end() or - std::distance(frame_map_.begin(), frame_it) < i) - return nullptr; - return std::prev(frame_it,i)->second; + if (frame_it == frame_map_.end() or std::distance(frame_map_.begin(), frame_it) < i) return nullptr; + + return std::prev(frame_it, i)->second; } FrameBasePtr TrajectoryBase::getPreviousFrame(FrameBasePtr _frame, const unsigned int& i) { auto frame_it = frame_map_.find(_frame->getTimeStamp()); WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!"); - - if (frame_it == frame_map_.end() or - std::distance(frame_map_.begin(), frame_it) < i) - return nullptr; - return std::prev(frame_it,i)->second; -} + if (frame_it == frame_map_.end() or std::distance(frame_map_.begin(), frame_it) < i) return nullptr; + return std::prev(frame_it, i)->second; +} -void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _factored_by, std::ostream& _stream, std::string _tabs) const +void TrajectoryBase::printHeader(int _depth, + bool _metric, + bool _state_blocks, + bool _factored_by, + std::ostream& _stream, + std::string _tabs) const { - _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl; + _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") + << std::endl; } -void TrajectoryBase::print(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const +void TrajectoryBase::print(int _depth, + bool _factored_by, + bool _metric, + bool _state_blocks, + std::ostream& _stream, + std::string _tabs) const { printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs); if (_depth >= 1) @@ -185,7 +177,7 @@ void TrajectoryBase::print(int _depth, bool _factored_by, bool _metric, bool _st CheckLog TrajectoryBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const { - CheckLog log; + CheckLog log; std::stringstream inconsistency_explanation; if (_verbose) @@ -194,8 +186,9 @@ CheckLog TrajectoryBase::localCheck(bool _verbose, std::ostream& _stream, std::s } // check pointer to Problem - inconsistency_explanation << "Trj->getProblem() [" << getProblem() - << "] -> " << " Prb->getTrajectory() [" << getProblem()->getTrajectory() << "] -> Trj [" << this << "] Mismatch!\n"; + inconsistency_explanation << "Trj->getProblem() [" << getProblem() << "] -> " + << " Prb->getTrajectory() [" << getProblem()->getTrajectory() << "] -> Trj [" << this + << "] Mismatch!\n"; log.assertTrue((getProblem()->getTrajectory() == shared_from_this()), inconsistency_explanation); return log; } @@ -205,8 +198,7 @@ bool TrajectoryBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, auto local_log = localCheck(_verbose, _stream, _tabs); _log.compose(local_log); for (auto F_pair : getFrameMap()) - if (F_pair.second) - F_pair.second->check(_log, _verbose, _stream, _tabs + " "); + if (F_pair.second) F_pair.second->check(_log, _verbose, _stream, _tabs + " "); return _log.is_consistent_; } -} // namespace wolf +} // namespace wolf diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp index 1f64403e997db99abc927e3b1ba5115e822f841f..cf3d61af8edd503cdccf62d5ec8b86adbb878bb1 100644 --- a/src/tree_manager/tree_manager_sliding_window.cpp +++ b/src/tree_manager/tree_manager_sliding_window.cpp @@ -22,25 +22,21 @@ namespace wolf { - void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame) { - int n_f = getProblem()->getTrajectory()->size(); + int n_f = getProblem()->getTrajectory()->size(); bool remove_first = (n_f > params_sw_->n_frames); - int n_fix = (n_f >= params_sw_->n_frames ? - params_sw_->n_fix_first_frames : - n_f - (params_sw_->n_frames - params_sw_->n_fix_first_frames)); + int n_fix = (n_f >= params_sw_->n_frames ? params_sw_->n_fix_first_frames + : n_f - (params_sw_->n_frames - params_sw_->n_fix_first_frames)); - auto frame = (remove_first ? - getProblem()->getTrajectory()->getFirstFrame()->getNextFrame() : - getProblem()->getTrajectory()->getFirstFrame()); - int fixed_frames = 0; + auto frame = (remove_first ? getProblem()->getTrajectory()->getFirstFrame()->getNextFrame() + : getProblem()->getTrajectory()->getFirstFrame()); + int fixed_frames = 0; // Fix n_fix first frames while (fixed_frames < n_fix) { - if (not frame) - break; + if (not frame) break; if (not frame->isFixed()) { WOLF_DEBUG("TreeManagerSlidingWindow fixing frame ", frame->id()); @@ -61,5 +57,4 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame) // Register in the FactoryTreeManager WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindow); -} // namespace wolf - +} // namespace wolf diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp index eb8567761c07658a6289260f16907553ffc4bed1..3a7cc6491075b5648b2ca4d65e23c07413a11399 100644 --- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp +++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp @@ -24,29 +24,26 @@ namespace wolf { - -TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) : - TreeManagerSlidingWindow(_params), - params_swdr_(_params), - count_frames_(0) +TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) + : TreeManagerSlidingWindow(_params), params_swdr_(_params), count_frames_(0) { NodeBase::setType("TreeManagerSlidingWindowDualRate"); } void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame) { - int n_f = getProblem()->getTrajectory()->size(); // in trajectory there are only key frames + int n_f = getProblem()->getTrajectory()->size(); // in trajectory there are only key frames // recent segment not complete - if (n_f <= params_swdr_->n_frames_recent) - return; + if (n_f <= params_swdr_->n_frames_recent) return; // REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames if (count_frames_ != 0) { WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames"); - FrameBasePtr remove_recent_frame = getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(params_swdr_->n_frames_recent); - FrameBasePtr keep_recent_frame = remove_recent_frame->getNextFrame(); + FrameBasePtr remove_recent_frame = + getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(params_swdr_->n_frames_recent); + FrameBasePtr keep_recent_frame = remove_recent_frame->getNextFrame(); // compose motion captures for all processors motion for (auto motion_provider_pair : getProblem()->getMotionProviderMap()) @@ -61,8 +58,10 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame) continue; } - auto cap_prev = std::dynamic_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor())); - auto cap_next = std::dynamic_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor())); + auto cap_prev = + std::dynamic_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor())); + auto cap_next = + std::dynamic_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor())); // merge captures (if exist) if (cap_prev and cap_next) @@ -82,12 +81,10 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame) // iterate counter count_frames_++; - if (count_frames_ == params_swdr_->rate_old_frames) - count_frames_ = 0; + if (count_frames_ == params_swdr_->rate_old_frames) count_frames_ = 0; } // Register in the FactoryTreeManager WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowDualRate); -} // namespace wolf - +} // namespace wolf diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp index cf48d302a5688d907721f493b99562e630bdb506..05fa8e86eccc379eb9b81b1064178cb1ef46aab8 100644 --- a/src/utils/graph_search.cpp +++ b/src/utils/graph_search.cpp @@ -22,35 +22,28 @@ using namespace wolf; -GraphSearch::GraphSearch() : - parents_() -{ - -} +GraphSearch::GraphSearch() : parents_() {} -GraphSearch::~GraphSearch() -{ - -} +GraphSearch::~GraphSearch() {} FactorBasePtrList GraphSearch::computeShortestPath(NodeStateBlocksPtr node1, NodeStateBlocksPtr node2, const unsigned int max_graph_dist) { - //WOLF_INFO("GraphSearch::computeShortestPath: from frame ", node1->id(), " to frame ", node2->id()); + // WOLF_INFO("GraphSearch::computeShortestPath: from frame ", node1->id(), " to frame ", node2->id()); std::set<NodeStateBlocksPtr> node_neigs({node1}); - parents_[node1] = std::pair<FactorBasePtr,NodeStateBlocksPtr>(nullptr, nullptr); + parents_[node1] = std::pair<FactorBasePtr, NodeStateBlocksPtr>(nullptr, nullptr); unsigned int depth = 0; - //WOLF_INFO(node1->id()); + // WOLF_INFO(node1->id()); while (not node_neigs.empty()) { node_neigs = getNeighborFrames(node_neigs); depth++; - //if (not node_neigs.empty()) + // if (not node_neigs.empty()) //{ // std::string node_neigs_str(depth, '.'); // for (auto node : node_neigs) @@ -61,11 +54,11 @@ FactorBasePtrList GraphSearch::computeShortestPath(NodeStateBlocksPtr node1, // finish if (node_neigs.count(node2) != 0) { - //WOLF_INFO("Frame ", node2->id(), " found!"); + // WOLF_INFO("Frame ", node2->id(), " found!"); assert(parents_.count(node2) != 0); FactorBasePtrList factor_path; - auto node_it = node2; + auto node_it = node2; while (node_it != node1) { @@ -77,10 +70,9 @@ FactorBasePtrList GraphSearch::computeShortestPath(NodeStateBlocksPtr node1, } // stop - if (max_graph_dist > 0 and depth == max_graph_dist) - break; + if (max_graph_dist > 0 and depth == max_graph_dist) break; } - //WOLF_INFO("Path to frame ", node2->id(), " NOT found!"); + // WOLF_INFO("Path to frame ", node2->id(), " NOT found!"); return FactorBasePtrList(); } @@ -97,18 +89,17 @@ std::set<NodeStateBlocksPtr> GraphSearch::getNeighborFrames(const std::set<NodeS // Iterate over all factors_by for (auto fac_by : facs_by) { - //WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id()); - //WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id()); + // WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id()); + // WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id()); for (auto node_other_w : fac_by->getNodesFactored()) { auto node_other = node_other_w.lock(); - //WOLF_INFO_COND(node_other, "node_other ", node_other->id()); - if (node_other != node and - parents_.count(node_other) == 0) + // WOLF_INFO_COND(node_other, "node_other ", node_other->id()); + if (node_other != node and parents_.count(node_other) == 0) { - //WOLF_INFO("registering"); + // WOLF_INFO("registering"); node_neigs.insert(node_other); - parents_[node_other] = std::pair<FactorBasePtr,NodeStateBlocksPtr>(fac_by, node); + parents_[node_other] = std::pair<FactorBasePtr, NodeStateBlocksPtr>(fac_by, node); } } } @@ -116,4 +107,3 @@ std::set<NodeStateBlocksPtr> GraphSearch::getNeighborFrames(const std::set<NodeS return node_neigs; } - diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp index 56f251465804a2af71b7737e5e3222281f8fe49b..44dfffcd359d897f7d15ce7c9db38bf3efb289ae 100644 --- a/src/utils/loader.cpp +++ b/src/utils/loader.cpp @@ -23,7 +23,6 @@ namespace wolf { - Loader::Loader(int _flags) { flags_ = _flags; diff --git a/test/dummy/dummy_object.h b/test/dummy/dummy_object.h index 7dbb1d6b6571153018df9e09aeb4df78f9da2f15..07a2983dc04f2ab460cf676d20006928c0d84aa3 100644 --- a/test/dummy/dummy_object.h +++ b/test/dummy/dummy_object.h @@ -26,7 +26,8 @@ #include "core/common/wolf.h" #include "factory_dummy_object.h" -namespace wolf { +namespace wolf +{ WOLF_PTR_TYPEDEFS(DummyObject); /* @@ -41,36 +42,34 @@ WOLF_PTR_TYPEDEFS(DummyObject); * DummyObjectClass(const std::string& _unique_name, * const ParamsServer& _server); */ -#define WOLF_DUMMY_OBJECT_CREATE(DummyObjectClass) \ -static DummyObjectPtr create(const std::string& _unique_name) \ -{ \ - DummyObjectPtr sub = std::make_shared<DummyObjectClass>(_unique_name); \ - return sub; \ -} \ +#define WOLF_DUMMY_OBJECT_CREATE(DummyObjectClass) \ + static DummyObjectPtr create(const std::string& _unique_name) \ + { \ + DummyObjectPtr sub = std::make_shared<DummyObjectClass>(_unique_name); \ + return sub; \ + } class DummyObject { - protected: - //wolf - std::string prefix_; - std::string name_; - std::string topic_; + protected: + // wolf + std::string prefix_; + std::string name_; + std::string topic_; - public: - DummyObject(const std::string& _unique_name) : - prefix_("ROS DummyObject/" + _unique_name), - name_(_unique_name) - { - //topic_ = _server.getParam<std::string>(prefix_ + "/topic"); - } + public: + DummyObject(const std::string& _unique_name) : prefix_("ROS DummyObject/" + _unique_name), name_(_unique_name) + { + // topic_ = _server.getParam<std::string>(prefix_ + "/topic"); + } - virtual ~DummyObject(){}; + virtual ~DummyObject(){}; - //std::string getTopic() const; + // std::string getTopic() const; - std::string getName() const; + std::string getName() const; - virtual void print() const = 0; + virtual void print() const = 0; }; inline std::string DummyObject::getName() const @@ -78,4 +77,4 @@ inline std::string DummyObject::getName() const return name_; } -} +} // namespace wolf diff --git a/test/dummy/dummy_object_derived.cpp b/test/dummy/dummy_object_derived.cpp index 02c690931496ad38ddd3e1099cb7a236d601b3aa..f5b3e10c0e481da5c81bc881b2badda7ffee734b 100644 --- a/test/dummy/dummy_object_derived.cpp +++ b/test/dummy/dummy_object_derived.cpp @@ -22,9 +22,9 @@ namespace wolf { - -DummyObjectDerived::DummyObjectDerived(const std::string& _unique_name) ://, - DummyObject(_unique_name) +DummyObjectDerived::DummyObjectDerived(const std::string& _unique_name) + : //, + DummyObject(_unique_name) { } @@ -35,4 +35,4 @@ void DummyObjectDerived::print() const WOLF_REGISTER_DUMMY_OBJECT(DummyObjectDerived) -} +} // namespace wolf diff --git a/test/dummy/dummy_object_derived.h b/test/dummy/dummy_object_derived.h index 3be8a35d9a7ce0c41b4721e956c4ff48a21a9abf..90640c4ecdc5a50c099ab435ad1c1a38beda127a 100644 --- a/test/dummy/dummy_object_derived.h +++ b/test/dummy/dummy_object_derived.h @@ -27,17 +27,17 @@ #include "dummy_object.h" -namespace wolf { - +namespace wolf +{ class DummyObjectDerived : public DummyObject { - public: - DummyObjectDerived(const std::string& _unique_name); - WOLF_DUMMY_OBJECT_CREATE(DummyObjectDerived); - - void print() const override; + public: + DummyObjectDerived(const std::string& _unique_name); + WOLF_DUMMY_OBJECT_CREATE(DummyObjectDerived); + + void print() const override; - virtual ~DummyObjectDerived(){}; + virtual ~DummyObjectDerived(){}; }; -} +} // namespace wolf diff --git a/test/dummy/dummy_object_derived_derived.cpp b/test/dummy/dummy_object_derived_derived.cpp index 004176952d00d9e3f4481aa5ad356b9b24823922..516bf6b60a20b32a49cd1c4f7cd09e62c2a44145 100644 --- a/test/dummy/dummy_object_derived_derived.cpp +++ b/test/dummy/dummy_object_derived_derived.cpp @@ -22,9 +22,8 @@ namespace wolf { - -DummyObjectDerivedDerived::DummyObjectDerivedDerived(const std::string& _unique_name) : - DummyObjectDerived(_unique_name) +DummyObjectDerivedDerived::DummyObjectDerivedDerived(const std::string& _unique_name) + : DummyObjectDerived(_unique_name) { } @@ -34,4 +33,4 @@ void DummyObjectDerivedDerived::print() const } WOLF_REGISTER_DUMMY_OBJECT(DummyObjectDerivedDerived) -} +} // namespace wolf diff --git a/test/dummy/dummy_object_derived_derived.h b/test/dummy/dummy_object_derived_derived.h index ba58696fc91f8d9905c052673cd78c028e9e30f5..8ba7a5a7f11af32988be590240436644f2cd93d9 100644 --- a/test/dummy/dummy_object_derived_derived.h +++ b/test/dummy/dummy_object_derived_derived.h @@ -27,17 +27,17 @@ #include "dummy_object_derived.h" -namespace wolf { - +namespace wolf +{ class DummyObjectDerivedDerived : public DummyObjectDerived { - public: - DummyObjectDerivedDerived(const std::string& _unique_name); - WOLF_DUMMY_OBJECT_CREATE(DummyObjectDerivedDerived); - - void print() const override; + public: + DummyObjectDerivedDerived(const std::string& _unique_name); + WOLF_DUMMY_OBJECT_CREATE(DummyObjectDerivedDerived); + + void print() const override; - virtual ~DummyObjectDerivedDerived(){}; + virtual ~DummyObjectDerivedDerived(){}; }; -} +} // namespace wolf diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h index 4bd1853b0812a1f1fc33ed36ff26485c0070939a..033d694dd9835168187e43be89a228fbfec064da 100644 --- a/test/dummy/factor_dummy_zero_1.h +++ b/test/dummy/factor_dummy_zero_1.h @@ -20,44 +20,41 @@ #pragma once -//Wolf includes +// Wolf includes #include "core/factor/factor_autodiff.h" //#include "ceres/jet.h" -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(FactorDummyZero1); -//class +// class class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1> { - public: - FactorDummyZero1(const FeatureBasePtr& _ftr_ptr, - const NodeStateBlocksPtr& _node, - const StateBlockPtr& _sb_ptr) : - FactorAutodiff("FactorDummy1", - TOP_OTHER, - _ftr_ptr->getMeasurement(), - _ftr_ptr->getMeasurementSquareRootInformationUpper(), - {_node}, - nullptr, - {_sb_ptr}, - false, - FAC_ACTIVE) - { - // - } - - ~FactorDummyZero1() override = default; - - template<typename T> - bool operator ()(const T* const _st1, - T* _residuals) const - { - _residuals[0] = _st1[0]; - return true; - } + public: + FactorDummyZero1(const FeatureBasePtr& _ftr_ptr, const NodeStateBlocksPtr& _node, const StateBlockPtr& _sb_ptr) + : FactorAutodiff("FactorDummy1", + TOP_OTHER, + _ftr_ptr->getMeasurement(), + _ftr_ptr->getMeasurementSquareRootInformationUpper(), + {_node}, + nullptr, + {_sb_ptr}, + false, + FAC_ACTIVE) + { + // + } + + ~FactorDummyZero1() override = default; + + template <typename T> + bool operator()(const T* const _st1, T* _residuals) const + { + _residuals[0] = _st1[0]; + return true; + } }; -} // namespace wolf +} // namespace wolf diff --git a/test/dummy/factor_dummy_zero_15.h b/test/dummy/factor_dummy_zero_15.h index 0b029e9d094b9a94f1dddd6d55def527374f942c..14cb3bc08cefa1cae5dcfcf5ad4265a8de966da2 100644 --- a/test/dummy/factor_dummy_zero_15.h +++ b/test/dummy/factor_dummy_zero_15.h @@ -25,7 +25,6 @@ namespace wolf { - // class template <unsigned int ID> class FactorDummyZero15 diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h index f6352318721a410107831cb1ee6163395b05c98a..44c5a0007cd045cfbb9ccaf9cced29b12bde2527 100644 --- a/test/dummy/factor_feature_dummy.h +++ b/test/dummy/factor_feature_dummy.h @@ -24,7 +24,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorFeatureDummy); class FactorFeatureDummy : public FactorBase diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h index 45220b0a10e0c777f17bac593418021dd469e8c8..4909085cf8cefdca8c1a8c217b6b400372163c8c 100644 --- a/test/dummy/factor_landmark_dummy.h +++ b/test/dummy/factor_landmark_dummy.h @@ -24,7 +24,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(FactorLandmarkDummy); class FactorLandmarkDummy : public FactorBase @@ -34,7 +33,7 @@ class FactorLandmarkDummy : public FactorBase const LandmarkBasePtr& _landmark_other_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE); + FactorStatus _status = FAC_ACTIVE); ~FactorLandmarkDummy() override = default; diff --git a/test/dummy/factor_relative_pose_2d_autodiff.h b/test/dummy/factor_relative_pose_2d_autodiff.h index fb20d134839c6f8b97aaff1b493a6ea2633c17bb..f8f9ec07fa042643a1f40f618c37bb21c23ff0ed 100644 --- a/test/dummy/factor_relative_pose_2d_autodiff.h +++ b/test/dummy/factor_relative_pose_2d_autodiff.h @@ -36,10 +36,10 @@ class FactorRelativePose2dAutodiff : public FactorAutodiff<FactorRelativePose2dA { public: FactorRelativePose2dAutodiff(const FeatureBasePtr& _ftr_ptr, - const FrameBasePtr& _frame_other_ptr, - const ProcessorBasePtr& _processor_ptr, - bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE) + const FrameBasePtr& _frame_other_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE) : FactorAutodiff("FactorOdom2dAutodiff", TOP_OTHER, _ftr_ptr->getMeasurement(), @@ -65,10 +65,10 @@ class FactorRelativePose2dAutodiff : public FactorAutodiff<FactorRelativePose2dA template <typename T> inline bool FactorRelativePose2dAutodiff::operator()(const T* const _p1, - const T* const _o1, - const T* const _p2, - const T* const _o2, - T* _residuals) const + const T* const _o1, + const T* const _p2, + const T* const _o2, + T* _residuals) const { // MAPS Eigen::Map<Eigen::Matrix<T, 3, 1> > res(_residuals); diff --git a/test/dummy/factory_dummy_object.h b/test/dummy/factory_dummy_object.h index eaad26a9439ea32c2a9b58e9ac4c9b7cf44a5d5d..12d2b3e40a4dec836e4fc64edc8447e6ee47b4ad 100644 --- a/test/dummy/factory_dummy_object.h +++ b/test/dummy/factory_dummy_object.h @@ -25,19 +25,19 @@ namespace wolf { - class DummyObject; -typedef Factory<std::shared_ptr<DummyObject>, - const std::string&> FactoryDummyObject; -template<> +typedef Factory<std::shared_ptr<DummyObject>, const std::string&> FactoryDummyObject; +template <> inline std::string FactoryDummyObject::getClass() const { - return "FactoryDummyObject"; + return "FactoryDummyObject"; } - -#define WOLF_REGISTER_DUMMY_OBJECT(DummyObjectType) \ - namespace{ const bool WOLF_UNUSED DummyObjectType##Registered = \ - wolf::FactoryDummyObject::registerCreator(#DummyObjectType, DummyObjectType::create); } \ +#define WOLF_REGISTER_DUMMY_OBJECT(DummyObjectType) \ + namespace \ + { \ + const bool WOLF_UNUSED DummyObjectType##Registered = \ + wolf::FactoryDummyObject::registerCreator(#DummyObjectType, DummyObjectType::create); \ + } } /* namespace wolf */ diff --git a/test/dummy/landmark_dummy.h b/test/dummy/landmark_dummy.h index e7d8228e4d7568bf603ef0a9cfc64a37358e192a..f17f4585ce7b2a9d7f3edf7973a829a7b647d471 100644 --- a/test/dummy/landmark_dummy.h +++ b/test/dummy/landmark_dummy.h @@ -20,7 +20,7 @@ #pragma once -//Wolf includes +// Wolf includes #include "core/landmark/landmark_base.h" #include "core/landmark/factory_landmark.h" @@ -28,44 +28,42 @@ // Std includes - -namespace wolf { - +namespace wolf +{ WOLF_PTR_TYPEDEFS(LandmarkDummy); - -//class LandmarkDummy + +// class LandmarkDummy class LandmarkDummy : public LandmarkBase { - public: - LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param); - LandmarkDummy(const YAML::Node& _node); - WOLF_LANDMARK_CREATE(LandmarkDummy); - - ~LandmarkDummy() override = default; - - int getIntParam() const; - double getDoubleParam() const; - - YAML::Node toYaml() const override; - - private: - const int int_param_; - const double double_param_; - -}; + public: + LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param); + LandmarkDummy(const YAML::Node& _node); + WOLF_LANDMARK_CREATE(LandmarkDummy); + ~LandmarkDummy() override = default; + + int getIntParam() const; + double getDoubleParam() const; + + YAML::Node toYaml() const override; + + private: + const int int_param_; + const double double_param_; +}; -inline LandmarkDummy::LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param) : - LandmarkBase("LandmarkDummy", _p_ptr, _o_ptr), - int_param_(_int_param), - double_param_(_double_param) +inline LandmarkDummy::LandmarkDummy(StateBlockPtr _p_ptr, + StateBlockPtr _o_ptr, + const int& _int_param, + const double& _double_param) + : LandmarkBase("LandmarkDummy", _p_ptr, _o_ptr), int_param_(_int_param), double_param_(_double_param) { } -inline LandmarkDummy::LandmarkDummy(const YAML::Node& _node) : - LandmarkBase("LandmarkDummy", _node), - int_param_(_node["int_param"].as<int>()), - double_param_(_node["double_param"].as<double>()) +inline LandmarkDummy::LandmarkDummy(const YAML::Node& _node) + : LandmarkBase("LandmarkDummy", _node), + int_param_(_node["int_param"].as<int>()), + double_param_(_node["double_param"].as<double>()) { } @@ -83,7 +81,7 @@ inline YAML::Node LandmarkDummy::toYaml() const { YAML::Node node = LandmarkBase::toYaml(); - node["int_param"] = int_param_; + node["int_param"] = int_param_; node["double_param"] = double_param_; return node; @@ -91,4 +89,4 @@ inline YAML::Node LandmarkDummy::toYaml() const WOLF_REGISTER_LANDMARK(LandmarkDummy); -} // namespace wolf +} // namespace wolf diff --git a/test/dummy/processor_diff_drive_mock.h b/test/dummy/processor_diff_drive_mock.h index 0cc61417a44d943a344b89e4142b9aba5b102035..d85674d25c8526154326554174325a6ce5f652fe 100644 --- a/test/dummy/processor_diff_drive_mock.h +++ b/test/dummy/processor_diff_drive_mock.h @@ -27,7 +27,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorDiffDriveMock); class ProcessorDiffDriveMock : public ProcessorDiffDrive @@ -84,7 +83,7 @@ class ProcessorDiffDriveMock : public ProcessorDiffDrive { return Base::deltaZero(); } - + VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override { return Base::getCalibration(_capture); diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h index ebb677ae078a5230244279ab42e539d823bacc91..e1829caf2b26fb9c9afbce9d2849d6e4b27220b5 100644 --- a/test/dummy/processor_loop_closure_dummy.h +++ b/test/dummy/processor_loop_closure_dummy.h @@ -24,7 +24,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy); // dummy class: @@ -119,6 +118,7 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure // Register in the FactoryProcessor #include "core/processor/factory_processor.h" -namespace wolf { +namespace wolf +{ WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureDummy); -} // namespace wolf +} // namespace wolf diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h index c619e86ebea8118804b9cfccd0e226950c0426e6..5eff684007d31a708bc65b0d71a751d34d62978e 100644 --- a/test/dummy/processor_motion_provider_dummy.h +++ b/test/dummy/processor_motion_provider_dummy.h @@ -25,46 +25,65 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(MotionProviderDummy); class ProcessorMotionProviderDummy : public ProcessorBase, public MotionProvider { - public: - ProcessorMotionProviderDummy(const YAML::Node& _params) : - ProcessorBase("ProcessorMotionProviderDummy", 2, _params), - MotionProvider({{'P',"StatePoint2d"},{'O',"StateAngle"}}, _params) - {} - ~ProcessorMotionProviderDummy(){}; + public: + ProcessorMotionProviderDummy(const YAML::Node& _params) + : ProcessorBase("ProcessorMotionProviderDummy", 2, _params), + MotionProvider({{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params) + { + } + ~ProcessorMotionProviderDummy(){}; - // Factory method for high level API - WOLF_PROCESSOR_CREATE(ProcessorMotionProviderDummy); + // Factory method for high level API + WOLF_PROCESSOR_CREATE(ProcessorMotionProviderDummy); - void configure(SensorBasePtr _sensor) override {}; - void processCapture(CaptureBasePtr) override {}; - void processKeyFrame(FrameBasePtr _keyframe_ptr) override {}; - bool triggerInCapture(CaptureBasePtr) const override { return false; }; - bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override { return false; }; - bool storeKeyFrame(FrameBasePtr) override { return false; }; - bool storeCapture(CaptureBasePtr) override { return false; }; - bool voteForKeyFrame() const override { return false; }; - TimeStamp getTimeStamp() const override {return TimeStamp(0);}; + void configure(SensorBasePtr _sensor) override{}; + void processCapture(CaptureBasePtr) override{}; + void processKeyFrame(FrameBasePtr _keyframe_ptr) override{}; + bool triggerInCapture(CaptureBasePtr) const override + { + return false; + }; + bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override + { + return false; + }; + bool storeKeyFrame(FrameBasePtr) override + { + return false; + }; + bool storeCapture(CaptureBasePtr) override + { + return false; + }; + bool voteForKeyFrame() const override + { + return false; + }; + TimeStamp getTimeStamp() const override + { + return TimeStamp(0); + }; - VectorComposite getState(const StateKeys& _structure = "") const override - { - return getOdometry(); - }; + VectorComposite getState(const StateKeys& _structure = "") const override + { + return getOdometry(); + }; - VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override - { - return getOdometry(); - }; + VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override + { + return getOdometry(); + }; }; } /* namespace wolf */ // Register in the FactoryProcessor #include "core/processor/factory_processor.h" -namespace wolf { +namespace wolf +{ WOLF_REGISTER_PROCESSOR(ProcessorMotionProviderDummy); -} // namespace wolf +} // namespace wolf diff --git a/test/dummy/processor_motion_provider_dummy_pov.h b/test/dummy/processor_motion_provider_dummy_pov.h index a54ecacaaf7cd0958a881502ef880b9b9a63d704..0ce065488a85e0bc10ae2358939d96e4e78d7264 100644 --- a/test/dummy/processor_motion_provider_dummy_pov.h +++ b/test/dummy/processor_motion_provider_dummy_pov.h @@ -25,46 +25,65 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(MotionProviderDummy); class ProcessorMotionProviderDummyPOV : public ProcessorBase, public MotionProvider { - public: - ProcessorMotionProviderDummyPOV(const YAML::Node& _params) : - ProcessorBase("ProcessorMotionProviderDummyPOV", 2, _params), - MotionProvider({{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}}, _params) - {} - ~ProcessorMotionProviderDummyPOV(){}; + public: + ProcessorMotionProviderDummyPOV(const YAML::Node& _params) + : ProcessorBase("ProcessorMotionProviderDummyPOV", 2, _params), + MotionProvider({{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'V', "StateVector2d"}}, _params) + { + } + ~ProcessorMotionProviderDummyPOV(){}; - // Factory method for high level API - WOLF_PROCESSOR_CREATE(ProcessorMotionProviderDummyPOV); + // Factory method for high level API + WOLF_PROCESSOR_CREATE(ProcessorMotionProviderDummyPOV); - void configure(SensorBasePtr _sensor) override {}; - void processCapture(CaptureBasePtr) override {}; - void processKeyFrame(FrameBasePtr _keyframe_ptr) override {}; - bool triggerInCapture(CaptureBasePtr) const override { return false; }; - bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override { return false; }; - bool storeKeyFrame(FrameBasePtr) override { return false; }; - bool storeCapture(CaptureBasePtr) override { return false; }; - bool voteForKeyFrame() const override { return false; }; - TimeStamp getTimeStamp() const override {return TimeStamp(0);}; + void configure(SensorBasePtr _sensor) override{}; + void processCapture(CaptureBasePtr) override{}; + void processKeyFrame(FrameBasePtr _keyframe_ptr) override{}; + bool triggerInCapture(CaptureBasePtr) const override + { + return false; + }; + bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override + { + return false; + }; + bool storeKeyFrame(FrameBasePtr) override + { + return false; + }; + bool storeCapture(CaptureBasePtr) override + { + return false; + }; + bool voteForKeyFrame() const override + { + return false; + }; + TimeStamp getTimeStamp() const override + { + return TimeStamp(0); + }; - VectorComposite getState(const StateKeys& _structure = "") const override - { - return getOdometry(); - }; + VectorComposite getState(const StateKeys& _structure = "") const override + { + return getOdometry(); + }; - VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override - { - return getOdometry(); - }; + VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override + { + return getOdometry(); + }; }; } /* namespace wolf */ // Register in the FactoryProcessor #include "core/processor/factory_processor.h" -namespace wolf { +namespace wolf +{ WOLF_REGISTER_PROCESSOR(ProcessorMotionProviderDummyPOV); -} // namespace wolf +} // namespace wolf diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp index 6cd82f5cc61a02e25562fe2156522cdf41f1e2a2..4d763733b5913ace2d5582717efddf96a1a1a085 100644 --- a/test/dummy/processor_tracker_feature_dummy.cpp +++ b/test/dummy/processor_tracker_feature_dummy.cpp @@ -23,7 +23,6 @@ namespace wolf { - unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_in, const CaptureBasePtr& _capture, FeatureBasePtrList& _features_out, @@ -104,11 +103,8 @@ FactorBasePtr ProcessorTrackerFeatureDummy::emplaceFactor(FeatureBasePtr _featur " with origin feature ", _feature_other_ptr->id()); - return FactorBase::emplace<FactorFeatureDummy>(_feature_ptr, - _feature_ptr, - _feature_other_ptr, - shared_from_this(), - applyLossFunction()); + return FactorBase::emplace<FactorFeatureDummy>( + _feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this(), applyLossFunction()); } } // namespace wolf diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h index ae9c809847abac7b6f95d7844ad4db33c3a093f2..ed35b621f5ec1b2c849f116e77d11779322eaeb7 100644 --- a/test/dummy/processor_tracker_feature_dummy.h +++ b/test/dummy/processor_tracker_feature_dummy.h @@ -26,7 +26,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy); // Class diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp index ec0d45406cd0f5ffed05d635a0c4c47abafe96e9..e273083950f12b424625e8b9deacd29b3cf48e29 100644 --- a/test/dummy/processor_tracker_landmark_dummy.cpp +++ b/test/dummy/processor_tracker_landmark_dummy.cpp @@ -26,7 +26,6 @@ namespace wolf { - ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(const YAML::Node& _params) : ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", "PO", _params), n_landmarks_lost_(_params["n_landmarks_lost"].as<unsigned int>()) diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h index 3a62646ca7117d706a5cfe358bfa7b4b468094ad..ebe439379f0f4e88dd097649551c227f3b1113f9 100644 --- a/test/dummy/processor_tracker_landmark_dummy.h +++ b/test/dummy/processor_tracker_landmark_dummy.h @@ -24,7 +24,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy); class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h index c2c640fd7974d51657ba3527063459ed8acb89f3..c8b677de82e80b2de5837edca3921feba362bd90 100644 --- a/test/dummy/sensor_dummy.h +++ b/test/dummy/sensor_dummy.h @@ -27,7 +27,6 @@ namespace wolf { - template <unsigned int DIM> class SensorDummy : public SensorBase { diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h index 6d8ae584e77531ce3078e68503ae965da890e255..3801ba02b34caa34df0b08a358a91fc6fc604b46 100644 --- a/test/dummy/sensor_dummy_poia.h +++ b/test/dummy/sensor_dummy_poia.h @@ -27,7 +27,6 @@ namespace wolf { - template <unsigned int DIM> class SensorDummyPoia : public SensorBase { diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h index 0b368f64c254c4d9b30c78f81ca66bc21258729f..33f83a029d3a41f2b0cfcf672312fa6b2342e105 100644 --- a/test/dummy/solver_manager_dummy.h +++ b/test/dummy/solver_manager_dummy.h @@ -24,97 +24,122 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(SolverManagerDummy); class SolverManagerDummy : public SolverManager { - public: - std::set<FactorBasePtr> factors_derived_; - std::map<StateBlockPtr,bool> state_block_fixed_; - std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_; - - SolverManagerDummy(const ProblemPtr& wolf_problem) : - SolverManager(wolf_problem) - { - }; - - bool isStateBlockFixedDerived(const StateBlockPtr& st) override - { - return state_block_fixed_.at(st); - }; - - bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override - { - return state_block_local_param_.at(st) == local_param; - }; - - bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override - { - return state_block_local_param_.at(st) != nullptr; - }; - - virtual int numStateBlocksDerived() const override - { - return state_block_fixed_.size(); - } - - virtual int numFactorsDerived() const override - { - return factors_derived_.size(); - }; - - bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override {return false;}; - bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override {return false;}; - - - // The following are dummy implementations - bool hasConverged() override { return true; } - bool wasStopped() override { return false; } - unsigned int iterations() override { return 1; } - double initialCost() override { return double(1); } - double finalCost() override { return double(0); } - double totalTime() override { return double(0); } - void printProfilingDerived(std::ostream& _stream) const override {} - - protected: - - bool checkDerived(std::string prefix="") const override {return true;} - std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");}; - void addFactorDerived(const FactorBasePtr& fac_ptr) override - { - factors_derived_.insert(fac_ptr); - }; - void removeFactorDerived(const FactorBasePtr& fac_ptr) override - { - factors_derived_.erase(fac_ptr); - }; - void addStateBlockDerived(const StateBlockPtr& state_ptr) override - { - state_block_fixed_[state_ptr] = state_ptr->isFixed(); - state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization(); - }; - void removeStateBlockDerived(const StateBlockPtr& state_ptr) override - { - state_block_fixed_.erase(state_ptr); - state_block_local_param_.erase(state_ptr); - }; - void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override - { - state_block_fixed_[state_ptr] = state_ptr->isFixed(); - }; - void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override - { - state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization(); - }; - bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override - { - return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1; - }; - - bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override - { - return factors_derived_.count(fac_ptr) == 1; - }; + public: + std::set<FactorBasePtr> factors_derived_; + std::map<StateBlockPtr, bool> state_block_fixed_; + std::map<StateBlockPtr, LocalParametrizationBasePtr> state_block_local_param_; + + SolverManagerDummy(const ProblemPtr& wolf_problem) : SolverManager(wolf_problem){}; + + bool isStateBlockFixedDerived(const StateBlockPtr& st) override + { + return state_block_fixed_.at(st); + }; + + bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, + const LocalParametrizationBasePtr& local_param) override + { + return state_block_local_param_.at(st) == local_param; + }; + + bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override + { + return state_block_local_param_.at(st) != nullptr; + }; + + virtual int numStateBlocksDerived() const override + { + return state_block_fixed_.size(); + } + + virtual int numFactorsDerived() const override + { + return factors_derived_.size(); + }; + + bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override + { + return false; + }; + bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override + { + return false; + }; + + // The following are dummy implementations + bool hasConverged() override + { + return true; + } + bool wasStopped() override + { + return false; + } + unsigned int iterations() override + { + return 1; + } + double initialCost() override + { + return double(1); + } + double finalCost() override + { + return double(0); + } + double totalTime() override + { + return double(0); + } + void printProfilingDerived(std::ostream& _stream) const override {} + + protected: + bool checkDerived(std::string prefix = "") const override + { + return true; + } + std::string solveDerived(const ReportVerbosity report_level) override + { + return std::string(""); + }; + void addFactorDerived(const FactorBasePtr& fac_ptr) override + { + factors_derived_.insert(fac_ptr); + }; + void removeFactorDerived(const FactorBasePtr& fac_ptr) override + { + factors_derived_.erase(fac_ptr); + }; + void addStateBlockDerived(const StateBlockPtr& state_ptr) override + { + state_block_fixed_[state_ptr] = state_ptr->isFixed(); + state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization(); + }; + void removeStateBlockDerived(const StateBlockPtr& state_ptr) override + { + state_block_fixed_.erase(state_ptr); + state_block_local_param_.erase(state_ptr); + }; + void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override + { + state_block_fixed_[state_ptr] = state_ptr->isFixed(); + }; + void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override + { + state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization(); + }; + bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override + { + return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1; + }; + + bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override + { + return factors_derived_.count(fac_ptr) == 1; + }; }; -} +} // namespace wolf diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h index c1c939022234be04ec4552fd94692103d7286120..90aacad7ec072cab30a0d02ce78a203519cbb2d9 100644 --- a/test/dummy/tree_manager_dummy.h +++ b/test/dummy/tree_manager_dummy.h @@ -24,13 +24,11 @@ namespace wolf { - WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerDummy) struct ParamsTreeManagerDummy : public ParamsTreeManagerBase { ParamsTreeManagerDummy() = default; - ParamsTreeManagerDummy(const YAML::Node& _n): - ParamsTreeManagerBase(_n) + ParamsTreeManagerDummy(const YAML::Node& _n) : ParamsTreeManagerBase(_n) { toy_param = _n["toy_param"].as<double>(); } @@ -41,8 +39,7 @@ struct ParamsTreeManagerDummy : public ParamsTreeManagerBase std::string print() const override { - return ParamsTreeManagerBase::print() + "\n" - + "toy_param: " + toString(toy_param) + "\n"; + return ParamsTreeManagerBase::print() + "\n" + "toy_param: " + toString(toy_param) + "\n"; } }; @@ -50,27 +47,25 @@ WOLF_PTR_TYPEDEFS(TreeManagerDummy) class TreeManagerDummy : public TreeManagerBase { - public: - TreeManagerDummy(ParamsTreeManagerBasePtr _params) : - TreeManagerBase("TreeManagerDummy", _params), - n_KF_(0) - {}; - WOLF_TREE_MANAGER_CREATE(TreeManagerDummy, ParamsTreeManagerBase) + public: + TreeManagerDummy(ParamsTreeManagerBasePtr _params) : TreeManagerBase("TreeManagerDummy", _params), n_KF_(0){}; + WOLF_TREE_MANAGER_CREATE(TreeManagerDummy, ParamsTreeManagerBase) - ~TreeManagerDummy() override{} + ~TreeManagerDummy() override {} - void keyFrameCallback(FrameBasePtr _KF) override - { - n_KF_++; - }; + void keyFrameCallback(FrameBasePtr _KF) override + { + n_KF_++; + }; - int n_KF_; + int n_KF_; }; } /* namespace wolf */ // Register in the FactoryTreeManager #include "core/tree_manager/factory_tree_manager.h" -namespace wolf { +namespace wolf +{ WOLF_REGISTER_TREE_MANAGER(TreeManagerDummy) } /* namespace wolf */ diff --git a/test/gtest_SE2.cpp b/test/gtest_SE2.cpp index 84733273d55c042b3c58b4a83951efb051cbed45..87164bebf9cba2fb9b07111391e9f570700687e5 100644 --- a/test/gtest_SE2.cpp +++ b/test/gtest_SE2.cpp @@ -27,7 +27,7 @@ using namespace SE2; TEST(SE2, exp) { - for (auto i = 1; i<10; i++) + for (auto i = 1; i < 10; i++) { double o = Vector1d::Random()(0) * M_PI; ASSERT_MATRIX_APPROX(SO2::exp(o), Rotation2Dd(o).matrix(), 1e-8); @@ -37,14 +37,14 @@ TEST(SE2, exp) TEST(SE2, invBlocks) { Vector2d p, ip; - double o, io; + double o, io; // zero p = Vector2d::Zero(); o = 0; - SE2::inverse(p,o,ip,io); - + SE2::inverse(p, o, ip, io); + ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8); ASSERT_NEAR(io, 0, 1e-8); @@ -52,8 +52,8 @@ TEST(SE2, invBlocks) p = Vector2d::Random(); o = 0; - SE2::inverse(p,o,ip,io); - + SE2::inverse(p, o, ip, io); + ASSERT_MATRIX_APPROX(ip, -p, 1e-8); ASSERT_NEAR(io, 0, 1e-8); @@ -61,8 +61,8 @@ TEST(SE2, invBlocks) p = Vector2d::Zero(); o = Vector1d::Random()(0) * M_PI; - SE2::inverse(p,o,ip,io); - + SE2::inverse(p, o, ip, io); + ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8); ASSERT_NEAR(io, -o, 1e-8); @@ -72,8 +72,8 @@ TEST(SE2, invBlocks) p = Vector2d::Random(); o = Vector1d::Random()(0) * M_PI; - SE2::inverse(p,o,ip,io); - + SE2::inverse(p, o, ip, io); + ASSERT_MATRIX_APPROX(ip, Rotation2Dd(-o).matrix() * -p, 1e-8); ASSERT_NEAR(io, -o, 1e-8); } @@ -86,25 +86,25 @@ TEST(SE2, invVector) // zero d = Vector3d::Zero(); - SE2::inverse(d,id); - + SE2::inverse(d, id); + ASSERT_MATRIX_APPROX(id, Vector3d::Zero(), 1e-8); // zero angle - d = Vector3d::Random(); + d = Vector3d::Random(); d(2) = 0; - SE2::inverse(d,id); - + SE2::inverse(d, id); + ASSERT_MATRIX_APPROX(id.head<2>(), -d.head<2>(), 1e-8); ASSERT_NEAR(id(2), 0, 1e-8); // zero translation - d = Vector3d::Zero(); + d = Vector3d::Zero(); d(2) = Vector1d::Random()(0) * M_PI; - SE2::inverse(d,id); - + SE2::inverse(d, id); + ASSERT_MATRIX_APPROX(id.head<2>(), Vector2d::Zero(), 1e-8); ASSERT_NEAR(id(2), -d(2), 1e-8); @@ -114,8 +114,8 @@ TEST(SE2, invVector) d = Vector3d::Random(); d(2) *= M_PI; - SE2::inverse(d,id); - + SE2::inverse(d, id); + ASSERT_MATRIX_APPROX(id.head<2>(), Rotation2Dd(-d(2)) * -d.head<2>(), 1e-8); ASSERT_NEAR(id(2), -d(2), 1e-8); } @@ -124,7 +124,7 @@ TEST(SE2, invVector) TEST(SE2, betweenBlocks) { Vector2d p1, p2, pd; - double o1, o2, od; + double o1, o2, od; // both origin: relative pose zero p1 = Vector2d::Zero(); @@ -153,11 +153,11 @@ TEST(SE2, betweenBlocks) // random relative pose for (auto i = 0; i < 10; i++) { - p1 = Vector2d::Random(); - o1 = Vector1d::Random()(0) * M_PI; + p1 = Vector2d::Random(); + o1 = Vector1d::Random()(0) * M_PI; Vector2d pd_gt = Vector2d::Random(); - double od_gt = Vector1d::Random()(0) * M_PI; - + double od_gt = Vector1d::Random()(0) * M_PI; + p2 = p1 + Rotation2Dd(o1) * pd_gt; o2 = o1 + od_gt; @@ -198,9 +198,9 @@ TEST(SE2, betweenVectors) p1(2) *= M_PI; Vector3d pd_gt = Vector3d::Random(); pd_gt(2) *= M_PI; - + p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>(); - p2(2) = p1(2) + pd_gt(2); + p2(2) = p1(2) + pd_gt(2); between(p1, p2, pd); @@ -238,9 +238,9 @@ TEST(SE2, betweenVectorsReturn) p1(2) *= M_PI; Vector3d pd_gt = Vector3d::Random(); pd_gt(2) *= M_PI; - + p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>(); - p2(2) = p1(2) + pd_gt(2); + p2(2) = p1(2) + pd_gt(2); pd = between(p1, p2); @@ -250,7 +250,6 @@ TEST(SE2, betweenVectorsReturn) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp index 700f6787f7992fb888fb56d84207849b929b9788..ca50261cc069537a01bfbcd16f0af8e48108ae56 100644 --- a/test/gtest_SE3.cpp +++ b/test/gtest_SE3.cpp @@ -28,14 +28,16 @@ using namespace SE3; TEST(SE3, exp_0) { Vector6d tau = Vector6d::Zero(); - Vector7d pose; pose << 0,0,0, 0,0,0,1; + Vector7d pose; + pose << 0, 0, 0, 0, 0, 0, 1; ASSERT_POSE3d_APPROX(pose, exp(tau), 1e-8); } TEST(SE3, log_I) { - Vector7d pose; pose << 0,0,0, 0,0,0,1; + Vector7d pose; + pose << 0, 0, 0, 0, 0, 0, 1; Vector6d tau = Vector6d::Zero(); ASSERT_MATRIX_APPROX(tau, log(pose), 1e-8); @@ -43,7 +45,7 @@ TEST(SE3, log_I) TEST(SE3, exp_log) { - Vector6d tau = Vector6d::Random() / 5.0; + Vector6d tau = Vector6d::Random() / 5.0; Vector7d pose = exp(tau); ASSERT_MATRIX_APPROX(tau, log(exp(tau)), 1e-8); @@ -56,8 +58,8 @@ TEST(SE3, exp_of_multiple) Vector7d pose, pose2, pose3; tau = Vector6d::Random() / 5; pose << exp(tau); - tau2 = 2*tau; - tau3 = 3*tau; + tau2 = 2 * tau; + tau3 = 3 * tau; pose2 = compose(pose, pose); pose3 = compose(pose2, pose); @@ -77,8 +79,8 @@ TEST(SE3, log_of_power) Vector7d pose, pose2, pose3; tau = Vector6d::Random() / 5; pose << exp(tau); - tau2 = 2*tau; - tau3 = 3*tau; + tau2 = 2 * tau; + tau3 = 3 * tau; pose2 = compose(pose, pose); pose3 = compose(pose2, pose); @@ -94,15 +96,17 @@ TEST(SE3, log_of_power) TEST(SE3, inverse) { - Vector3d p; p.setRandom(); - Vector4d qvec; qvec.setRandom().normalized(); + Vector3d p; + p.setRandom(); + Vector4d qvec; + qvec.setRandom().normalized(); Quaterniond q(qvec); - Vector3d pi_true = -(q.conjugate() * p); + Vector3d pi_true = -(q.conjugate() * p); Quaterniond qi_true = q.conjugate(); // separate API - Vector3d pi_out; + Vector3d pi_out; Quaterniond qi_out; inverse(p, q, pi_out, qi_out); @@ -110,8 +114,10 @@ TEST(SE3, inverse) ASSERT_MATRIX_APPROX(qi_out.coeffs(), qi_true.coeffs(), 1e-8); // Vector7d API - Vector7d pose; pose << p, q.coeffs(); - Vector7d posei_true; posei_true << pi_true, qi_true.coeffs(); + Vector7d pose; + pose << p, q.coeffs(); + Vector7d posei_true; + posei_true << pi_true, qi_true.coeffs(); Vector7d posei_out; inverse(pose, posei_out); ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8); @@ -122,13 +128,15 @@ TEST(SE3, inverse) TEST(SE3, inverseComposite) { - Vector3d p; p.setRandom(); - Vector4d qvec; qvec.setRandom().normalized(); + Vector3d p; + p.setRandom(); + Vector4d qvec; + qvec.setRandom().normalized(); VectorComposite pose_vc("PO", {p, qvec}); - Quaterniond q(qvec); + Quaterniond q(qvec); // ground truth - Vector3d pi_true = -(q.conjugate() * p); + Vector3d pi_true = -(q.conjugate() * p); Quaterniond qi_true = q.conjugate(); VectorComposite pose_vc_out("PO", Vector7d::Zero(), {3, 4}); @@ -143,48 +151,51 @@ TEST(SE3, inverseComposite) TEST(SE3, composeBlocks) { - Vector3d p1, p2, pc; + Vector3d p1, p2, pc; Quaterniond q1, q2, qc; p1.setRandom(); - q1 = exp_q(Vector3d::Random()*100); + q1 = exp_q(Vector3d::Random() * 100); p2.setRandom(); - q2 = exp_q(Vector3d::Random()*100); + q2 = exp_q(Vector3d::Random() * 100); compose(p1, q1, p2, q2, pc, qc); - ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8); - ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8); + ASSERT_MATRIX_APPROX(pc, p1 + q1 * p2, 1e-8); + ASSERT_QUATERNION_APPROX(qc, q1 * q2, 1e-8); } TEST(SE3, composeVectorBlocks) { - Vector3d p1, p2, pc; + Vector3d p1, p2, pc; Quaterniond q1, q2, qc; p1.setRandom(); - q1 = exp_q(Vector3d::Random()*100); + q1 = exp_q(Vector3d::Random() * 100); p2.setRandom(); - q2 = exp_q(Vector3d::Random()*100); + q2 = exp_q(Vector3d::Random() * 100); compose(p1, q1.coeffs(), p2, q2.coeffs(), pc, qc.coeffs()); - ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8); - ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8); + ASSERT_MATRIX_APPROX(pc, p1 + q1 * p2, 1e-8); + ASSERT_QUATERNION_APPROX(qc, q1 * q2, 1e-8); } TEST(SE3, composeEigenVectors) { - Vector3d p1, p2, pc; + Vector3d p1, p2, pc; Quaterniond q1, q2, qc; p1.setRandom(); - q1 = exp_q(Vector3d::Random()*100); + q1 = exp_q(Vector3d::Random() * 100); p2.setRandom(); - q2 = exp_q(Vector3d::Random()*100); + q2 = exp_q(Vector3d::Random() * 100); - compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks + compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks - Vector7d x1; x1 << p1, q1.coeffs(); - Vector7d x2; x2 << p2, q2.coeffs(); - Vector7d xc, xc_true; xc_true << pc, qc.coeffs(); + Vector7d x1; + x1 << p1, q1.coeffs(); + Vector7d x2; + x2 << p2, q2.coeffs(); + Vector7d xc, xc_true; + xc_true << pc, qc.coeffs(); compose(x1, x2, xc); @@ -193,16 +204,16 @@ TEST(SE3, composeEigenVectors) TEST(SE3, composeVectorComposite) { - Vector3d p1, p2, pc; + Vector3d p1, p2, pc; Quaterniond q1, q2, qc; p1.setRandom(); - q1 = exp_q(Vector3d::Random()*100); + q1 = exp_q(Vector3d::Random() * 100); p2.setRandom(); - q2 = exp_q(Vector3d::Random()*100); + q2 = exp_q(Vector3d::Random() * 100); - compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks + compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks - VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3,4}); + VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3, 4}); x1.emplace('P', p1); x1.emplace('O', q1.coeffs()); @@ -224,16 +235,16 @@ TEST(SE3, composeVectorComposite_Jacobians) Vector3d p1, p2, pc; Quaterniond q1, q2, qc; p1.setRandom(); - q1 = exp_q(Vector3d::Random()*100); + q1 = exp_q(Vector3d::Random() * 100); p2.setRandom(); - q2 = exp_q(Vector3d::Random()*100); + q2 = exp_q(Vector3d::Random() * 100); Matrix3d R1 = q1.matrix(); Matrix3d R2 = q2.matrix(); - compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks + compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks - VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3,4}); + VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3, 4}); MatrixComposite J_xc_x1, J_xc_x2; x1.emplace('P', p1); @@ -247,23 +258,25 @@ TEST(SE3, composeVectorComposite_Jacobians) ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8); ASSERT_QUATERNION_VECTOR_APPROX(xc.at('O'), qc.coeffs(), 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), - R1 * skew(p2) , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose() , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity(), 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), -R1 * skew(p2), 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero(), 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose(), 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1 , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero() , 1e-8); - ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity() , 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1, 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero(), 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero(), 1e-8); + ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity(), 1e-8); } TEST(SE3, exp_0_Composite) { // we check that exp(zero) = identity - Vector3d p; p.setZero(); - Vector3d theta; theta.setZero(); + Vector3d p; + p.setZero(); + Vector3d theta; + theta.setZero(); VectorComposite tau; @@ -274,22 +287,25 @@ TEST(SE3, exp_0_Composite) ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8); ASSERT_QUATERNION_VECTOR_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8); - } TEST(SE3, plus_0_Composite) { // we check that x plus zero = x - Vector3d p; p.setRandom(); - Vector4d q; q.setRandom().normalized(); + Vector3d p; + p.setRandom(); + Vector4d q; + q.setRandom().normalized(); VectorComposite x; x.emplace('P', p); x.emplace('O', q); - Vector3d dp; dp.setZero(); - Vector3d dtheta; dtheta.setZero(); + Vector3d dp; + dp.setZero(); + Vector3d dtheta; + dtheta.setZero(); VectorComposite tau; tau.emplace('P', dp); @@ -305,13 +321,14 @@ TEST(SE3, interpolate_I_xyz) { Vector7d p1, p2, p_pre; - p1 << 0,0,0, 0,0,0,1; - p2 << 1,2,3, 0,0,0,1; + p1 << 0, 0, 0, 0, 0, 0, 1; + p2 << 1, 2, 3, 0, 0, 0, 1; double t = 0.2; interpolate(p1, p2, t, p_pre); - Vector7d p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1; + Vector7d p_gt; + p_gt << 0.2, 0.4, 0.6, 0, 0, 0, 1; ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8); } @@ -320,13 +337,14 @@ TEST(SE3, interpolate_xyz_xyz) { Vector7d p1, p2, p_pre; - p1 << 1,2,3, 0,0,0,1; - p2 << 2,4,6, 0,0,0,1; + p1 << 1, 2, 3, 0, 0, 0, 1; + p2 << 2, 4, 6, 0, 0, 0, 1; double t = 0.2; interpolate(p1, p2, t, p_pre); - Vector7d p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1; + Vector7d p_gt; + p_gt << 1.2, 2.4, 3.6, 0, 0, 0, 1; ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8); } @@ -335,13 +353,14 @@ TEST(SE3, interpolate_I_qx) { Vector7d p1, p2, p_pre; - p1 << 0,0,0, 0,0,0,1; - p2 << 0,0,0, 1,0,0,0; + p1 << 0, 0, 0, 0, 0, 0, 1; + p2 << 0, 0, 0, 1, 0, 0, 0; double t = 0.5; interpolate(p1, p2, t, p_pre); - Vector7d p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2; + Vector7d p_gt; + p_gt << 0, 0, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2; ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8); } @@ -350,13 +369,14 @@ TEST(SE3, interpolate_I_qy) { Vector7d p1, p2, p_pre; - p1 << 0,0,0, 0,0,0,1; - p2 << 0,0,0, 0,1,0,0; + p1 << 0, 0, 0, 0, 0, 0, 1; + p2 << 0, 0, 0, 0, 1, 0, 0; double t = 0.5; interpolate(p1, p2, t, p_pre); - Vector7d p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2; + Vector7d p_gt; + p_gt << 0, 0, 0, 0, sqrt(2) / 2, 0, sqrt(2) / 2; ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8); } @@ -365,13 +385,14 @@ TEST(SE3, interpolate_I_qz) { Vector7d p1, p2, p_pre; - p1 << 0,0,0, 0,0,0,1; - p2 << 0,0,0, 0,0,1,0; + p1 << 0, 0, 0, 0, 0, 0, 1; + p2 << 0, 0, 0, 0, 0, 1, 0; double t = 0.5; interpolate(p1, p2, t, p_pre); - Vector7d p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2; + Vector7d p_gt; + p_gt << 0, 0, 0, 0, 0, sqrt(2) / 2, sqrt(2) / 2; ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8); } @@ -381,12 +402,13 @@ TEST(SE3, interpolate_I_qxyz) Vector7d p1, p2, dp, p_pre, p_gt; Vector3d da; - da.setRandom(); da /= 5; + da.setRandom(); + da /= 5; - p1 << 0,0,0, 0,0,0,1; - dp << 0,0,0, exp_q(da).coeffs(); - p_gt = compose(p1, dp); - p2 = compose(p_gt, dp); + p1 << 0, 0, 0, 0, 0, 0, 1; + dp << 0, 0, 0, exp_q(da).coeffs(); + p_gt = compose(p1, dp); + p2 = compose(p_gt, dp); double t = 0.5; interpolate(p1, p2, t, p_pre); @@ -399,10 +421,12 @@ TEST(SE3, interpolate_half) Vector7d p1, p2, dp, p_pre, p_gt; Vector6d da; - p1.setRandom(); p1.tail(4).normalize(); + p1.setRandom(); + p1.tail(4).normalize(); - da.setRandom(); da /= 5; // small rotation - dp << exp(da); + da.setRandom(); + da /= 5; // small rotation + dp << exp(da); // compose double, then interpolate 1/2 @@ -420,10 +444,12 @@ TEST(SE3, interpolate_third) Vector7d p1, p2, dp, dp2, dp3, p_pre, p_gt; Vector6d da; - p1.setRandom(); p1.tail(4).normalize(); + p1.setRandom(); + p1.tail(4).normalize(); - da.setRandom(); da /= 5; // small rotation - dp << exp(da); + da.setRandom(); + da /= 5; // small rotation + dp << exp(da); dp2 = compose(dp, dp); dp3 = compose(dp2, dp); @@ -432,16 +458,16 @@ TEST(SE3, interpolate_third) p_gt = compose(p1, dp); p2 = compose(p1, dp3); - double t = 1.0/3.0; + double t = 1.0 / 3.0; interpolate(p1, p2, t, p_pre); ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8); } - TEST(SE3, toSE3_I) { - Vector7d pose; pose << 0,0,0, 0,0,0,1; + Vector7d pose; + pose << 0, 0, 0, 0, 0, 0, 1; Matrix4d R; toSE3(pose, R); @@ -450,7 +476,6 @@ TEST(SE3, toSE3_I) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_buffer_frame.cpp b/test/gtest_buffer_frame.cpp index a373155132d40049e6ef3991ff392cc058191522..54e12a7f3aab55343117a5c9e7190bb5d9fedd69 100644 --- a/test/gtest_buffer_frame.cpp +++ b/test/gtest_buffer_frame.cpp @@ -33,26 +33,25 @@ using namespace Eigen; class BufferFrameTest : public testing::Test { - public: - - BufferFrame buffer_kf; - FrameBasePtr f10, f20, f21, f28; - double tt10, tt20, tt21, tt28; - TimeStamp timestamp; - double timetolerance; - - void SetUp(void) override - { - f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr); - f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr); - f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr); - f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr); - - tt10 = 1.0; - tt20 = 1.0; - tt21 = 1.0; - tt28 = 1.0; - }; + public: + BufferFrame buffer_kf; + FrameBasePtr f10, f20, f21, f28; + double tt10, tt20, tt21, tt28; + TimeStamp timestamp; + double timetolerance; + + void SetUp(void) override + { + f10 = std::make_shared<FrameBase>(TimeStamp(10), nullptr, nullptr, nullptr); + f20 = std::make_shared<FrameBase>(TimeStamp(20), nullptr, nullptr, nullptr); + f21 = std::make_shared<FrameBase>(TimeStamp(21), nullptr, nullptr, nullptr); + f28 = std::make_shared<FrameBase>(TimeStamp(28), nullptr, nullptr, nullptr); + + tt10 = 1.0; + tt20 = 1.0; + tt21 = 1.0; + tt28 = 1.0; + }; }; TEST_F(BufferFrameTest, empty) @@ -63,32 +62,32 @@ TEST_F(BufferFrameTest, empty) TEST_F(BufferFrameTest, emplace) { buffer_kf.emplace(10, f10); - ASSERT_EQ(buffer_kf.size(), (unsigned int) 1); + ASSERT_EQ(buffer_kf.size(), (unsigned int)1); buffer_kf.emplace(20, f20); - ASSERT_EQ(buffer_kf.size(), (unsigned int) 2); + ASSERT_EQ(buffer_kf.size(), (unsigned int)2); } TEST_F(BufferFrameTest, clear) { buffer_kf.emplace(10, f10); buffer_kf.emplace(20, f20); - ASSERT_EQ(buffer_kf.size(), (unsigned int) 2); + ASSERT_EQ(buffer_kf.size(), (unsigned int)2); buffer_kf.clear(); ASSERT_TRUE(buffer_kf.empty()); } TEST_F(BufferFrameTest, doubleCheckTimeTolerance) { - buffer_kf.clear(); - buffer_kf.emplace(10, f10); - buffer_kf.emplace(20, f20); - // min time tolerance > diff between time stamps. It should return true - ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 20, TimeStamp(20), 20)); - // min time tolerance < diff between time stamps. It should return true - ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 1, TimeStamp(20), 20)); + buffer_kf.clear(); + buffer_kf.emplace(10, f10); + buffer_kf.emplace(20, f20); + // min time tolerance > diff between time stamps. It should return true + ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 20, TimeStamp(20), 20)); + // min time tolerance < diff between time stamps. It should return true + ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 1, TimeStamp(20), 20)); } -//TEST_F(BufferFrameTest, select) +// TEST_F(BufferFrameTest, select) //{ // // Evaluation using two packs (p1,p2) // // with different time tolerances (tp1,tp2) @@ -149,64 +148,63 @@ TEST_F(BufferFrameTest, doubleCheckTimeTolerance) TEST_F(BufferFrameTest, selectFirstBefore) { - buffer_kf.clear(); - - buffer_kf.emplace(10, f10); - buffer_kf.emplace(20, f20); - buffer_kf.emplace(21, f21); - - // input time stamps - std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5}; - double tt = 0.01; - - // Solution matrix - // q_ts | pack - //================= - // first time - //----------------- - // 9.5 nullptr - // 9.995 10 - // 10,005 10 - // 19.5 10 - // 20.5 10 - // 21.5 10 - // second time - //----------------- - // 9.5 nullptr - // 9.995 null - // 10,005 null - // 19.5 null - // 20.5 20 - // 21.5 20 - // third time - //----------------- - // 9.5 nullptr - // 9.995 null - // 10,005 null - // 19.5 null - // 20.5 null - // 21.5 21 - - Eigen::MatrixXd truth(3,6), res(3,6); - truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21; - res.setZero(); - - for (int i=0; i<3; i++) - { - FrameBasePtr packQ; - int j = 0; - for (auto ts : q_ts) - { - packQ = buffer_kf.selectFirstBefore(ts, tt); - if (packQ) - res(i,j) = packQ->getTimeStamp().get(); - - j++; - } - buffer_kf.removeUpTo(packQ->getTimeStamp()); - } - - ASSERT_MATRIX_APPROX(res, truth, 1e-6); + buffer_kf.clear(); + + buffer_kf.emplace(10, f10); + buffer_kf.emplace(20, f20); + buffer_kf.emplace(21, f21); + + // input time stamps + std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5}; + double tt = 0.01; + + // Solution matrix + // q_ts | pack + //================= + // first time + //----------------- + // 9.5 nullptr + // 9.995 10 + // 10,005 10 + // 19.5 10 + // 20.5 10 + // 21.5 10 + // second time + //----------------- + // 9.5 nullptr + // 9.995 null + // 10,005 null + // 19.5 null + // 20.5 20 + // 21.5 20 + // third time + //----------------- + // 9.5 nullptr + // 9.995 null + // 10,005 null + // 19.5 null + // 20.5 null + // 21.5 21 + + Eigen::MatrixXd truth(3, 6), res(3, 6); + truth << 0, 10, 10, 10, 10, 10, 0, 0, 0, 0, 20, 20, 0, 0, 0, 0, 0, 21; + res.setZero(); + + for (int i = 0; i < 3; i++) + { + FrameBasePtr packQ; + int j = 0; + for (auto ts : q_ts) + { + packQ = buffer_kf.selectFirstBefore(ts, tt); + if (packQ) res(i, j) = packQ->getTimeStamp().get(); + + j++; + } + buffer_kf.removeUpTo(packQ->getTimeStamp()); + } + + ASSERT_MATRIX_APPROX(res, truth, 1e-6); } TEST_F(BufferFrameTest, removeUpTo) @@ -220,27 +218,26 @@ TEST_F(BufferFrameTest, removeUpTo) // it should remove f20 and f10, thus size should be 1 after removal // Specifically, only f21 should remain - buffer_kf.removeUpTo( f20->getTimeStamp() ); - ASSERT_EQ(buffer_kf.size(), (unsigned int) 1); - ASSERT_TRUE(buffer_kf.select(f10->getTimeStamp(),tt)==nullptr); - ASSERT_TRUE(buffer_kf.select(f20->getTimeStamp(),tt)==nullptr); - ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(),tt)!=nullptr); + buffer_kf.removeUpTo(f20->getTimeStamp()); + ASSERT_EQ(buffer_kf.size(), (unsigned int)1); + ASSERT_TRUE(buffer_kf.select(f10->getTimeStamp(), tt) == nullptr); + ASSERT_TRUE(buffer_kf.select(f20->getTimeStamp(), tt) == nullptr); + ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(), tt) != nullptr); // Chech removal of an imprecise time stamp // Specifically, only f28 should remain buffer_kf.emplace(28, f28); - ASSERT_EQ(buffer_kf.size(), (unsigned int) 2); - FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr); + ASSERT_EQ(buffer_kf.size(), (unsigned int)2); + FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22), nullptr, nullptr, nullptr); // PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5); - buffer_kf.removeUpTo( f22->getTimeStamp() ); - ASSERT_EQ(buffer_kf.size(), (unsigned int) 1); - ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(),tt)==nullptr); - ASSERT_TRUE(buffer_kf.select(f28->getTimeStamp(),tt)!=nullptr); + buffer_kf.removeUpTo(f22->getTimeStamp()); + ASSERT_EQ(buffer_kf.size(), (unsigned int)1); + ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(), tt) == nullptr); + ASSERT_TRUE(buffer_kf.select(f28->getTimeStamp(), tt) != nullptr); } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp index ed25b7d02a5d4eb11baf016cf947ed6886fc2470..4bfbe7b5a28055c57cedcca15e7033218d3b3fb4 100644 --- a/test/gtest_capture_base.cpp +++ b/test/gtest_capture_base.cpp @@ -32,7 +32,7 @@ using namespace Eigen; TEST(CaptureBase, ConstructorNoSensor) { - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 + CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 ASSERT_EQ(C->getTimeStamp(), 1.2); ASSERT_FALSE(C->getFrame()); @@ -42,8 +42,9 @@ TEST(CaptureBase, ConstructorNoSensor) TEST(CaptureBase, ConstructorWithSensor) { - SensorBasePtr S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5 + SensorBasePtr S = + FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); + CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5 ASSERT_EQ(C->getTimeStamp(), 1.5); ASSERT_FALSE(C->getFrame()); ASSERT_FALSE(C->getProblem()); @@ -52,8 +53,9 @@ TEST(CaptureBase, ConstructorWithSensor) TEST(CaptureBase, Static_sensor_params) { - SensorBasePtr S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5 + SensorBasePtr S = + FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); + CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5 // query capture blocks ASSERT_EQ(C->getSensorP(), S->getP()); @@ -63,11 +65,12 @@ TEST(CaptureBase, Static_sensor_params) TEST(CaptureBase, Dynamic_sensor_params) { - SensorBasePtr S = FactorySensorFile::create("SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_dir}); - StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero())); - StateBlockPtr o(std::make_shared<StateAngle>(0) ); - StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero())); - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5 + SensorBasePtr S = FactorySensorFile::create( + "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_dir}); + StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero())); + StateBlockPtr o(std::make_shared<StateAngle>(0)); + StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero())); + CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5 // query capture blocks ASSERT_EQ(C->getSensorP(), p); @@ -77,40 +80,41 @@ TEST(CaptureBase, Dynamic_sensor_params) TEST(CaptureBase, addFeature) { - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 - + CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 + auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity()); - + ASSERT_FALSE(C->getFeatureList().empty()); ASSERT_EQ(C->getFeatureList().front(), f); } TEST(CaptureBase, print) { - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 - auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity()); + CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 + auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity()); C->print(4, 1, 1, 1); } TEST(CaptureBase, process) { - SensorBasePtr S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); + SensorBasePtr S = + FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr)); - ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail + ASSERT_DEATH({ C->process(); }, ""); // No sensor in the capture should fail C->setSensor(S); - ASSERT_TRUE(C->process()); // This should not fail (although it does nothing) + ASSERT_TRUE(C->process()); // This should not fail (although it does nothing) } TEST(CaptureBase, move_from_F_to_KF) { ProblemPtr problem = Problem::create("PO", 2); - auto KF = problem->emplaceFrame(0.0); // dummy F object + auto KF = problem->emplaceFrame(0.0); // dummy F object auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0); - auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object + auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0); @@ -130,7 +134,7 @@ TEST(CaptureBase, move_from_F_to_null) FrameBasePtr F0 = nullptr; - auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object + auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0); @@ -146,7 +150,7 @@ TEST(CaptureBase, move_from_null_to_KF) { ProblemPtr problem = Problem::create("PO", 2); - auto KF = problem->emplaceFrame(0.0); // dummy F object + auto KF = problem->emplaceFrame(0.0); // dummy F object auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0); @@ -164,7 +168,7 @@ TEST(CaptureBase, move_from_null_to_F) { ProblemPtr problem = Problem::create("PO", 2); - auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object + auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object auto C = std::make_shared<CaptureBase>("Dummy", 0.0); @@ -179,19 +183,17 @@ TEST(CaptureBase, move_from_KF_to_F) { ProblemPtr problem = Problem::create("PO", 2); - auto KF = problem->emplaceFrame(0.0); // dummy F object + auto KF = problem->emplaceFrame(0.0); // dummy F object - auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object + auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object auto C = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0); ASSERT_DEATH(C->move(F), ""); } - int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index c0d16d1b69565ceca4eb4fbae701507fd44151ea..3a6cb174c2924631c799e35c6082ec778fbfd55c 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -89,18 +89,20 @@ TEST(Emplace, Processor) ASSERT_EQ(P->getHardware(), S->getHardware()); ASSERT_EQ(P->getHardware()->getSensorList().front(), S); - auto prc = - ProcessorBase::emplace<ProcessorOdom2d>(S, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir}); + auto prc = ProcessorBase::emplace<ProcessorOdom2d>( + S, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir}); ASSERT_EQ(P, prc->getProblem()); ASSERT_EQ(S, prc->getSensor()); ASSERT_EQ(prc, S->getProcessorList().front()); SensorBasePtr sen2 = SensorBase::emplace<SensorDummy2d>( - P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml"), {wolf_dir}); + P->getHardware(), + YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml"), + {wolf_dir}); - ProcessorOdom2dPtr prc2 = - ProcessorBase::emplace<ProcessorOdom2d>(sen2, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir}); + ProcessorOdom2dPtr prc2 = ProcessorBase::emplace<ProcessorOdom2d>( + sen2, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir}); ASSERT_EQ(P, prc->getProblem()); ASSERT_EQ(sen2, prc2->getSensor()); diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp index e81269f3924200b7f10b5475f1fcbd447180c6ad..3bcd8720c630d2f284616b35d52367f795f00bfd 100644 --- a/test/gtest_example.cpp +++ b/test/gtest_example.cpp @@ -22,21 +22,21 @@ TEST(TestTest, DummyTestExample) { - EXPECT_FALSE(false); + EXPECT_FALSE(false); - ASSERT_TRUE(true); + ASSERT_TRUE(true); - int my_int = 5; + int my_int = 5; - ASSERT_EQ(my_int, 5); + ASSERT_EQ(my_int, 5); -// PRINTF("All good at TestTest::DummyTestExample !\n"); + // PRINTF("All good at TestTest::DummyTestExample !\n"); } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - //::testing::GTEST_FLAG(filter) = "TestTest.DummyTestExample"; // Test only this one - //::testing::GTEST_FLAG(filter) = "TestTest.*"; // Test only the tests in this group - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + //::testing::GTEST_FLAG(filter) = "TestTest.DummyTestExample"; // Test only this one + //::testing::GTEST_FLAG(filter) = "TestTest.*"; // Test only the tests in this group + return RUN_ALL_TESTS(); } diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp index e2e5b94d66071b9d1f9ec39a684fb5b13c457159..4d29a8cbb3409d3b786fb2c89a0811c45a9e2261 100644 --- a/test/gtest_factor_autodiff.cpp +++ b/test/gtest_factor_autodiff.cpp @@ -188,8 +188,8 @@ TEST(FactorAutodiff, EmplaceOdom2d) FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2)); // SENSOR - auto sensor_ptr = FactorySensorFile::create( - "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); + auto sensor_ptr = + FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // CAPTURE auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); @@ -198,7 +198,8 @@ TEST(FactorAutodiff, EmplaceOdom2d) auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Vector3d::Zero(), Matrix3d::Identity()); // FACTOR - auto factor_ptr = FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); + auto factor_ptr = + FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); ASSERT_TRUE(factor_ptr->getFeature()); ASSERT_TRUE(factor_ptr->getFeature()->getCapture()); @@ -220,8 +221,8 @@ TEST(FactorAutodiff, ResidualOdom2d) FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose); // SENSOR - auto sensor_ptr = FactorySensorFile::create( - "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); + auto sensor_ptr = + FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // CAPTURE auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); @@ -232,7 +233,8 @@ TEST(FactorAutodiff, ResidualOdom2d) auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Matrix3d::Identity()); // FACTOR - auto factor_ptr = FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); + auto factor_ptr = + FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); // EVALUATE @@ -263,8 +265,8 @@ TEST(FactorAutodiff, JacobianOdom2d) FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose); // SENSOR - auto sensor_ptr = FactorySensorFile::create( - "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); + auto sensor_ptr = + FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // CAPTURE auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); @@ -275,7 +277,8 @@ TEST(FactorAutodiff, JacobianOdom2d) auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Matrix3d::Identity()); // FACTOR - auto factor_ptr = FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); + auto factor_ptr = + FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); // COMPUTE JACOBIANS @@ -337,8 +340,8 @@ TEST(FactorAutodiff, AutodiffVsAnalytic) FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose); // SENSOR - auto sensor_ptr = FactorySensorFile::create( - "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); + auto sensor_ptr = + FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // CAPTURE auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp index 3a6a15df242fae1551b605e2180ee62efdb97558..f7ea48c4c93a5bf92e93a59df2549f787e6c333d 100644 --- a/test/gtest_factor_base.cpp +++ b/test/gtest_factor_base.cpp @@ -36,24 +36,24 @@ class FactorBaseTest : public testing::Test void SetUp() override { YAML::Node rand_params; - rand_params["name"] = "Sensor0"; - rand_params["states"]["P"]["type"] = "StatePoint2d"; - rand_params["states"]["P"]["state"] = Vector2d::Random(); - rand_params["states"]["P"]["mode"] = "initial_guess"; + rand_params["name"] = "Sensor0"; + rand_params["states"]["P"]["type"] = "StatePoint2d"; + rand_params["states"]["P"]["state"] = Vector2d::Random(); + rand_params["states"]["P"]["mode"] = "initial_guess"; rand_params["states"]["P"]["dynamic"] = false; - rand_params["states"]["O"]["type"] = "StateAngle"; - rand_params["states"]["O"]["state"] = Vector1d::Random(); - rand_params["states"]["O"]["mode"] = "initial_guess"; + rand_params["states"]["O"]["type"] = "StateAngle"; + rand_params["states"]["O"]["state"] = Vector1d::Random(); + rand_params["states"]["O"]["mode"] = "initial_guess"; rand_params["states"]["O"]["dynamic"] = false; - rand_params["noise_p_std"] = 0.1; - rand_params["noise_o_std"] = 0.1; + rand_params["noise_p_std"] = 0.1; + rand_params["noise_o_std"] = 0.1; - S0 = SensorDummy2d::create(rand_params, {}); + S0 = SensorDummy2d::create(rand_params, {}); rand_params["name"] = "Sensor1"; - S1 = SensorDummy2d::create(rand_params, {}); - F0 = std::make_shared<FrameBase>(0.0, SpecStateComposite(rand_params["states"])); - F1 = std::make_shared<FrameBase>(1.0, SpecStateComposite(rand_params["states"])); - C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr); + S1 = SensorDummy2d::create(rand_params, {}); + F0 = std::make_shared<FrameBase>(0.0, SpecStateComposite(rand_params["states"])); + F1 = std::make_shared<FrameBase>(1.0, SpecStateComposite(rand_params["states"])); + C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr); C0->emplaceStateBlocks(SpecStateComposite(rand_params["states"])); C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr); C1->emplaceStateBlocks(SpecStateComposite(rand_params["states"])); diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp index b9c736e9d4669d7bbac1f3f853fc4df556aa11ff..f974f971c781ff2c0f95fc2e62f059e45976318f 100644 --- a/test/gtest_factor_block_difference.cpp +++ b/test/gtest_factor_block_difference.cpp @@ -230,7 +230,6 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy) nullptr, false); - // perturbate KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); @@ -261,7 +260,6 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz) nullptr, false); - // perturbate KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); diff --git a/test/gtest_factor_pose_2d_with_extrinsics.cpp b/test/gtest_factor_pose_2d_with_extrinsics.cpp index 813db2bbee87841f5e60d7e038a79b6f1ad8ae0d..05387053815c37c49fb30c8101ed6dced18cb596 100644 --- a/test/gtest_factor_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_pose_2d_with_extrinsics.cpp @@ -45,7 +45,7 @@ SolverCeres solver(problem_ptr); // Sensor auto sensor_pose2d = - problem_ptr->installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir}); + problem_ptr -> installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero()); diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp index 0b417126af892a4041b0141069dd2fb810f16e43..536f22a521cbbbfb7c88037e5aeef321abba79da 100644 --- a/test/gtest_factor_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp @@ -45,7 +45,7 @@ SolverCeres solver(problem_ptr); // Sensor auto sensor_pose3d = - problem_ptr->installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir}); + problem_ptr -> installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); diff --git a/test/gtest_factor_relative_pose_2d_autodiff.cpp b/test/gtest_factor_relative_pose_2d_autodiff.cpp index 022da481ddb86bbf70ff7cd785b759df0183a431..6154a736d5bd685aa944826a1d51e17ab0a6d2dd 100644 --- a/test/gtest_factor_relative_pose_2d_autodiff.cpp +++ b/test/gtest_factor_relative_pose_2d_autodiff.cpp @@ -25,17 +25,16 @@ #include "core/capture/capture_odom_2d.h" #include "core/math/rotations.h" - using namespace Eigen; using namespace wolf; using std::cout; using std::endl; // Input odometry data and covariance -Matrix3d data_cov = 0.1*Matrix3d::Identity(); +Matrix3d data_cov = 0.1 * Matrix3d::Identity(); // Problem and solver -ProblemPtr problem_ptr = Problem::create("PO", 2); +ProblemPtr problem_ptr = Problem::create("PO", 2); SolverCeres solver(problem_ptr); // Two frames @@ -65,11 +64,11 @@ TEST(FactorRelativePose2dAutodiff, fix_0_solve) // x1 groundtruth Vector3d x1; x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>(); - x1(2) = pi2pi(x0(2) + delta(2)); + x1(2) = pi2pi(x0(2) + delta(2)); // Set states - frm0->setState(x0,"PO"); - frm1->setState(x1,"PO"); + frm0->setState(x0, "PO"); + frm1->setState(x1, "PO"); cap1->setData(delta); // feature & factor with delta measurement @@ -106,11 +105,11 @@ TEST(FactorRelativePose2dAutodiff, fix_1_solve) // x1 groundtruth Vector3d x1; x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>(); - x1(2) = pi2pi(x0(2) + delta(2)); + x1(2) = pi2pi(x0(2) + delta(2)); // Set states - frm0->setState(x0,"PO"); - frm1->setState(x1,"PO"); + frm0->setState(x0, "PO"); + frm1->setState(x1, "PO"); cap1->setData(delta); // feature & factor with delta measurement @@ -134,6 +133,6 @@ TEST(FactorRelativePose2dAutodiff, fix_1_solve) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index 9fb33e2ca2685c87d0cfff6b9ca7ea938024c9b9..94d4d03741f4a4defa44e42890fedbc360d2740b 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -49,7 +49,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 2); SolverCeres solver(problem_ptr); // Sensor -auto sensor = problem_ptr->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero()); diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp index 859e93ed9790ce2491216c798a26947434ee4ae5..7ad85c7096c5ca16ccf9fb8f1a82e1e2435147d5 100644 --- a/test/gtest_factor_relative_pose_3d.cpp +++ b/test/gtest_factor_relative_pose_3d.cpp @@ -40,7 +40,7 @@ Vector7d delta, x_0, x_1, x_f, x_l; // Input odometry data and covariance Matrix6d data_cov = 1e-3 * Matrix6d::Identity(); -//Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal(); +// Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal(); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 3); @@ -217,7 +217,7 @@ TEST(FactorRelativePose3d, frame_solve_frame0) // solve for frm0 std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - //WOLF_INFO(report); + // WOLF_INFO(report); checksFrameFrame(); } diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp index 0368ea83fda7c7d3e271867869281c8059efcc07..7afafe8d970e5953633f1c5fa1333a1f2cea4939 100644 --- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp @@ -47,7 +47,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 3); SolverCeres solver(problem_ptr); // Sensor -auto sensor = problem_ptr->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp index 5e63eeea24890a3727201bc0bfda1420a6af4f64..086376faec4ca414c30bdf9857b92dab844832fc 100644 --- a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp @@ -49,7 +49,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 2); SolverCeres solver(problem_ptr); // Sensor -auto sensor = problem_ptr->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // Frame FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero()); @@ -97,14 +97,8 @@ void generateRandomProblemLandmark() // feature & factor with delta measurement fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov); - fac = FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(fea, - fea->getMeasurement(), - fea->getMeasurementSquareRootInformationUpper(), - frm, - lmk, - sensor, - nullptr, - false); + fac = FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>( + fea, fea->getMeasurement(), fea->getMeasurementSquareRootInformationUpper(), frm, lmk, sensor, nullptr, false); } void checksFrameLandmark() diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp index 61c3a89b7c742836d9fc0cd19a244f4a127c0305..b63ebe1895dff57859d37d7eae5423e2df311a8e 100644 --- a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp @@ -50,7 +50,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 3); SolverCeres solver(problem_ptr); // Sensor -auto sensor = problem_ptr->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); // Frame FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); @@ -123,30 +123,33 @@ void generateRandomProblemLandmark() fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta1, data_cov); fea2 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta2, data_cov); fea3 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta3, data_cov); - fac1 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea1, - fea1->getMeasurement(), - fea1->getMeasurementSquareRootInformationUpper(), - frm, - lmk1, - sensor, - nullptr, - false); - fac2 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea2, - fea2->getMeasurement(), - fea2->getMeasurementSquareRootInformationUpper(), - frm, - lmk2, - sensor, - nullptr, - false); - fac3 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea3, - fea3->getMeasurement(), - fea3->getMeasurementSquareRootInformationUpper(), - frm, - lmk3, - sensor, - nullptr, - false); + fac1 = + FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea1, + fea1->getMeasurement(), + fea1->getMeasurementSquareRootInformationUpper(), + frm, + lmk1, + sensor, + nullptr, + false); + fac2 = + FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea2, + fea2->getMeasurement(), + fea2->getMeasurementSquareRootInformationUpper(), + frm, + lmk2, + sensor, + nullptr, + false); + fac3 = + FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea3, + fea3->getMeasurement(), + fea3->getMeasurementSquareRootInformationUpper(), + frm, + lmk3, + sensor, + nullptr, + false); } void checksFrameLandmark() diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp index f07b2ccbe117f73995a29c2afa8dc58352f0c349..977149e6f139f8a95fad52412673f28dbf2e72a0 100644 --- a/test/gtest_factor_velocity_local_direction_3d.cpp +++ b/test/gtest_factor_velocity_local_direction_3d.cpp @@ -25,7 +25,6 @@ #include "core/capture/capture_odom_2d.h" #include "core/math/rotations.h" - using namespace Eigen; using namespace wolf; @@ -33,161 +32,147 @@ int N_TESTS = 100; class FactorVelocityLocalDirection3dTest : public testing::Test { - protected: - ProblemPtr problem; - SolverManagerPtr solver; - FrameBasePtr frm; - StateBlockPtr sb_p; - StateBlockPtr sb_o; - StateBlockPtr sb_v; - CaptureBasePtr cap; - - int n_solved; - std::vector<double> convergence, iterations, times, error; - - virtual void SetUp() + protected: + ProblemPtr problem; + SolverManagerPtr solver; + FrameBasePtr frm; + StateBlockPtr sb_p; + StateBlockPtr sb_o; + StateBlockPtr sb_v; + CaptureBasePtr cap; + + int n_solved; + std::vector<double> convergence, iterations, times, error; + + virtual void SetUp() + { + // create problem + problem = Problem::create("POV", 3); + + // create solver + auto params_solver = std::make_shared<ParamsCeres>(); + params_solver->solver_options.max_num_iterations = 1e3; + solver = std::make_shared<SolverCeres>(problem, params_solver); + + // Frame + frm = problem->emplaceFrame(0.0, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished()); + sb_p = frm->getP(); + sb_p->fix(); + sb_o = frm->getO(); + sb_v = frm->getV(); + + // Capture + cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase", frm->getTimeStamp(), nullptr); + } + + FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local, const double& angle_stdev) + { + // emplace feature + FeatureBasePtr fea = + FeatureBase::emplace<FeatureBase>(cap, "FeatureBase", v_local, Matrix1d(angle_stdev * angle_stdev)); + + // emplace factor + return FactorBase::emplace<FactorVelocityLocalDirection3d>( + fea, fea->getMeasurement(), fea->getMeasurementSquareRootInformationUpper(), frm, 0, nullptr, false); + } + + bool testLocalVelocity(const Quaterniond& o, const Vector3d& v_local, bool estimate_o, bool estimate_v) + { + assert(cap->getFeatureList().empty()); + + // set state + sb_o->setState(o.coeffs()); + sb_v->setState(o * v_local); + + // fix or unfix & perturb + if (not estimate_o) + sb_o->fix(); + else { - // create problem - problem = Problem::create("POV", 3); - - // create solver - auto params_solver = std::make_shared<ParamsCeres>(); - params_solver->solver_options.max_num_iterations = 1e3; - solver = std::make_shared<SolverCeres>(problem, params_solver); - - // Frame - frm = problem->emplaceFrame(0.0, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); - sb_p = frm->getP(); - sb_p->fix(); - sb_o = frm->getO(); - sb_v = frm->getV(); - - // Capture - cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase", - frm->getTimeStamp(), nullptr); + sb_o->unfix(); + sb_o->perturb(); } - - FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local, - const double& angle_stdev) + if (not estimate_v) + sb_v->fix(); + else { - // emplace feature - FeatureBasePtr fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase", - v_local, - Matrix1d(angle_stdev * angle_stdev)); - - // emplace factor - return FactorBase::emplace<FactorVelocityLocalDirection3d>(fea, - fea->getMeasurement(), - fea->getMeasurementSquareRootInformationUpper(), - frm, - 0, - nullptr, - false); + sb_v->unfix(); + sb_v->setState(Vector3d::Random()); } - bool testLocalVelocity(const Quaterniond& o, - const Vector3d& v_local, - bool estimate_o, - bool estimate_v) + // emplace feature & factor + auto fac = emplaceFeatureAndFactor(v_local, 0.01); + + // solve + std::string report = solver->solve(SolverManager::ReportVerbosity::FULL); + + // local coordinates + auto v_est_local_normalized = + (Quaterniond(Vector4d(sb_o->getState())).conjugate() * sb_v->getState()).normalized(); + auto cos_angle_local = v_est_local_normalized.dot(v_local); + if (cos_angle_local > 1) cos_angle_local = 1; + if (cos_angle_local < -1) cos_angle_local = -1; + + // global coordinates + auto v_global = Quaterniond(Vector4d(sb_o->getState())) * v_local; + auto v_est_normalized = sb_v->getState().normalized(); + auto cos_angle_global = v_est_normalized.dot(v_global); + if (cos_angle_global > 1) cos_angle_global = 1; + if (cos_angle_global < -1) cos_angle_global = -1; + + // Check angle error + if (std::abs(acos(cos_angle_local)) > M_PI / 180 or std::abs(acos(cos_angle_global)) > M_PI / 180) { - assert(cap->getFeatureList().empty()); - - // set state - sb_o->setState(o.coeffs()); - sb_v->setState(o * v_local); - - // fix or unfix & perturb - if (not estimate_o) - sb_o->fix(); - else - { - sb_o->unfix(); - sb_o->perturb(); - } - if (not estimate_v) - sb_v->fix(); - else - { - sb_v->unfix(); - sb_v->setState(Vector3d::Random()); - } - - // emplace feature & factor - auto fac = emplaceFeatureAndFactor(v_local, 0.01); - - // solve - std::string report = solver->solve(SolverManager::ReportVerbosity::FULL); - - // local coordinates - auto v_est_local_normalized = (Quaterniond(Vector4d(sb_o->getState())).conjugate() * sb_v->getState()).normalized(); - auto cos_angle_local = v_est_local_normalized.dot(v_local); - if (cos_angle_local > 1) - cos_angle_local = 1; - if (cos_angle_local < -1) - cos_angle_local = -1; - - // global coordinates - auto v_global = Quaterniond(Vector4d(sb_o->getState())) * v_local; - auto v_est_normalized = sb_v->getState().normalized(); - auto cos_angle_global = v_est_normalized.dot(v_global); - if (cos_angle_global > 1) - cos_angle_global = 1; - if (cos_angle_global < -1) - cos_angle_global = -1; - - // Check angle error - if (std::abs(acos(cos_angle_local)) > M_PI/180 or - std::abs(acos(cos_angle_global)) > M_PI/180) - { - WOLF_ERROR("\n\n!!!!!!!!!!!! ERROR TOO BIG iteration: ", iterations.size()); - WOLF_INFO("cos(angle local) = ", cos_angle_local); - WOLF_INFO("angle local = ", acos(cos_angle_local)); - WOLF_INFO("cos(angle global) = ", cos_angle_global); - WOLF_INFO("angle global = ", acos(cos_angle_global)); - - problem->print(4,1,1,1); - WOLF_INFO(report); - - // Reset - fac->getFeature()->remove(); - - return false; - } + WOLF_ERROR("\n\n!!!!!!!!!!!! ERROR TOO BIG iteration: ", iterations.size()); + WOLF_INFO("cos(angle local) = ", cos_angle_local); + WOLF_INFO("angle local = ", acos(cos_angle_local)); + WOLF_INFO("cos(angle global) = ", cos_angle_global); + WOLF_INFO("angle global = ", acos(cos_angle_global)); + + problem->print(4, 1, 1, 1); + WOLF_INFO(report); // Reset fac->getFeature()->remove(); - // Update performaces - convergence.push_back(solver->hasConverged() ? 1 : 0); - iterations.push_back(solver->iterations()); - times.push_back(solver->totalTime()); - error.push_back(acos(cos_angle_local)); - - return true; + return false; } - void printAverageResults() - { - WOLF_INFO("/////////////////// Average results: n_solved = ", iterations.size()); - double conv_avg = Eigen::Map<Eigen::VectorXd>(convergence.data(), convergence.size()).mean(); - double iter_avg = Eigen::Map<Eigen::VectorXd>(iterations.data(), iterations.size()).mean(); - double time_avg = Eigen::Map<Eigen::VectorXd>(times.data(), times.size()).mean(); - double err_avg = Eigen::Map<Eigen::VectorXd>(error.data(), error.size()).mean(); - - double iter_stdev = (Eigen::Map<Eigen::ArrayXd>(iterations.data(), iterations.size()) - iter_avg).matrix().norm(); - double time_stdev = (Eigen::Map<Eigen::ArrayXd>(times.data(), times.size()) - time_avg).matrix().norm(); - double err_stdev = (Eigen::Map<Eigen::ArrayXd>(error.data(), error.size()) - err_avg).matrix().norm(); - - WOLF_INFO("\tconvergence: ", 100 * conv_avg, "%"); - WOLF_INFO("\titerations: ", iter_avg, " - stdev: ", iter_stdev); - WOLF_INFO("\ttime: ", 1000 * time_avg, " ms", " - stdev: ", time_stdev); - WOLF_INFO("\tangle error: ", err_avg, " - stdev: ", err_stdev); - } + // Reset + fac->getFeature()->remove(); + + // Update performaces + convergence.push_back(solver->hasConverged() ? 1 : 0); + iterations.push_back(solver->iterations()); + times.push_back(solver->totalTime()); + error.push_back(acos(cos_angle_local)); + + return true; + } + + void printAverageResults() + { + WOLF_INFO("/////////////////// Average results: n_solved = ", iterations.size()); + double conv_avg = Eigen::Map<Eigen::VectorXd>(convergence.data(), convergence.size()).mean(); + double iter_avg = Eigen::Map<Eigen::VectorXd>(iterations.data(), iterations.size()).mean(); + double time_avg = Eigen::Map<Eigen::VectorXd>(times.data(), times.size()).mean(); + double err_avg = Eigen::Map<Eigen::VectorXd>(error.data(), error.size()).mean(); + + double iter_stdev = + (Eigen::Map<Eigen::ArrayXd>(iterations.data(), iterations.size()) - iter_avg).matrix().norm(); + double time_stdev = (Eigen::Map<Eigen::ArrayXd>(times.data(), times.size()) - time_avg).matrix().norm(); + double err_stdev = (Eigen::Map<Eigen::ArrayXd>(error.data(), error.size()) - err_avg).matrix().norm(); + + WOLF_INFO("\tconvergence: ", 100 * conv_avg, "%"); + WOLF_INFO("\titerations: ", iter_avg, " - stdev: ", iter_stdev); + WOLF_INFO("\ttime: ", 1000 * time_avg, " ms", " - stdev: ", time_stdev); + WOLF_INFO("\tangle error: ", err_avg, " - stdev: ", err_stdev); + } }; // Input odometry data and covariance Vector3d v_local(1.0, 0.0, 0.0); -double angle_stdev = 0.1; +double angle_stdev = 0.1; TEST_F(FactorVelocityLocalDirection3dTest, check_tree) { @@ -201,35 +186,29 @@ TEST_F(FactorVelocityLocalDirection3dTest, check_tree) TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV) { for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), - Vector3d::Random().normalized(), - false, true)); + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), Vector3d::Random().normalized(), false, true)); printAverageResults(); } TEST_F(FactorVelocityLocalDirection3dTest, random_solveV) { for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), - Vector3d::Random().normalized(), - false, true)); + ASSERT_TRUE(testLocalVelocity( + Quaterniond(Vector4d::Random().normalized()), Vector3d::Random().normalized(), false, true)); printAverageResults(); } TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_degenerated) { for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4, - false, true)); + ASSERT_TRUE(testLocalVelocity( + Quaterniond::Identity(), (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4, false, true)); for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, - false, true)); + ASSERT_TRUE(testLocalVelocity( + Quaterniond::Identity(), (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, false, true)); for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, - false, true)); + ASSERT_TRUE(testLocalVelocity( + Quaterniond::Identity(), (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, false, true)); printAverageResults(); } @@ -238,50 +217,47 @@ TEST_F(FactorVelocityLocalDirection3dTest, random_solveV_degenerated) for (auto i = 0; i < N_TESTS; i++) ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, - false, true)); + false, + true)); for (auto i = 0; i < N_TESTS; i++) ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, - false, true)); + false, + true)); for (auto i = 0; i < N_TESTS; i++) ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, - false, true)); + false, + true)); printAverageResults(); } TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO) { for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), - Vector3d::Random().normalized(), - true, false)); + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), Vector3d::Random().normalized(), true, false)); printAverageResults(); } TEST_F(FactorVelocityLocalDirection3dTest, random_solveO) { for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), - Vector3d::Random().normalized(), - true, false)); + ASSERT_TRUE(testLocalVelocity( + Quaterniond(Vector4d::Random().normalized()), Vector3d::Random().normalized(), true, false)); printAverageResults(); } TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_degenerated) { for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4, - true, false)); + ASSERT_TRUE(testLocalVelocity( + Quaterniond::Identity(), (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4, true, false)); for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, - true, false)); + ASSERT_TRUE(testLocalVelocity( + Quaterniond::Identity(), (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, true, false)); for (auto i = 0; i < N_TESTS; i++) - ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, - true, false)); + ASSERT_TRUE(testLocalVelocity( + Quaterniond::Identity(), (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, true, false)); printAverageResults(); } @@ -290,20 +266,23 @@ TEST_F(FactorVelocityLocalDirection3dTest, random_solveO_degenerated) for (auto i = 0; i < N_TESTS; i++) ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, - true, false)); + true, + false)); for (auto i = 0; i < N_TESTS; i++) ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, - true, false)); + true, + false)); for (auto i = 0; i < N_TESTS; i++) ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, - true, false)); + true, + false)); printAverageResults(); } -int main(int argc, char **argv) +int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp index 83f04481e9dde10ac4e51634e9fcb55ed6e96637..ff2c68f9452f5260530e79ad3b3745e7a1f9b5ae 100644 --- a/test/gtest_factory_state_block.cpp +++ b/test/gtest_factory_state_block.cpp @@ -25,7 +25,6 @@ #include "core/sensor/factory_sensor.h" #include "core/state_block/state_block_derived.h" - using namespace wolf; using namespace Eigen; @@ -40,75 +39,76 @@ TEST(FactoryStateBlock, creator_non_registered_key) TEST(FactoryStateBlock, creator_StateBlock) { - ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Vector3d(1,2,3), false), std::runtime_error); + ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Vector3d(1, 2, 3), false), std::runtime_error); } TEST(FactoryStateBlock, creator_Quaternion) { - auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1,2,3,4).normalized(), false); + auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1, 2, 3, 4).normalized(), false); - ASSERT_EQ(sbq->getSize() , 4); + ASSERT_EQ(sbq->getSize(), 4); ASSERT_EQ(sbq->getLocalSize(), 3); ASSERT_TRUE(sbq->hasLocalParametrization()); } TEST(FactoryStateBlock, creator_Quaternion_not_normalized) { - ASSERT_THROW(auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1,2,3,4), false), std::runtime_error); + ASSERT_THROW(auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1, 2, 3, 4), false), + std::runtime_error); } TEST(FactoryStateBlock, creator_Angle) { auto sba = FactoryStateBlock::create("StateAngle", Vector1d(1), false); - ASSERT_EQ(sba->getSize() , 1); + ASSERT_EQ(sba->getSize(), 1); ASSERT_EQ(sba->getLocalSize(), 1); ASSERT_TRUE(sba->hasLocalParametrization()); } TEST(FactoryStateBlock, creator_Homogeneous3d) { - auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Vector4d(1,2,3,4), false); + auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Vector4d(1, 2, 3, 4), false); - ASSERT_EQ(sbh->getSize() , 4); + ASSERT_EQ(sbh->getSize(), 4); ASSERT_EQ(sbh->getLocalSize(), 3); ASSERT_TRUE(sbh->hasLocalParametrization()); } TEST(FactoryStateBlock, creator_Point) { - auto sba = FactoryStateBlock::create("StatePoint2d", Vector2d(1,2), false); + auto sba = FactoryStateBlock::create("StatePoint2d", Vector2d(1, 2), false); - ASSERT_EQ(sba->getSize() , 2); + ASSERT_EQ(sba->getSize(), 2); ASSERT_EQ(sba->getLocalSize(), 2); ASSERT_FALSE(sba->hasLocalParametrization()); - sba = FactoryStateBlock::create("StatePoint3d", Vector3d(1,2,3), false); + sba = FactoryStateBlock::create("StatePoint3d", Vector3d(1, 2, 3), false); - ASSERT_EQ(sba->getSize() , 3); + ASSERT_EQ(sba->getSize(), 3); ASSERT_EQ(sba->getLocalSize(), 3); ASSERT_FALSE(sba->hasLocalParametrization()); // fails - ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error); + ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false), std::runtime_error); } TEST(FactoryStateBlock, creator_Vector) { - auto sba = FactoryStateBlock::create("StateVector2d", Vector2d(1,2), false); + auto sba = FactoryStateBlock::create("StateVector2d", Vector2d(1, 2), false); - ASSERT_EQ(sba->getSize() , 2); + ASSERT_EQ(sba->getSize(), 2); ASSERT_EQ(sba->getLocalSize(), 2); ASSERT_FALSE(sba->hasLocalParametrization()); - sba = FactoryStateBlock::create("StateVector3d", Vector3d(1,2,3), false); + sba = FactoryStateBlock::create("StateVector3d", Vector3d(1, 2, 3), false); - ASSERT_EQ(sba->getSize() , 3); + ASSERT_EQ(sba->getSize(), 3); ASSERT_EQ(sba->getLocalSize(), 3); ASSERT_FALSE(sba->hasLocalParametrization()); // fails - ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error); + ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false), std::runtime_error); } TEST(FactoryStateBlock, creator_Params) @@ -139,7 +139,7 @@ TEST(FactoryStateBlock, creator_Params) ASSERT_FALSE(sb1->hasLocalParametrization()); // fails - ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::runtime_error); + ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false), std::runtime_error); } //////////////////////////////////////////////////////// @@ -160,7 +160,7 @@ TEST(FactoryStateBlockIdentity, creator_Quaternion) { auto sbq = FactoryStateBlockIdentity::create("StateQuaternion"); - ASSERT_EQ(sbq->getSize() , 4); + ASSERT_EQ(sbq->getSize(), 4); ASSERT_EQ(sbq->getLocalSize(), 3); ASSERT_TRUE(sbq->hasLocalParametrization()); ASSERT_TRUE(sbq->isValid()); @@ -171,7 +171,7 @@ TEST(FactoryStateBlockIdentity, creator_Angle) { auto sba = FactoryStateBlockIdentity::create("StateAngle"); - ASSERT_EQ(sba->getSize() , 1); + ASSERT_EQ(sba->getSize(), 1); ASSERT_EQ(sba->getLocalSize(), 1); ASSERT_TRUE(sba->hasLocalParametrization()); ASSERT_TRUE(sba->isValid()); @@ -182,7 +182,7 @@ TEST(FactoryStateBlockIdentity, creator_Homogeneous3d) { auto sbh = FactoryStateBlockIdentity::create("StateHomogeneous3d"); - ASSERT_EQ(sbh->getSize() , 4); + ASSERT_EQ(sbh->getSize(), 4); ASSERT_EQ(sbh->getLocalSize(), 3); ASSERT_TRUE(sbh->hasLocalParametrization()); ASSERT_TRUE(sbh->isValid()); @@ -193,15 +193,15 @@ TEST(FactoryStateBlockIdentity, creator_Point) { auto sbp2d = FactoryStateBlockIdentity::create("StatePoint2d"); - ASSERT_EQ(sbp2d->getSize() , 2); + ASSERT_EQ(sbp2d->getSize(), 2); ASSERT_EQ(sbp2d->getLocalSize(), 2); ASSERT_FALSE(sbp2d->hasLocalParametrization()); ASSERT_TRUE(sbp2d->isValid()); ASSERT_MATRIX_APPROX(Vector2d::Zero(), sbp2d->getState(), Constants::EPS_SMALL); - auto sbp3d = FactoryStateBlockIdentity::create("StatePoint3d"); + auto sbp3d = FactoryStateBlockIdentity::create("StatePoint3d"); - ASSERT_EQ(sbp3d->getSize() , 3); + ASSERT_EQ(sbp3d->getSize(), 3); ASSERT_EQ(sbp3d->getLocalSize(), 3); ASSERT_FALSE(sbp3d->hasLocalParametrization()); ASSERT_TRUE(sbp3d->isValid()); @@ -212,7 +212,7 @@ TEST(FactoryStateBlockIdentity, creator_Vector) { auto sbv2d = FactoryStateBlockIdentity::create("StateVector2d"); - ASSERT_EQ(sbv2d->getSize() , 2); + ASSERT_EQ(sbv2d->getSize(), 2); ASSERT_EQ(sbv2d->getLocalSize(), 2); ASSERT_FALSE(sbv2d->hasLocalParametrization()); ASSERT_TRUE(sbv2d->isValid()); @@ -220,7 +220,7 @@ TEST(FactoryStateBlockIdentity, creator_Vector) auto sbv3d = FactoryStateBlockIdentity::create("StateVector3d"); - ASSERT_EQ(sbv3d->getSize() , 3); + ASSERT_EQ(sbv3d->getSize(), 3); ASSERT_EQ(sbv3d->getLocalSize(), 3); ASSERT_FALSE(sbv3d->hasLocalParametrization()); ASSERT_TRUE(sbv3d->isValid()); @@ -272,7 +272,7 @@ TEST(FactoryStateBlockIdentity, creator_Params) ASSERT_FALSE(sb8->hasLocalParametrization()); ASSERT_FALSE(sb9->hasLocalParametrization()); ASSERT_FALSE(sb10->hasLocalParametrization()); - + ASSERT_TRUE(sb1->isValid()); ASSERT_TRUE(sb2->isValid()); ASSERT_TRUE(sb3->isValid()); @@ -283,7 +283,7 @@ TEST(FactoryStateBlockIdentity, creator_Params) ASSERT_TRUE(sb8->isValid()); ASSERT_TRUE(sb9->isValid()); ASSERT_TRUE(sb10->isValid()); - + ASSERT_MATRIX_APPROX(Vector1d::Zero(), sb1->getState(), Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(Vector2d::Zero(), sb2->getState(), Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(Vector3d::Zero(), sb3->getState(), Constants::EPS_SMALL); @@ -345,7 +345,7 @@ TEST(FactoryStateBlockIdentityVector, creator_Point) TEST(FactoryStateBlockIdentityVector, creator_Vector) { auto vec = FactoryStateBlockIdentityVector::create("StateVector2d"); - + ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec, Constants::EPS_SMALL); vec = FactoryStateBlockIdentityVector::create("StateVector3d"); @@ -376,7 +376,7 @@ TEST(FactoryStateBlockIdentityVectorVector, creator_Params) ASSERT_EQ(vec_8.size(), 8); ASSERT_EQ(vec_9.size(), 9); ASSERT_EQ(vec_10.size(), 10); - + ASSERT_MATRIX_APPROX(Vector1d::Zero(), vec_1, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec_2, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(Vector3d::Zero(), vec_3, Constants::EPS_SMALL); diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp index 2cf2ec0402bab7e28c17053ffcba0e7b5e2a97df..7db36fade14f11d0edb605edb7eaabc1a6931a86 100644 --- a/test/gtest_feature_base.cpp +++ b/test/gtest_feature_base.cpp @@ -27,41 +27,52 @@ using namespace Eigen; TEST(FeatureBase, ConstructorCov) { - Vector3d m; m << 1,2,3; - Matrix3d M; M.setRandom(); M = M*M.transpose(); - Matrix3d I = M.inverse(); - Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A - Eigen::MatrixXd U = llt_of_info.matrixU(); - Eigen::MatrixXd L = llt_of_info.matrixL(); + Vector3d m; + m << 1, 2, 3; + Matrix3d M; + M.setRandom(); + M = M * M.transpose(); + Matrix3d I = M.inverse(); + Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A + Eigen::MatrixXd U = llt_of_info.matrixU(); + Eigen::MatrixXd L = llt_of_info.matrixL(); FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, M)); ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6); ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6); - ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6); + ASSERT_MATRIX_APPROX( + f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), + U.transpose() * U, + 1e-6); ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6); } TEST(FeatureBase, ConstructorInfo) { - Vector3d m; m << 1,2,3; - Matrix3d M; M.setRandom(); M = M*M.transpose(); - Matrix3d I = M.inverse(); - Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A - Eigen::MatrixXd U = llt_of_info.matrixU(); - Eigen::MatrixXd L = llt_of_info.matrixL(); + Vector3d m; + m << 1, 2, 3; + Matrix3d M; + M.setRandom(); + M = M * M.transpose(); + Matrix3d I = M.inverse(); + Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A + Eigen::MatrixXd U = llt_of_info.matrixU(); + Eigen::MatrixXd L = llt_of_info.matrixL(); FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, I, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6); ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6); - ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6); + ASSERT_MATRIX_APPROX( + f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), + U.transpose() * U, + 1e-6); ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6); } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 69aec43368945f67115c7f729cda3e27b80a2c40..0510064021e4d52d7c8ae65d624701dff690899f 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -69,8 +69,7 @@ TEST(FrameBase, LinksBasic) ASSERT_FALSE(F->getTrajectory()); ASSERT_FALSE(F->getProblem()); - auto S = FactorySensorFile::create( - "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); + auto S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); ASSERT_FALSE(F->getCaptureOf(S)); ASSERT_TRUE(F->getCaptureList().empty()); ASSERT_TRUE(F->getFactoredBySet().empty()); @@ -86,8 +85,8 @@ TEST(FrameBase, LinksToTree) FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0)); auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0)); - auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr); - auto p = P->installProcessor("ProcessorOdom2d", S, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir}); + auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr); + auto p = P->installProcessor("ProcessorOdom2d", S, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir}); auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double, 1, 1>::Identity() * .01); auto fac = FactorBase::emplace<FactorRelativePose2d>( f, Vector3d::Random(), Matrix3d::Identity(), F1, F2, p, false, TOP_MOTION); diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp index 73080589f56fb0940f50ca3c2ae05d6ecd9691bb..f6c0df1ad7cf7a4e6251bf027ec840138740ef82 100644 --- a/test/gtest_local_param.cpp +++ b/test/gtest_local_param.cpp @@ -20,7 +20,6 @@ #include "core/utils/utils_gtest.h" - #include "core/state_block/local_parametrization_quaternion.h" #include "core/state_block/local_parametrization_homogeneous.h" #include "core/math/rotations.h" @@ -29,20 +28,20 @@ #include <iostream> -#define JAC_NUMERIC(T, _x0, _J, dx) \ -{ \ - VectorXd dv(3); \ - Map<const VectorXd> _dv (dv.data(), 3); \ - VectorXd xo(4); \ - Map<VectorXd> _xo (xo.data(), 4); \ - for (int i = 0; i< 3; i++) \ - {\ - dv.setZero();\ - dv(i) = dx;\ - T.plus(_x0, _dv, _xo);\ - _J.col(i) = (_xo - _x0)/dx; \ - } \ -} +#define JAC_NUMERIC(T, _x0, _J, dx) \ + { \ + VectorXd dv(3); \ + Map<const VectorXd> _dv(dv.data(), 3); \ + VectorXd xo(4); \ + Map<VectorXd> _xo(xo.data(), 4); \ + for (int i = 0; i < 3; i++) \ + { \ + dv.setZero(); \ + dv(i) = dx; \ + T.plus(_x0, _dv, _xo); \ + _J.col(i) = (_xo - _x0) / dx; \ + } \ + } using namespace Eigen; using namespace std; @@ -50,40 +49,39 @@ using namespace wolf; TEST(TestLocalParametrization, QuaternionLocal) { - // Storage VectorXd x_storage(22); - MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size. + MatrixXd M_storage(1, 12); // matrix dimensions do not matter, just storage size. x_storage.setRandom(); M_storage.setZero(); // QUATERNION ------------------------------------------ - Map<VectorXd> q(&x_storage(0),4); + Map<VectorXd> q(&x_storage(0), 4); q.normalize(); - Map<VectorXd> da(&x_storage(4),3); + Map<VectorXd> da(&x_storage(4), 3); da /= 10.0; - Map<VectorXd> qo_m(&x_storage(7),4); - Map<MatrixRowXd> J(&M_storage(0,0),4,3); - MatrixXd J_num(4,3); + Map<VectorXd> qo_m(&x_storage(7), 4); + Map<MatrixRowXd> J(&M_storage(0, 0), 4, 3); + MatrixXd J_num(4, 3); LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc; - Map<const VectorXd> q_m(q.data(),4); - Map<const VectorXd> da_m(da.data(),3); + Map<const VectorXd> q_m(q.data(), 4); + Map<const VectorXd> da_m(da.data(), 3); - Qpar_loc.plus(q_m,da_m,qo_m); + Qpar_loc.plus(q_m, da_m, qo_m); ASSERT_DOUBLE_EQ(qo_m.norm(), 1); Quaterniond qref = Map<Quaterniond>(q.data()) * wolf::v2q(da); - ASSERT_QUATERNION_VECTOR_APPROX(qo_m,qref.coeffs(),Constants::EPS); + ASSERT_QUATERNION_VECTOR_APPROX(qo_m, qref.coeffs(), Constants::EPS); - Qpar_loc.computeJacobian(q_m,J); + Qpar_loc.computeJacobian(q_m, J); JAC_NUMERIC(Qpar_loc, q_m, J_num, 1e-9) - ASSERT_NEAR((J-J_num).norm(), 0, 1e-6); + ASSERT_NEAR((J - J_num).norm(), 0, 1e-6); Map<const VectorXd> qoc_m(qo_m.data(), 4); Map<VectorXd> da2_m(x_storage.data() + 10, 3); @@ -91,44 +89,43 @@ TEST(TestLocalParametrization, QuaternionLocal) Qpar_loc.minus(q_m, qoc_m, da2_m); ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10); - } TEST(TestLocalParametrization, QuaternionGlobal) { // Storage VectorXd x_storage(22); - MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size. + MatrixXd M_storage(1, 12); // matrix dimensions do not matter, just storage size. x_storage.setRandom(); M_storage.setZero(); // QUATERNION ------------------------------------------ - Map<VectorXd> q(&x_storage(0),4); + Map<VectorXd> q(&x_storage(0), 4); q.normalize(); - Map<VectorXd> da(&x_storage(4),3); + Map<VectorXd> da(&x_storage(4), 3); da /= 10.0; - Map<VectorXd> qo_m(&x_storage(7),4); - Map<MatrixRowXd> J(&M_storage(0,0),4,3); - MatrixXd J_num(4,3); + Map<VectorXd> qo_m(&x_storage(7), 4); + Map<MatrixRowXd> J(&M_storage(0, 0), 4, 3); + MatrixXd J_num(4, 3); LocalParametrizationQuaternion<DQ_GLOBAL> Qpar_glob; - Map<const VectorXd> q_m(q.data(),4); - Map<const VectorXd> da_m(da.data(),3); + Map<const VectorXd> q_m(q.data(), 4); + Map<const VectorXd> da_m(da.data(), 3); - Qpar_glob.plus(q_m,da_m,qo_m); + Qpar_glob.plus(q_m, da_m, qo_m); ASSERT_DOUBLE_EQ(qo_m.norm(), 1); - Quaterniond qref = wolf::v2q(da) * Map<Quaterniond>(q.data()); - ASSERT_QUATERNION_VECTOR_APPROX(qo_m,qref.coeffs(),Constants::EPS); + Quaterniond qref = wolf::v2q(da) * Map<Quaterniond>(q.data()); + ASSERT_QUATERNION_VECTOR_APPROX(qo_m, qref.coeffs(), Constants::EPS); - Qpar_glob.computeJacobian(q_m,J); + Qpar_glob.computeJacobian(q_m, J); JAC_NUMERIC(Qpar_glob, q_m, J_num, 1e-9) - ASSERT_NEAR((J-J_num).norm(), 0, 1e-6); + ASSERT_NEAR((J - J_num).norm(), 0, 1e-6); Map<const VectorXd> qoc_m(qo_m.data(), 4); Map<VectorXd> da2_m(x_storage.data() + 10, 3); @@ -136,37 +133,36 @@ TEST(TestLocalParametrization, QuaternionGlobal) Qpar_glob.minus(q_m, qoc_m, da2_m); ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10); - } TEST(TestLocalParametrization, Homogeneous) { // Storage VectorXd x_storage(22); - MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size. + MatrixXd M_storage(1, 12); // matrix dimensions do not matter, just storage size. // HOMOGENEOUS ---------------------------------------- - Map<VectorXd> h(&x_storage(11),4); + Map<VectorXd> h(&x_storage(11), 4); h.setRandom(); - Map<VectorXd> d(&x_storage(15),3); - d << .1,.2,.3; - Map<VectorXd> ho_m(&x_storage(18),4); - Map<MatrixRowXd> J(&M_storage(0,0),4,3); - MatrixXd J_num(4,3); + Map<VectorXd> d(&x_storage(15), 3); + d << .1, .2, .3; + Map<VectorXd> ho_m(&x_storage(18), 4); + Map<MatrixRowXd> J(&M_storage(0, 0), 4, 3); + MatrixXd J_num(4, 3); LocalParametrizationHomogeneous Hpar; - Map<const VectorXd> h_m(h.data(),4); - Map<const VectorXd> d_m(d.data(),3); + Map<const VectorXd> h_m(h.data(), 4); + Map<const VectorXd> d_m(d.data(), 3); - Hpar.plus(h_m,d_m,ho_m); + Hpar.plus(h_m, d_m, ho_m); ASSERT_DOUBLE_EQ(ho_m.norm(), h.norm()); - Hpar.computeJacobian(h_m,J); + Hpar.computeJacobian(h_m, J); JAC_NUMERIC(Hpar, h_m, J_num, 1e-9) - ASSERT_NEAR((J-J_num).norm(), 0, 1e-6); + ASSERT_NEAR((J - J_num).norm(), 0, 1e-6); Map<const VectorXd> hoc_m(ho_m.data(), 4); Map<VectorXd> d2_m(x_storage.data() + 10, 3); @@ -174,12 +170,10 @@ TEST(TestLocalParametrization, Homogeneous) Hpar.minus(h_m, hoc_m, d2_m); ASSERT_MATRIX_APPROX(d_m, d2_m, 1e-10); - } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_logging.cpp b/test/gtest_logging.cpp index 5b6add59afcf6e269b5eb2e9e4badd7483071c47..0f843330a5db975fefe2fc6ce7d436aefa715a3f 100644 --- a/test/gtest_logging.cpp +++ b/test/gtest_logging.cpp @@ -21,7 +21,6 @@ #include "core/common/wolf.h" #include "core/utils/utils_gtest.h" - TEST(logging, info) { WOLF_INFO("test info ", 5, " ", 0.123); @@ -49,7 +48,6 @@ TEST(logging, debug) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp index 40bf058829cb85e641b0024be5bb37e6df959ca3..1fc54de9fd095d5057557092ef0de56fc6dd13d8 100644 --- a/test/gtest_make_posdef.cpp +++ b/test/gtest_make_posdef.cpp @@ -27,7 +27,7 @@ using namespace wolf; TEST(MakePosDefTest, OkTest) { - MatrixXd M = MatrixXd::Identity(3,3); + MatrixXd M = MatrixXd::Identity(3, 3); EXPECT_TRUE(isCovariance(M)); EXPECT_FALSE(makePosDef(M)); @@ -35,7 +35,7 @@ TEST(MakePosDefTest, OkTest) TEST(MakePosDefTest, FixTest) { - MatrixXd M = MatrixXd::Zero(3,3); + MatrixXd M = MatrixXd::Zero(3, 3); EXPECT_FALSE(isCovariance(M)); EXPECT_TRUE(makePosDef(M)); @@ -44,6 +44,6 @@ TEST(MakePosDefTest, FixTest) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp index d811cacc9977e1491e637a19caad5b9dd8893d3f..51a8aa9eb3dbd48ea504138b63660c80f206f4ef 100644 --- a/test/gtest_map_yaml.cpp +++ b/test/gtest_map_yaml.cpp @@ -45,11 +45,11 @@ Vector3d p3_3d = Vector3d::Random(); Vector4d o2_3d = Vector4d::Random().normalized(); Vector4d o3_3d = Vector4d::Random().normalized(); -int int_param = (int) (Vector1d::Random()(0) * 1e4); +int int_param = (int)(Vector1d::Random()(0) * 1e4); double double_param = Vector1d::Random()(0) * 1e4; std::string wolf_dir = _WOLF_CODE_DIR; -std::string map_path = wolf_dir + "/test/yaml/maps"; +std::string map_path = wolf_dir + "/test/yaml/maps"; TEST(MapYaml, save_2d) { @@ -66,7 +66,7 @@ TEST(MapYaml, save_2d) LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), p3_sb, o3_sb, int_param, double_param); // Check Problem functions - std::string filename = map_path + "/map_2d_problem.yaml"; + std::string filename = map_path + "/map_2d_problem.yaml"; std::cout << "Saving map to file: " << filename << std::endl; problem->saveMap(filename, "2d map saved from Problem::saveMap()"); @@ -84,7 +84,7 @@ TEST(MapYaml, load_2d) for (auto map : maps) { - std::string filename = map_path + "/" + map; + std::string filename = map_path + "/" + map; std::cout << "Loading map to file: " << filename << std::endl; problem->loadMap(filename); @@ -130,8 +130,7 @@ TEST(MapYaml, load_2d) // empty the map { auto lmk_list = problem->getMap()->getLandmarkList(); - for (auto lmk : lmk_list) - lmk->remove(); + for (auto lmk : lmk_list) lmk->remove(); } ASSERT_TRUE(problem->getMap()->getLandmarkList().empty()); } @@ -152,7 +151,7 @@ TEST(MapYaml, save_3d) LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), p3_sb, o3_sb, int_param, double_param); // Check Problem functions - std::string filename = map_path + "/map_3d_problem.yaml"; + std::string filename = map_path + "/map_3d_problem.yaml"; std::cout << "Saving map to file: " << filename << std::endl; problem->saveMap(filename, "3d map saved from Problem::saveMap()"); @@ -165,12 +164,12 @@ TEST(MapYaml, save_3d) TEST(MapYaml, load_3d) { ProblemPtr problem = Problem::create("PO", 3); - + std::list<std::string> maps{"map_3d_problem.yaml", "map_3d_map.yaml"}; for (auto map : maps) { - std::string filename = map_path + "/" + map; + std::string filename = map_path + "/" + map; std::cout << "Loading map to file: " << filename << std::endl; problem->loadMap(filename); @@ -218,8 +217,7 @@ TEST(MapYaml, load_3d) // empty the map { auto lmk_list = problem->getMap()->getLandmarkList(); - for (auto lmk : lmk_list) - lmk->remove(); + for (auto lmk : lmk_list) lmk->remove(); } ASSERT_TRUE(problem->getMap()->getLandmarkList().empty()); } @@ -227,18 +225,18 @@ TEST(MapYaml, load_3d) TEST(MapYaml, remove_map_files) { - std::string filename = map_path + "/map_2d_problem.yaml"; + std::string filename = map_path + "/map_2d_problem.yaml"; ASSERT_TRUE(std::remove(filename.c_str()) == 0); - filename = map_path + "/map_2d_map.yaml"; + filename = map_path + "/map_2d_map.yaml"; ASSERT_TRUE(std::remove(filename.c_str()) == 0); - filename = map_path + "/map_3d_problem.yaml"; + filename = map_path + "/map_3d_problem.yaml"; ASSERT_TRUE(std::remove(filename.c_str()) == 0); - filename = map_path + "/map_3d_map.yaml"; + filename = map_path + "/map_3d_map.yaml"; ASSERT_TRUE(std::remove(filename.c_str()) == 0); } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp index 7a9f35d4680c9dc80ada91af08a712dd91d5d56d..c9ee303dea57d4486836de3effac3aca4b3d9472 100644 --- a/test/gtest_motion_buffer.cpp +++ b/test/gtest_motion_buffer.cpp @@ -33,22 +33,23 @@ using namespace wolf; Motion newMotion(TimeStamp t, double d, double D, double C, double J_d, double J_D) { Motion m(t, 1, 1, 1, 0); - m.delta_(0) = d; - m.delta_integr_(0) = D; - m.delta_cov_(0) = C; - m.jacobian_delta_(0,0) = J_d; - m.jacobian_delta_integr_(0,0) = J_D; + m.delta_(0) = d; + m.delta_integr_(0) = D; + m.delta_cov_(0) = C; + m.jacobian_delta_(0, 0) = J_d; + m.jacobian_delta_integr_(0, 0) = J_D; return m; } -namespace{ +namespace +{ TimeStamp t0(0), t1(1), t2(2), t3(3), t4(4); -Motion m0 = newMotion(t0, 0, 0 , 0, .1, 1); // ts, delta, Delta, delta_cov, J_delta, J_Delta -Motion m1 = newMotion(t1, 1, 1 , 1, .1, 1); -Motion m2 = newMotion(t2, 2, 3 , 1, .1, 1); -Motion m3 = newMotion(t3, 3, 6 , 1, .1, 1); -Motion m4 = newMotion(t4, 4, 10, 1, .1, 1); -} +Motion m0 = newMotion(t0, 0, 0, 0, .1, 1); // ts, delta, Delta, delta_cov, J_delta, J_Delta +Motion m1 = newMotion(t1, 1, 1, 1, .1, 1); +Motion m2 = newMotion(t2, 2, 3, 1, .1, 1); +Motion m3 = newMotion(t3, 3, 6, 1, .1, 1); +Motion m4 = newMotion(t4, 4, 10, 1, .1, 1); +} // namespace TEST(MotionBuffer, QueryTimeStamps) { @@ -58,20 +59,20 @@ TEST(MotionBuffer, QueryTimeStamps) MB.push_back(m1); TimeStamp t; - t.set(-1); // t is older than m0.ts_ -> return m0 - ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_); + t.set(-1); // t is older than m0.ts_ -> return m0 + ASSERT_EQ(MB.getMotion(t).ts_, m0.ts_); - t.set( 0); // t is exactly m0.ts_ -> return m0 - ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_); + t.set(0); // t is exactly m0.ts_ -> return m0 + ASSERT_EQ(MB.getMotion(t).ts_, m0.ts_); - t.set(0.5); // t is between m0.ts_ and m1.ts_ -> return m0 - ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_); + t.set(0.5); // t is between m0.ts_ and m1.ts_ -> return m0 + ASSERT_EQ(MB.getMotion(t).ts_, m0.ts_); - t.set(+1); // t is exactly m1.ts_ -> return m1 - ASSERT_EQ(MB.getMotion(t).ts_ , m1.ts_); + t.set(+1); // t is exactly m1.ts_ -> return m1 + ASSERT_EQ(MB.getMotion(t).ts_, m1.ts_); - t.set(+2); // t is newer than m1.ts_ -> return m1 - ASSERT_EQ(MB.getMotion(t).ts_ , m1.ts_); + t.set(+2); // t is newer than m1.ts_ -> return m1 + ASSERT_EQ(MB.getMotion(t).ts_, m1.ts_); } TEST(MotionBuffer, getMotion) @@ -110,64 +111,64 @@ TEST(MotionBuffer, Split) MB.push_back(m1); MB.push_back(m2); MB.push_back(m3); - MB.push_back(m4); // put 5 motions + MB.push_back(m4); // put 5 motions MotionBuffer MB_old; - TimeStamp t = 1.5; // between m1 and m2 + TimeStamp t = 1.5; // between m1 and m2 MB.split(t, MB_old); - ASSERT_EQ(MB_old.size(), (unsigned int) 2); - ASSERT_EQ(MB .size(), (unsigned int) 3); + ASSERT_EQ(MB_old.size(), (unsigned int)2); + ASSERT_EQ(MB.size(), (unsigned int)3); ASSERT_EQ(MB_old.getMotion(t1).ts_, t1); - ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1 - ASSERT_EQ(MB .getMotion(t1).ts_, t2); // first ts is t2 - ASSERT_EQ(MB .getMotion(t2).ts_, t2); + ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1 + ASSERT_EQ(MB.getMotion(t1).ts_, t2); // first ts is t2 + ASSERT_EQ(MB.getMotion(t2).ts_, t2); } // TEST(MotionBuffer, integrateCovariance) // { // MotionBuffer MB; -// +// // MB.push_back(m0); // MB.push_back(m1); // MB.push_back(m2); // MB.push_back(m3); // MB.push_back(m4); // put 5 motions -// +// // Eigen::MatrixXd cov = MB.integrateCovariance(); // ASSERT_NEAR(cov(0), 0.04, 1e-8); -// +// // } -// +// // TEST(MotionBuffer, integrateCovariance_ts) // { // MotionBuffer MB; -// +// // MB.push_back(m0); // MB.push_back(m1); // MB.push_back(m2); // MB.push_back(m3); // MB.push_back(m4); // put 5 motions -// +// // Eigen::MatrixXd cov = MB.integrateCovariance(t2); // ASSERT_NEAR(cov(0), 0.02, 1e-8); // } -// +// // TEST(MotionBuffer, integrateCovariance_ti_tf) // { // MotionBuffer MB; -// +// // MB.push_back(m0); // MB.push_back(m1); // MB.push_back(m2); // MB.push_back(m3); // MB.push_back(m4); // put 5 motions -// +// // Eigen::MatrixXd cov = MB.integrateCovariance(t1, t3); // ASSERT_NEAR(cov(0), 0.03, 1e-8); -// +// // cov = MB.integrateCovariance(t0, t3); // first delta_cov is zero so it does not integrate // ASSERT_NEAR(cov(0), 0.03, 1e-8); // } @@ -181,17 +182,16 @@ TEST(MotionBuffer, print) MB.push_back(m2); MB.print(); - MB.print(0,0,0,0); - MB.print(1,0,0,0); - MB.print(0,1,0,0); - MB.print(0,0,1,0); - MB.print(0,0,0,1); - MB.print(1,1,1,1); + MB.print(0, 0, 0, 0); + MB.print(1, 0, 0, 0); + MB.print(0, 1, 0, 0); + MB.print(0, 0, 1, 0); + MB.print(0, 0, 0, 1); + MB.print(1, 1, 1, 1); } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp index aad3b80beccfdca35668ef556151b8eb0610bfbf..51152122d45d6e607c4fd04cbf953434df831ec2 100644 --- a/test/gtest_motion_provider.cpp +++ b/test/gtest_motion_provider.cpp @@ -60,13 +60,13 @@ class MotionProviderTest : public testing::Test wolf_dir + "/test/yaml/processor_motion_provider_dummy1.yaml", {wolf_dir}); mp1 = std::dynamic_pointer_cast<MotionProvider>(prc1); - + prc2 = problem->installProcessor("ProcessorMotionProviderDummy", sen, wolf_dir + "/test/yaml/processor_motion_provider_dummy2.yaml", {wolf_dir}); mp2 = std::dynamic_pointer_cast<MotionProvider>(prc2); - + prc3 = problem->installProcessor("ProcessorMotionProviderDummyPOV", sen, wolf_dir + "/test/yaml/processor_motion_provider_dummy3.yaml", diff --git a/test/gtest_node_state_blocks.cpp b/test/gtest_node_state_blocks.cpp index 002776eb1e1c0aacfb9d147255ecf3d7c7f72015..6ee7208a79c080a4547149fb71fc20b55624ccfa 100644 --- a/test/gtest_node_state_blocks.cpp +++ b/test/gtest_node_state_blocks.cpp @@ -122,7 +122,7 @@ TEST(NodeStateBlocksTest, constructor_specs) ASSERT_FALSE(N->isFrame()); ASSERT_FALSE(N->isLandmark()); ASSERT_FALSE(N->isSensor()); - + // apply priors N->emplacePriors(specs); @@ -165,8 +165,11 @@ TEST(NodeStateBlocksTest, constructor_types_vectors) ASSERT_FALSE(N->isSensor()); // apply priors - vectors = VectorComposite({{'P', Vector3d::Zero()}, {'O', Quaterniond::Identity().coeffs()}, {'I', Vector5d::Zero()}, {'A', Vector1d::Zero()}}); - N->emplacePriors(SpecStateComposite(types, vectors, true)); // changes states and fix + vectors = VectorComposite({{'P', Vector3d::Zero()}, + {'O', Quaterniond::Identity().coeffs()}, + {'I', Vector5d::Zero()}, + {'A', Vector1d::Zero()}}); + N->emplacePriors(SpecStateComposite(types, vectors, true)); // changes states and fix checkNode(N, 'P', Vector3d::Zero(), true, MatrixXd(0, 0)); checkNode(N, 'O', Quaterniond::Identity().coeffs(), true, MatrixXd(0, 0)); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 9d73512493f377099f1cbe531c9c29defaa1e6d8..c353ccaddebf009a8602c1c2b76289c1d41c1d6f 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -324,7 +324,7 @@ TEST(Problem, StateBlocks) // consume notifications SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); - SM->update(); // calls P->consumeNotifications(); + SM->update(); // calls P->consumeNotifications(); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(0)); // consumeNotifications empties the notification map @@ -383,7 +383,8 @@ TEST(Problem, perturb) auto problem = Problem::create("PO", 2); // make a sensor first - auto sensor = problem->installSensor("SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir}); + auto sensor = problem->installSensor( + "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir}); Vector3d pose; pose << 0, 0, 0; @@ -462,9 +463,11 @@ TEST(Problem, check) auto problem = Problem::create("PO", 2); // make sensors first - auto sensor = problem->installSensor("SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir}); + auto sensor = problem->installSensor( + "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir}); - auto sensor_lmk = problem->installSensor("SensorDummy2d", wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_dir}); + auto sensor_lmk = problem->installSensor( + "SensorDummy2d", wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_dir}); // landmarks for (int l = 0; l < 5; l++) diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 96c3ba38aa71397c5c6db9ed6f258d07b9cdb702..a32fc468932f9a8a0c908c3ff1569fd68421893b 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -124,8 +124,9 @@ TEST(ProcessorBase, KeyFrameCallback) // keyframe creation if (ii == 5) - problem->emplaceFrame(t, SpecStateComposite{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "initial_guess")}, - {'O', SpecState("StateAngle", Vector1d(0), "initial_guess")}}); + problem->emplaceFrame(t, + SpecStateComposite{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "initial_guess")}, + {'O', SpecState("StateAngle", Vector1d(0), "initial_guess")}}); problem->print(4, 1, 1, 0); std::cout << "6\n"; diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp index f8027176cbe2c977292e9083b93a36c67c7a55d2..a6fa39febbd5adca3371c6402755651a2057852c 100644 --- a/test/gtest_processor_fixed_wing_model.cpp +++ b/test/gtest_processor_fixed_wing_model.cpp @@ -37,26 +37,27 @@ std::string wolf_dir = _WOLF_CODE_DIR; class ProcessorFixedWingModelTest : public testing::Test { - protected: - ProblemPtr problem; - SolverManagerPtr solver; - SensorBasePtr sensor; - ProcessorBasePtr processor; - - virtual void SetUp() - { - // create problem - problem = Problem::create("POV", 3); - - // create solver - solver = std::make_shared<SolverCeres>(problem); - - // Emplace sensor - sensor = problem->installSensor("SensorMotionModel", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir}); - - // Emplace processor - processor = problem->installProcessor("ProcessorFixedWingModel", sensor, wolf_dir + "/test/yaml/processor_fixed_wing_model.yaml", {wolf_dir}); - } + protected: + ProblemPtr problem; + SolverManagerPtr solver; + SensorBasePtr sensor; + ProcessorBasePtr processor; + + virtual void SetUp() + { + // create problem + problem = Problem::create("POV", 3); + + // create solver + solver = std::make_shared<SolverCeres>(problem); + + // Emplace sensor + sensor = problem->installSensor("SensorMotionModel", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir}); + + // Emplace processor + processor = problem->installProcessor( + "ProcessorFixedWingModel", sensor, wolf_dir + "/test/yaml/processor_fixed_wing_model.yaml", {wolf_dir}); + } }; TEST_F(ProcessorFixedWingModelTest, setup) @@ -67,7 +68,7 @@ TEST_F(ProcessorFixedWingModelTest, setup) TEST_F(ProcessorFixedWingModelTest, keyFrameCallback) { // new frame - auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); + auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished()); // keyframecallback problem->keyFrameCallback(frm1, nullptr); @@ -91,7 +92,7 @@ TEST_F(ProcessorFixedWingModelTest, keyFrameCallback) TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated) { // new frame - auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); + auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished()); // keyframecallback problem->keyFrameCallback(frm1, nullptr); @@ -118,7 +119,7 @@ TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated) TEST_F(ProcessorFixedWingModelTest, solve_origin) { // new frame - auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); + auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished()); // keyframecallback problem->keyFrameCallback(frm1, nullptr); @@ -141,6 +142,6 @@ TEST_F(ProcessorFixedWingModelTest, solve_origin) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp index 13a5d9c66acf2209c01d0f69ecd40a513c1de346..c43e75fd1a93a320e42b7f6e1a4816b58e188f52 100644 --- a/test/gtest_processor_landmark_external.cpp +++ b/test/gtest_processor_landmark_external.cpp @@ -101,7 +101,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, // Sensors sensor = problem->installSensor("SensorOdom" + toString(dim) + "d", wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml", - {wolf_dir}); + {wolf_dir}); sensor_odom = problem->installSensor("SensorOdom" + toString(dim) + "d", wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml", {wolf_dir}); diff --git a/test/gtest_processor_odom_2d.cpp b/test/gtest_processor_odom_2d.cpp index 5d058ea76d28346af232fc0bb2f0b53e5295a7f0..65b356bd7eededed4f4defb69a6b6d0c0cc1cee4 100644 --- a/test/gtest_processor_odom_2d.cpp +++ b/test/gtest_processor_odom_2d.cpp @@ -92,7 +92,7 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve) // motion data VectorXd data(Vector2d(1, M_PI_4)); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01; - int N = 7; // number of process() steps + int N = 7; // number of process() steps // Create Wolf tree nodes ProblemPtr problem = Problem::create("PO", 2); @@ -221,7 +221,7 @@ TEST(ProcessorOdom2d, KF_callback) double dt = .01; VectorXd data(Vector2d(1, M_PI_4)); // advance 1m Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01; - int N = 8; // number of process() steps + int N = 8; // number of process() steps // NOTE: We integrate and create KFs as follows: // n = 0 1 2 3 4 5 6 7 8 diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp index 4fbe1749055b214e1e3e77b66e9e344cd5096ea3..41da876257905410f3803e511482e92bc4c57006 100644 --- a/test/gtest_processor_tracker_feature_dummy.cpp +++ b/test/gtest_processor_tracker_feature_dummy.cpp @@ -122,7 +122,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processNew) CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor); processor->setInc(inc_cap); - auto n_new_feat = processor->callProcessNew(10); // detect 10 features + auto n_new_feat = processor->callProcessNew(10); // detect 10 features ASSERT_EQ(n_new_feat, 10); // detected 10 features ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features @@ -143,13 +143,13 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown) CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor); processor->setInc(inc_cap); - processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking + processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost - processor->callReset(); // now incoming is last and last is origin + processor->callReset(); // now incoming is last and last is origin // Put a capture on last_ptr_ CaptureBasePtr new_cap = std::make_shared<CaptureVoid>(0, sensor); @@ -206,13 +206,13 @@ TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors) CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor); processor->setInc(inc_cap); - processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking + processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost - processor->callReset(); // now incoming is last and last is origin + processor->callReset(); // now incoming is last and last is origin // test establishFactors() processor->callEstablishFactors(); diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp index a8194b169b87988708f17440dc5fa413a368989a..3f8dae8a4cd65384ff31e4f564794f6dbc530930 100644 --- a/test/gtest_processor_tracker_landmark_dummy.cpp +++ b/test/gtest_processor_tracker_landmark_dummy.cpp @@ -170,7 +170,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processNew) auto n_new_feat = processor->callProcessNew(10); // detect 10 features in last, create landmarks, find landmarks in incoming - ASSERT_EQ(n_new_feat, 10); // detected 10 features + ASSERT_EQ(n_new_feat, 10); // detected 10 features ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 10); // created 10 landmarks ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found @@ -245,9 +245,9 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors) CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor); processor->setInc(inc_cap); - processor->callProcessNew(15); // detect 15 features, 1 of each 10 tracks is lost + processor->callProcessNew(15); // detect 15 features, 1 of each 10 tracks is lost - ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features + ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 of each 10 tracks is lost ASSERT_EQ(processor->getMatchesLandmarkFromLast().size(), 15); // all landmarks ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 14); // 1 of each 10 tracks is lost diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp index 0cba6a4f5cea42c2259308e337329d4a81688251..565a2003edb4f389457a82495283c35556013f5c 100644 --- a/test/gtest_rotation.cpp +++ b/test/gtest_rotation.cpp @@ -18,14 +18,14 @@ // 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/>. -//Eigen +// Eigen #include <Eigen/Geometry> -//Wolf +// Wolf #include "core/common/wolf.h" #include "core/math/rotations.h" -//std +// std #include <iostream> #include <fstream> #include <iomanip> @@ -42,13 +42,13 @@ using namespace Eigen; TEST(exp_q, unit_norm) { - Vector3d v0 = Vector3d::Random(); - double scale = 1.0; + Vector3d v0 = Vector3d::Random(); + double scale = 1.0; for (int i = 0; i < 20; i++) { - Vector3d v = v0 * scale; + Vector3d v = v0 * scale; Quaterniond q = exp_q(v); - ASSERT_NEAR(q.norm(), 1.0, 1e-10) << "Failed at scale 1e-" << i << " with angle = " << 2.0*q.vec().norm(); + ASSERT_NEAR(q.norm(), 1.0, 1e-10) << "Failed at scale 1e-" << i << " with angle = " << 2.0 * q.vec().norm(); scale /= 10; } } @@ -56,12 +56,12 @@ TEST(exp_q, unit_norm) TEST(rotations, pi2pi) { ASSERT_NEAR(M_PI_2, pi2pi((double)M_PI_2), 1e-10); - ASSERT_NEAR(-M_PI_2, pi2pi(3.0*M_PI_2), 1e-10); + ASSERT_NEAR(-M_PI_2, pi2pi(3.0 * M_PI_2), 1e-10); ASSERT_NEAR(-M_PI_2, pi2pi(-M_PI_2), 1e-10); - ASSERT_NEAR(M_PI_2, pi2pi(-3.0*M_PI_2), 1e-10); + ASSERT_NEAR(M_PI_2, pi2pi(-3.0 * M_PI_2), 1e-10); // ASSERT_NEAR(M_PI, pi2pi(M_PI), 1e-10); // Exact PI is not safely testable because of numeric issues. - ASSERT_NEAR(M_PI-.01, pi2pi(M_PI-.01), 1e-10); - ASSERT_NEAR(-M_PI+.01, pi2pi(M_PI+.01), 1e-10); + ASSERT_NEAR(M_PI - .01, pi2pi(M_PI - .01), 1e-10); + ASSERT_NEAR(-M_PI + .01, pi2pi(M_PI + .01), 1e-10); } TEST(skew, Skew_vee) @@ -81,27 +81,27 @@ TEST(skew, Skew_vee) TEST(exp_q, v2q_q2v) { using namespace wolf; - //defines scalars - double deg_to_rad = M_PI/180.0; + // defines scalars + double deg_to_rad = M_PI / 180.0; Vector4d vec0, vec1; - //v2q + // v2q Vector3d rot_vector0, rot_vector1; rot_vector0 = Vector3d::Random(); - rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin - rot_vector0 = rot_vector0*deg_to_rad; + rot_vector1 = rot_vector0 * 100 * deg_to_rad; // far from origin + rot_vector0 = rot_vector0 * deg_to_rad; Quaterniond quat0, quat1; quat0 = v2q(rot_vector0); quat1 = v2q(rot_vector1); - //q2v + // q2v Vector3d quat_to_v0, quat_to_v1; VectorXd quat_to_v0x, quat_to_v1x; - quat_to_v0 = q2v(quat0); - quat_to_v1 = q2v(quat1); + quat_to_v0 = q2v(quat0); + quat_to_v1 = q2v(quat1); quat_to_v0x = q2v(quat0); quat_to_v1x = q2v(quat1); @@ -114,27 +114,27 @@ TEST(exp_q, v2q_q2v) TEST(exp_R, v2R_R2v) { using namespace wolf; - //First test is to verify we get the good result with v -> v2R -> R2v -> v - //test 2 : how small can angles in rotation vector be ? + // First test is to verify we get the good result with v -> v2R -> R2v -> v + // test 2 : how small can angles in rotation vector be ? - //definition - double deg_to_rad = M_PI/180.0; + // definition + double deg_to_rad = M_PI / 180.0; Vector3d rot_vector0, rot_vector1; rot_vector0 = Vector3d::Random(); - rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin - rot_vector0 = rot_vector0*deg_to_rad; + rot_vector1 = rot_vector0 * 100 * deg_to_rad; // far from origin + rot_vector0 = rot_vector0 * deg_to_rad; Matrix3d rot0, rot1; rot0 = v2R(rot_vector0); rot1 = v2R(rot_vector1); - //R2v + // R2v Vector3d rot0_vec, rot1_vec; rot0_vec = R2v(rot0); rot1_vec = R2v(rot1); - //check now + // check now ASSERT_MATRIX_APPROX(rot0_vec, rot_vector0, wolf::Constants::EPS); ASSERT_MATRIX_APPROX(rot1_vec, rot_vector1, wolf::Constants::EPS); } @@ -142,41 +142,43 @@ TEST(exp_R, v2R_R2v) TEST(log_R, R2v_v2R_limits) { using namespace wolf; - //test 2 : how small can angles in rotation vector be ? - double scale = 1; + // test 2 : how small can angles in rotation vector be ? + double scale = 1; Matrix3d v_to_R, initial_matrix; - Vector3d R_to_v; + Vector3d R_to_v; - //Vector3d rv; - for(int i = 0; i<8; i++){ + // Vector3d rv; + for (int i = 0; i < 8; i++) + { initial_matrix = v2R(Vector3d::Random().eval() * scale); - R_to_v = R2v(initial_matrix); + R_to_v = R2v(initial_matrix); v_to_R = v2R(R_to_v); ASSERT_MATRIX_APPROX(v_to_R, initial_matrix, wolf::Constants::EPS); - scale = scale*0.1; + scale = scale * 0.1; } } TEST(log_R, R2v_v2R_AAlimits) { using namespace wolf; - //let's see how small the angles can be here : limit reached at scale/10 = 1e-16 - double scale = 1; + // let's see how small the angles can be here : limit reached at scale/10 = 1e-16 + double scale = 1; Matrix3d rotation_mat; Vector3d rv; - for(int i = 0; i<8; i++){ + for (int i = 0; i < 8; i++) + { rotation_mat = v2R(Vector3d::Random().eval() * scale); - //rv = R2v(rotation_mat); //decomposing R2v below + // rv = R2v(rotation_mat); //decomposing R2v below AngleAxis<double> aa0 = AngleAxis<double>(rotation_mat); - rv = aa0.axis() * aa0.angle(); - //std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl; + rv = aa0.axis() * aa0.angle(); + // std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl; ASSERT_FALSE(rv == Vector3d::Zero()); - scale = scale*0.1; + scale = scale * 0.1; } } @@ -186,47 +188,50 @@ TEST(exp_q, v2q2R2v) double scale = 1; // testing new path : vector -> quaternion -> matrix -> vector - for(int i = 0; i< 8; i++){ - Vector3d vector_ = Vector3d::Random()*scale; - Quaterniond quat_ = v2q(vector_); - Matrix3d mat_ = quat_.matrix(); - Vector3d vector_bis = R2v(mat_); + for (int i = 0; i < 8; i++) + { + Vector3d vector_ = Vector3d::Random() * scale; + Quaterniond quat_ = v2q(vector_); + Matrix3d mat_ = quat_.matrix(); + Vector3d vector_bis = R2v(mat_); ASSERT_MATRIX_APPROX(vector_, vector_bis, wolf::Constants::EPS); - scale = scale*0.1; + scale = scale * 0.1; } } TEST(rotations, AngleAxis_limits) { using namespace wolf; - //Hypothesis : problem with construction of AngleAxis objects. - // Example : if R = I + [delta t]_x (happenning in the IMU case with delta t = 0.001). Then angle mays be evaluated as 0 (due to cosinus definition ?) - // Here we try to get the AngleAxis object from a random rotation matrix, then get back to the rotation matrix using AngleAxis::toRotationMatrix() + // Hypothesis : problem with construction of AngleAxis objects. + // Example : if R = I + [delta t]_x (happenning in the IMU case with delta t = 0.001). Then angle mays be evaluated + // as 0 (due to cosinus definition ?) Here we try to get the AngleAxis object from a random rotation matrix, then + // get back to the rotation matrix using AngleAxis::toRotationMatrix() - double scale = 1; + double scale = 1; Matrix3d res, res_i, rotation_mati, rotation_mat; Vector3d rv; - for(int i = 0; i<8; i++){ //FIX ME : Random() will not create a rotation matrix. Then, R2v(Random_matrix()) makes no sense at all. - rotation_mat = v2R(Vector3d::Random().eval() * scale); + for (int i = 0; i < 8; i++) + { // FIX ME : Random() will not create a rotation matrix. Then, R2v(Random_matrix()) makes no sense at all. + rotation_mat = v2R(Vector3d::Random().eval() * scale); rotation_mati = rotation_mat; - //rv = R2v(rotation_mat); //decomposing R2v below + // rv = R2v(rotation_mat); //decomposing R2v below AngleAxis<double> aa0 = AngleAxis<double>(rotation_mat); - rv = aa0.axis() * aa0.angle(); - //std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl; + rv = aa0.axis() * aa0.angle(); + // std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl; res = aa0.toRotationMatrix(); // now we set the diagonal to identity AngleAxis<double> aa1 = AngleAxis<double>(rotation_mat); - rv = aa1.axis() * aa1.angle(); - //std::cout << "aa1.axis : " << aa0.axis().transpose() << ",\t aa1.angles :" << aa0.angle() <<std::endl; + rv = aa1.axis() * aa1.angle(); + // std::cout << "aa1.axis : " << aa0.axis().transpose() << ",\t aa1.angles :" << aa0.angle() <<std::endl; res_i = aa1.toRotationMatrix(); ASSERT_MATRIX_APPROX(res, rotation_mat, wolf::Constants::EPS); ASSERT_MATRIX_APPROX(res_i, rotation_mati, wolf::Constants::EPS); - scale = scale*0.1; + scale = scale * 0.1; } } @@ -234,27 +239,30 @@ TEST(compose, Quat_compos_const_rateOfTurn) { using namespace wolf; - // ********* constant rate of turn ********* + // ********* constant rate of turn ********* - /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3 - (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. - But this is not OK, we cannot expect those 2 rotation integration to be equal. - The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3 - - more specifically : + /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * + dq(w*dt) * q' (mathematically)) and in R3 (q2v(v2q(v0*n*dt))). with v0 + << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. But this + is not OK, we cannot expect those 2 rotation integration to be equal. The whole point of the SO3 thing is that we + cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold + of SO3 + + more specifically : - At a constant velocity, because we keep a constant rotation axis, the integral is the same. - - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3. + - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only + good method is the SO3. We change the idea : define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz. Then compare the final orientation from rotation matrix composition and quaternion composition */ - double deg_to_rad = M_PI/180.0; - Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity()); + double deg_to_rad = M_PI / 180.0; + Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity()); Eigen::Quaterniond q0, qRot; q0.setIdentity(); - Eigen::Vector3d tmp_vec; //will be used to store rate of turn data + Eigen::Vector3d tmp_vec; // will be used to store rate of turn data const unsigned int x_rot_vel = 5; // deg/s const unsigned int y_rot_vel = 2; // deg/s @@ -275,61 +283,65 @@ TEST(compose, Quat_compos_const_rateOfTurn) wz = z_rot_vel; */ - //there is no need to compute the rate of turn at each time because it is constant here : + // there is no need to compute the rate of turn at each time because it is constant here : tmpx = deg_to_rad * x_rot_vel; // rad/s tmpy = deg_to_rad * y_rot_vel; tmpz = deg_to_rad * z_rot_vel; tmp_vec << tmpx, tmpy, tmpz; const double dt = 0.1; - for(unsigned int data_iter = 0; data_iter < 100; data_iter ++) - { - rot0 = rot0 * v2R(tmp_vec*dt); - q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically) - + for (unsigned int data_iter = 0; data_iter < 100; data_iter++) + { + rot0 = rot0 * v2R(tmp_vec * dt); + q0 = q0 * v2q(tmp_vec * dt); // succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * + // q' (mathematically) } // Compare results from rotation matrix composition and quaternion composition - qRot = (v2q(R2v(rot0))); - - Eigen::Vector3d final_orientation(q2v(qRot)); - ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; + qRot = (v2q(R2v(rot0))); + + Eigen::Vector3d final_orientation(q2v(qRot)); + ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, wolf::Constants::EPS)) + << "final orientation expected : " << final_orientation.transpose() + << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; } TEST(compose, Quat_compos_var_rateOfTurn) { using namespace wolf; - //********* changing rate of turn - same freq for all axis ********* + //********* changing rate of turn - same freq for all axis ********* + + /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * + dq(w*dt) * q' (mathematically)) and in R3 (q2v(v2q(v0*n*dt))). with v0 + << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. But this + is not OK, we cannot expect those 2 rotation integration to be equal. The whole point of the SO3 thing is that we + cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold + of SO3 - /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3 - (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. - But this is not OK, we cannot expect those 2 rotation integration to be equal. - The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3 - - more specifically : + more specifically : - At a constant velocity, because we keep a constant rotation axis, the integral is the same. - - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3. + - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only + good method is the SO3. We change the idea : define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz. Then compare the final orientation from ox, oy, oz and quaternion we get by data integration - ******* RESULT : ******* + ******* RESULT : ******* The error in this test is due to discretization. The smaller is dt and the better is the integration ! with dt = 0.001, the error is in 1e-5 */ - double deg_to_rad = M_PI/180.0; - Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity()); + double deg_to_rad = M_PI / 180.0; + Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity()); Eigen::Quaterniond q0, qRot; q0.setIdentity(); - Eigen::Vector3d tmp_vec; //will be used to store rate of turn data - double time = 0; - const unsigned int x_rot_vel = 15; // deg/s - const unsigned int y_rot_vel = 15; // deg/s + Eigen::Vector3d tmp_vec; // will be used to store rate of turn data + double time = 0; + const unsigned int x_rot_vel = 15; // deg/s + const unsigned int y_rot_vel = 15; // deg/s const unsigned int z_rot_vel = 15; // deg/s double tmpx, tmpy, tmpz; @@ -349,29 +361,31 @@ TEST(compose, Quat_compos_var_rateOfTurn) const double dt = 0.001; - for(unsigned int data_iter = 0; data_iter <= 10000; data_iter ++) - { - tmpx = M_PI*x_rot_vel*cos(wolf::toRad(x_rot_vel * time))*deg_to_rad; - tmpy = M_PI*y_rot_vel*cos(wolf::toRad(y_rot_vel * time))*deg_to_rad; - tmpz = M_PI*z_rot_vel*cos(wolf::toRad(z_rot_vel * time))*deg_to_rad; + for (unsigned int data_iter = 0; data_iter <= 10000; data_iter++) + { + tmpx = M_PI * x_rot_vel * cos(wolf::toRad(x_rot_vel * time)) * deg_to_rad; + tmpy = M_PI * y_rot_vel * cos(wolf::toRad(y_rot_vel * time)) * deg_to_rad; + tmpz = M_PI * z_rot_vel * cos(wolf::toRad(z_rot_vel * time)) * deg_to_rad; tmp_vec << tmpx, tmpy, tmpz; - rot0 = rot0 * v2R(tmp_vec*dt); - q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically) + rot0 = rot0 * v2R(tmp_vec * dt); + q0 = q0 * v2q(tmp_vec * dt); // succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * + // q' (mathematically) time += dt; } // Compare results from rotation matrix composition and quaternion composition qRot = (v2q(R2v(rot0))); - + Eigen::Vector3d final_orientation(q2v(qRot)); - - EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; - ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,0.0001)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; + EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, wolf::Constants::EPS)) + << "final orientation expected : " << final_orientation.transpose() + << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; + ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, 0.0001)) + << "final orientation expected : " << final_orientation.transpose() + << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; } TEST(compose, Quat_compos_var_rateOfTurn_diff) @@ -380,33 +394,37 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff) // ********* changing rate of turn - different freq for 1 axis ********* - /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3 - (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. - But this is not OK, we cannot expect those 2 rotation integration to be equal. - The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3 - - more specifically : + /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * + dq(w*dt) * q' (mathematically)) and in R3 (q2v(v2q(v0*n*dt))). with v0 + << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. But this + is not OK, we cannot expect those 2 rotation integration to be equal. The whole point of the SO3 thing is that we + cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold + of SO3 + + more specifically : - At a constant velocity, because we keep a constant rotation axis, the integral is the same. - - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3. + - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only + good method is the SO3. We change the idea : define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz. Then compare the final orientation from ox, oy, oz and quaternion we get by data integration - ******* RESULT : ******* + ******* RESULT : ******* Things are more tricky here. The errors go growing with time. - with dt = 0.001, the error is in 1e-4 for 1 s integration ! But this may also depend on the frequency given to the rotation on each of the axis. + with dt = 0.001, the error is in 1e-4 for 1 s integration ! But this may also depend on the frequency given to the + rotation on each of the axis. */ - double deg_to_rad = M_PI/180.0; - Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity()); + double deg_to_rad = M_PI / 180.0; + Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity()); Eigen::Quaterniond q0, qRot; q0.setIdentity(); - Eigen::Vector3d tmp_vec; //will be used to store rate of turn data - double time = 0; - const unsigned int x_rot_vel = 1; // deg/s - const unsigned int y_rot_vel = 3; // deg/s + Eigen::Vector3d tmp_vec; // will be used to store rate of turn data + double time = 0; + const unsigned int x_rot_vel = 1; // deg/s + const unsigned int y_rot_vel = 3; // deg/s const unsigned int z_rot_vel = 6; // deg/s double tmpx, tmpy, tmpz; @@ -426,178 +444,191 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff) const double dt = 0.001; - for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++) - { - tmpx = M_PI*x_rot_vel*cos(wolf::toRad(x_rot_vel * time))*deg_to_rad; - tmpy = M_PI*y_rot_vel*cos(wolf::toRad(y_rot_vel * time))*deg_to_rad; - tmpz = M_PI*z_rot_vel*cos(wolf::toRad(z_rot_vel * time))*deg_to_rad; + for (unsigned int data_iter = 0; data_iter <= 1000; data_iter++) + { + tmpx = M_PI * x_rot_vel * cos(wolf::toRad(x_rot_vel * time)) * deg_to_rad; + tmpy = M_PI * y_rot_vel * cos(wolf::toRad(y_rot_vel * time)) * deg_to_rad; + tmpz = M_PI * z_rot_vel * cos(wolf::toRad(z_rot_vel * time)) * deg_to_rad; tmp_vec << tmpx, tmpy, tmpz; - rot0 = rot0 * v2R(tmp_vec*dt); - q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically) + rot0 = rot0 * v2R(tmp_vec * dt); + q0 = q0 * v2q(tmp_vec * dt); // succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * + // q' (mathematically) time += dt; } // Compare results from rotation matrix composition and quaternion composition qRot = (v2q(R2v(rot0))); - + Eigen::Vector3d final_orientation(q2v(qRot)); - EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; - - ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,0.001)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; + EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, wolf::Constants::EPS)) + << "final orientation expected : " << final_orientation.transpose() + << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; + + ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, 0.001)) + << "final orientation expected : " << final_orientation.transpose() + << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; } TEST(Plus, Random) { Quaterniond q; - q .coeffs().setRandom().normalize(); + q.coeffs().setRandom().normalize(); Vector3d v; - v .setRandom(); - - Quaterniond q2 = q * exp_q(v); - Quaterniond q3 = exp_q(v) * q; + v.setRandom(); - ASSERT_QUATERNION_APPROX(plus(q,v) , q2, 1e-12); - ASSERT_QUATERNION_APPROX(plus_right(q,v), q2, 1e-12); - ASSERT_QUATERNION_APPROX(plus_left(v,q) , q3, 1e-12); + Quaterniond q2 = q * exp_q(v); + Quaterniond q3 = exp_q(v) * q; + ASSERT_QUATERNION_APPROX(plus(q, v), q2, 1e-12); + ASSERT_QUATERNION_APPROX(plus_right(q, v), q2, 1e-12); + ASSERT_QUATERNION_APPROX(plus_left(v, q), q3, 1e-12); } TEST(Plus, Identity_plus_small) { Quaterniond q; - q .setIdentity(); + q.setIdentity(); Vector3d v; - v .setRandom(); - v *= 1e-6; + v.setRandom(); + v *= 1e-6; Quaterniond q2; - q2.w() = 1; - q2.vec() = 0.5*v; + q2.w() = 1; + q2.vec() = 0.5 * v; - ASSERT_QUATERNION_APPROX(plus(q,v), q2, 1e-12); + ASSERT_QUATERNION_APPROX(plus(q, v), q2, 1e-12); } TEST(Minus_and_diff, Random) { Quaterniond q1, q2, qo; - q1 .coeffs().setRandom().normalize(); - q2 .coeffs().setRandom().normalize(); + q1.coeffs().setRandom().normalize(); + q2.coeffs().setRandom().normalize(); - Vector3d vr = log_q(q1.conjugate() * q2); - Vector3d vl = log_q(q2 * q1.conjugate()); + Vector3d vr = log_q(q1.conjugate() * q2); + Vector3d vl = log_q(q2 * q1.conjugate()); ASSERT_MATRIX_APPROX(minus(q1, q2), vr, 1e-12); ASSERT_MATRIX_APPROX(diff(q1, q2), vr, 1e-12); ASSERT_MATRIX_APPROX(minus_left(q1, q2), vl, 1e-12); qo = plus(q1, minus(q1, q2)); - if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q + if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q ASSERT_QUATERNION_APPROX(qo, q2, 1e-12); qo = plus(q1, diff(q1, q2)); - if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q + if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q ASSERT_QUATERNION_APPROX(qo, q2, 1e-12); qo = plus_left(minus_left(q1, q2), q1); - if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q + if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q ASSERT_QUATERNION_APPROX(qo, q2, 1e-12); } TEST(Jacobians, Jr) { - Vector3d theta; theta.setRandom(); - Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4; + Vector3d theta; + theta.setRandom(); + Vector3d dtheta; + dtheta.setRandom(); + dtheta *= 1e-4; // Check the main Jr property for q and R // exp( theta + d_theta ) \approx exp(theta) * exp(Jr * d_theta) Matrix3d Jr = jac_SO3_right(theta); - ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(theta) * exp_q(Jr*dtheta), 1e-8); - ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(theta) * exp_R(Jr*dtheta)), 1e-8); + ASSERT_QUATERNION_APPROX(exp_q(theta + dtheta), exp_q(theta) * exp_q(Jr * dtheta), 1e-8); + ASSERT_MATRIX_APPROX(exp_R(theta + dtheta), (exp_R(theta) * exp_R(Jr * dtheta)), 1e-8); } TEST(Jacobians, Jl) { - Vector3d theta; theta.setRandom(); - Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4; + Vector3d theta; + theta.setRandom(); + Vector3d dtheta; + dtheta.setRandom(); + dtheta *= 1e-4; // Check the main Jl property for q and R // exp( theta + d_theta ) \approx exp(Jl * d_theta) * exp(theta) Matrix3d Jl = jac_SO3_left(theta); - ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(Jl*dtheta) * exp_q(theta), 1e-8); - ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(Jl*dtheta) * exp_R(theta)), 1e-8); + ASSERT_QUATERNION_APPROX(exp_q(theta + dtheta), exp_q(Jl * dtheta) * exp_q(theta), 1e-8); + ASSERT_MATRIX_APPROX(exp_R(theta + dtheta), (exp_R(Jl * dtheta) * exp_R(theta)), 1e-8); // Jl = Jr.tr ASSERT_MATRIX_APPROX(Jl, jac_SO3_right(theta).transpose(), 1e-8); // Jl = R*Jr - ASSERT_MATRIX_APPROX(Jl, exp_R(theta)*jac_SO3_right(theta), 1e-8); + ASSERT_MATRIX_APPROX(Jl, exp_R(theta) * jac_SO3_right(theta), 1e-8); } TEST(Jacobians, Jr_inv) { - Vector3d theta; theta.setRandom(); - Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4; + Vector3d theta; + theta.setRandom(); + Vector3d dtheta; + dtheta.setRandom(); + dtheta *= 1e-4; Quaterniond q = v2q(theta); Matrix3d R = v2R(theta); // Check the main Jr_inv property for q and R // log( R * exp(d_theta) ) \approx log( R ) + Jrinv * d_theta Matrix3d Jr_inv = jac_SO3_right_inv(theta); - ASSERT_MATRIX_APPROX(log_q(q * exp_q(dtheta)), log_q(q) + Jr_inv*dtheta, 1e-8); - ASSERT_MATRIX_APPROX(log_R(R * exp_R(dtheta)), log_R(R) + Jr_inv*dtheta, 1e-8); + ASSERT_MATRIX_APPROX(log_q(q * exp_q(dtheta)), log_q(q) + Jr_inv * dtheta, 1e-8); + ASSERT_MATRIX_APPROX(log_R(R * exp_R(dtheta)), log_R(R) + Jr_inv * dtheta, 1e-8); } TEST(Jacobians, Jl_inv) { - Vector3d theta; theta.setRandom(); - Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4; + Vector3d theta; + theta.setRandom(); + Vector3d dtheta; + dtheta.setRandom(); + dtheta *= 1e-4; Quaterniond q = v2q(theta); Matrix3d R = v2R(theta); // Check the main Jl_inv property for q and R // log( exp(d_theta) * R ) \approx log( R ) + Jlinv * d_theta Matrix3d Jl_inv = jac_SO3_left_inv(theta); - ASSERT_MATRIX_APPROX(log_q(exp_q(dtheta) * q), log_q(q) + Jl_inv*dtheta, 1e-8); - ASSERT_MATRIX_APPROX(log_R(exp_R(dtheta) * R), log_R(R) + Jl_inv*dtheta, 1e-8); + ASSERT_MATRIX_APPROX(log_q(exp_q(dtheta) * q), log_q(q) + Jl_inv * dtheta, 1e-8); + ASSERT_MATRIX_APPROX(log_R(exp_R(dtheta) * R), log_R(R) + Jl_inv * dtheta, 1e-8); } TEST(Jacobians, compose) { - - Vector3d th1(.1,.2,.3), th2(.3,.1,.2); + Vector3d th1(.1, .2, .3), th2(.3, .1, .2); Quaterniond q1(exp_q(th1)); Quaterniond q2(exp_q(th2)); Quaterniond qc; - Matrix3d J1a, J2a, J1n, J2n; + Matrix3d J1a, J2a, J1n, J2n; // composition and analytic Jacobians wolf::compose(q1, q2, qc, J1a, J2a); // Numeric Jacobians - double dx = 1e-6; - Vector3d pert; + double dx = 1e-6; + Vector3d pert; Quaterniond q1_pert, q2_pert, qc_pert; - for (int i = 0; i<3; i++) + for (int i = 0; i < 3; i++) { pert.setZero(); pert(i) = dx; // Jac wrt q1 - q1_pert = q1*exp_q(pert); - qc_pert = q1_pert * q2; - J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx; + q1_pert = q1 * exp_q(pert); + qc_pert = q1_pert * q2; + J1n.col(i) = log_q(qc.conjugate() * qc_pert) / dx; // Jac wrt q2 - q2_pert = q2*exp_q(pert); - qc_pert = q1 * q2_pert; - J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx; + q2_pert = q2 * exp_q(pert); + qc_pert = q1 * q2_pert; + J2n.col(i) = log_q(qc.conjugate() * qc_pert) / dx; } ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5); @@ -606,34 +637,33 @@ TEST(Jacobians, compose) TEST(Jacobians, between) { - - Vector3d th1(.1,.2,.3), th2(.3,.1,.2); + Vector3d th1(.1, .2, .3), th2(.3, .1, .2); Quaterniond q1(exp_q(th1)); Quaterniond q2(exp_q(th2)); Quaterniond qc; - Matrix3d J1a, J2a, J1n, J2n; + Matrix3d J1a, J2a, J1n, J2n; // composition and analytic Jacobians wolf::between(q1, q2, qc, J1a, J2a); // Numeric Jacobians - double dx = 1e-6; - Vector3d pert; + double dx = 1e-6; + Vector3d pert; Quaterniond q1_pert, q2_pert, qc_pert; - for (int i = 0; i<3; i++) + for (int i = 0; i < 3; i++) { pert.setZero(); pert(i) = dx; // Jac wrt q1 - q1_pert = q1*exp_q(pert); - qc_pert = q1_pert.conjugate() * q2; - J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx; + q1_pert = q1 * exp_q(pert); + qc_pert = q1_pert.conjugate() * q2; + J1n.col(i) = log_q(qc.conjugate() * qc_pert) / dx; // Jac wrt q2 - q2_pert = q2*exp_q(pert); - qc_pert = q1.conjugate() * q2_pert; - J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx; + q2_pert = q2 * exp_q(pert); + qc_pert = q1.conjugate() * q2_pert; + J2n.col(i) = log_q(qc.conjugate() * qc_pert) / dx; } ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5); @@ -642,74 +672,79 @@ TEST(Jacobians, between) TEST(exp_q, small) { - Vector3d u; u.setRandom().normalize(); - Vector3d v; + Vector3d u; + u.setRandom().normalize(); + Vector3d v; Quaterniond q; - double scale = 1.0; - for (int i = 0; i<20; i++) + double scale = 1.0; + for (int i = 0; i < 20; i++) { - v = u*scale; - q = exp_q(v); + v = u * scale; + q = exp_q(v); WOLF_TRACE("scale = ", scale, "; ratio = ", (q.vec().array() / v.array()).transpose()); - scale /= 10; + scale /= 10; } - ASSERT_MATRIX_APPROX(q.vec()/(10*scale), u/2, 1e-12); + ASSERT_MATRIX_APPROX(q.vec() / (10 * scale), u / 2, 1e-12); } TEST(log_q, double_cover) { - Quaterniond qp; qp.coeffs().setRandom().normalize(); - Quaterniond qn; qn.coeffs() = - qp.coeffs(); + Quaterniond qp; + qp.coeffs().setRandom().normalize(); + Quaterniond qn; + qn.coeffs() = -qp.coeffs(); ASSERT_MATRIX_APPROX(log_q(qp), log_q(qn), 1e-16); } TEST(log_q, small) { - Vector3d u; u.setRandom().normalize(); + Vector3d u; + u.setRandom().normalize(); double scale = 1.0; - for (int i = 0; i<20; i++) + for (int i = 0; i < 20; i++) { - Vector3d v = u*scale; - Quaterniond q = exp_q(v); - Vector3d l = log_q(q); + Vector3d v = u * scale; + Quaterniond q = exp_q(v); + Vector3d l = log_q(q); ASSERT_MATRIX_APPROX(v, l, 1e-10); - scale /= 10; + scale /= 10; } } TEST(Conversions, q2R_R2q) { - Vector3d v; v.setRandom(); + Vector3d v; + v.setRandom(); Quaterniond q = v2q(v); - Matrix3d R = v2R(v); + Matrix3d R = v2R(v); Quaterniond q_R = R2q(R); Quaterniond qq_R(R); - ASSERT_NEAR(q.norm(), 1, wolf::Constants::EPS); - ASSERT_NEAR(q_R.norm(), 1, wolf::Constants::EPS); + ASSERT_NEAR(q.norm(), 1, wolf::Constants::EPS); + ASSERT_NEAR(q_R.norm(), 1, wolf::Constants::EPS); ASSERT_NEAR(qq_R.norm(), 1, wolf::Constants::EPS); ASSERT_MATRIX_APPROX(q.coeffs(), R2q(R).coeffs(), wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(q.coeffs(), qq_R.coeffs(), wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(R, q2R(q), wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(q.coeffs(), qq_R.coeffs(), wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(R, q2R(q), wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS); } TEST(Conversions, R2q_norm_of_q) { - Isometry3d T = Translation3d(1,2,3) * AngleAxisd(M_PI_4, Vector3d(1,2,3).normalized()); - Eigen::Vector4d cqt = R2q(T.linear()).coeffs(); - Eigen::Matrix3d R = T.linear(); + Isometry3d T = Translation3d(1, 2, 3) * AngleAxisd(M_PI_4, Vector3d(1, 2, 3).normalized()); + Eigen::Vector4d cqt = R2q(T.linear()).coeffs(); + Eigen::Matrix3d R = T.linear(); Eigen::Vector4d cqt2 = R2q(R).coeffs(); - std::cout << "cqt: " << cqt.transpose() << std::endl; - std::cout << "cqt2: " << cqt2.transpose() << std::endl; - std::cout << "cqt.norm(): " << cqt.norm() << std::endl; - std::cout << "cqt2.norm(): " << cqt2.norm() << std::endl; + std::cout << "cqt: " << cqt.transpose() << std::endl; + std::cout << "cqt2: " << cqt2.transpose() << std::endl; + std::cout << "cqt.norm(): " << cqt.norm() << std::endl; + std::cout << "cqt2.norm(): " << cqt2.norm() << std::endl; ASSERT_NEAR(cqt.norm(), 1, 1e-8); ASSERT_NEAR(cqt2.norm(), 1, 1e-8); @@ -726,7 +761,7 @@ TEST(Conversions, R2q_norm_of_q) TEST(Conversions, e2q_q2e) { - Vector3d e, eo; + Vector3d e, eo; Quaterniond q; e << 0.1, .2, .3; @@ -738,14 +773,13 @@ TEST(Conversions, e2q_q2e) eo = q2e(q.coeffs()); ASSERT_MATRIX_APPROX(eo, e, 1e-10); - } TEST(Conversions, e2q_q2R_R2e) { - Vector3d e, eo; + Vector3d e, eo; Quaterniond q; - Matrix3d R; + Matrix3d R; e << 0.1, .2, .3; q = e2q(e); @@ -770,9 +804,9 @@ TEST(Conversions, e2R_R2e) TEST(Conversions, e2R_R2q_q2e) { - Vector3d e, eo; + Vector3d e, eo; Quaterniond q; - Matrix3d R; + Matrix3d R; e << 0.1, 0.2, 0.3; R = e2R(e); @@ -788,8 +822,8 @@ int main(int argc, char **argv) using namespace wolf; /* - LIST OF FUNCTIONS : - - pi2pi + LIST OF FUNCTIONS : + - pi2pi - skew -> Skew_vee OK - vee -> Skew_vee OK - v2q v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS) diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp index 6b7c4bf1bbdc29b35196509f41c57c8e298a7775..9ad36f3918ffa7163abd8ef890aa9769495010eb 100644 --- a/test/gtest_sensor_diff_drive.cpp +++ b/test/gtest_sensor_diff_drive.cpp @@ -50,7 +50,7 @@ TEST(SensorDiffDrive, create) param["states"]["I"]["mode"] = "fix"; param["states"]["I"]["dynamic"] = false; - param["name"] = "just a sensor"; + param["name"] = "just a sensor"; param["ticks_per_wheel_revolution"] = 4; param["ticks_std_factor"] = 2; diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp index 5dec8d6b59a1d77200d2cd556c87d07785059c5a..94aec75efaf2f8a8231d1894b107e6b181c50bd9 100644 --- a/test/gtest_sensor_odom.cpp +++ b/test/gtest_sensor_odom.cpp @@ -28,20 +28,20 @@ using namespace yaml_schema_cpp; std::string wolf_dir = _WOLF_CODE_DIR; -ProblemPtr problem_2D = Problem::create("PO", 2); -ProblemPtr problem_3D = Problem::create("PO", 3); -VectorXd vector0 = VectorXd(0); -VectorXd p_state_2D = (VectorXd(2) << 1, 2).finished(); -VectorXd p_state_3D = (VectorXd(3) << 1, 2, 3).finished(); -VectorXd o_state_2D = (VectorXd(1) << 3).finished(); -VectorXd o_state_3D = (VectorXd(4) << 1, 0, 0, 0).finished(); -double k_disp_to_disp = 0.1; -double k_disp_to_rot = 0.2; -double k_rot_to_rot = 0.3; -double min_disp_var = 0.01; -double min_rot_var = 0.02; -double noise_p_std = 0.1; -double noise_o_std = 0.01; +ProblemPtr problem_2D = Problem::create("PO", 2); +ProblemPtr problem_3D = Problem::create("PO", 3); +VectorXd vector0 = VectorXd(0); +VectorXd p_state_2D = (VectorXd(2) << 1, 2).finished(); +VectorXd p_state_3D = (VectorXd(3) << 1, 2, 3).finished(); +VectorXd o_state_2D = (VectorXd(1) << 3).finished(); +VectorXd o_state_3D = (VectorXd(4) << 1, 0, 0, 0).finished(); +double k_disp_to_disp = 0.1; +double k_disp_to_rot = 0.2; +double k_rot_to_rot = 0.3; +double min_disp_var = 0.01; +double min_rot_var = 0.02; +double noise_p_std = 0.1; +double noise_o_std = 0.01; void checkSensor(SensorBasePtr S, char _key, diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp index 6ad469b78ada22280553f939a4a17b565fd43a5a..7a642d31fbb5194774a61c1fcd5c5a37e977e264 100644 --- a/test/gtest_sensor_pose.cpp +++ b/test/gtest_sensor_pose.cpp @@ -109,8 +109,9 @@ TEST(SensorPose, creators_and_factories) // checks checkSensor(S, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0); checkSensor(S, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0); - ASSERT_MATRIX_APPROX( - S->computeNoiseCov(Vector1d::Zero()), MatrixXd((dim == 2 ? std_noise_2D : std_noise_3D).cwiseAbs2().asDiagonal()), 1e-9); + ASSERT_MATRIX_APPROX(S->computeNoiseCov(Vector1d::Zero()), + MatrixXd((dim == 2 ? std_noise_2D : std_noise_3D).cwiseAbs2().asDiagonal()), + 1e-9); // emplace ------------------------------------------------------------------------ auto S2 = dim == 2 ? std::static_pointer_cast<SensorBase>( @@ -141,7 +142,6 @@ TEST(SensorPose, creators_and_factories) } } - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp index 82dda2203ca4a0c288a28162c2ebcd22976548a7..6859e3625787787b4841dd729941e2c516fc47dd 100644 --- a/test/gtest_shared_from_this.cpp +++ b/test/gtest_shared_from_this.cpp @@ -25,52 +25,51 @@ class CChildBase; class CParentBase : public wolf::NodeBase { - public: + public: + std::list<std::shared_ptr<CChildBase> > child_list_; - std::list<std::shared_ptr<CChildBase> > child_list_; + CParentBase() : NodeBase(""){}; - CParentBase() : - NodeBase("") - {}; + ~CParentBase() override{}; - ~CParentBase() override{}; + virtual void addChild(std::shared_ptr<CChildBase> _child_ptr) final + { + child_list_.push_back(_child_ptr); + } - virtual void addChild(std::shared_ptr<CChildBase> _child_ptr) final - { - child_list_.push_back(_child_ptr); - } - - bool hasChildren() const override {return false;}; + bool hasChildren() const override + { + return false; + }; }; class CParentDerived : public CParentBase { - public: - - CParentDerived(){}; + public: + CParentDerived(){}; }; class CChildBase : public wolf::NodeBase, public std::enable_shared_from_this<CChildBase> { - public: - std::shared_ptr<CParentBase> parent_ptr_; - - CChildBase(std::shared_ptr<CParentBase> _parent_ptr) : - NodeBase(""), - parent_ptr_(_parent_ptr) - { - auto wptr = std::shared_ptr<CChildBase>( this, [](CChildBase*){} ); - - parent_ptr_->addChild(shared_from_this()); - }; - bool hasChildren() const override {return false;}; + public: + std::shared_ptr<CParentBase> parent_ptr_; + + CChildBase(std::shared_ptr<CParentBase> _parent_ptr) : NodeBase(""), parent_ptr_(_parent_ptr) + { + auto wptr = std::shared_ptr<CChildBase>(this, [](CChildBase *) {}); + + parent_ptr_->addChild(shared_from_this()); + }; + bool hasChildren() const override + { + return false; + }; }; class CChildDerived : public CChildBase { - public: - - CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){}; + public: + CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){}; }; TEST(TestTest, SingleTest) @@ -92,16 +91,17 @@ TEST(TestTest, SingleTest) ASSERT_EQ(child_derived_ptr->getName(), parent_derived_ptr->child_list_.front()->getName()); ASSERT_EQ(parent_derived_ptr->getName(), child_derived_ptr->parent_ptr_->getName()); - //std::cout << "parent_derived_ptr->getName() " << parent_derived_ptr->getName() << std::endl; - //std::cout << "child_derived_ptr->getName() " << child_derived_ptr->getName() << std::endl; - //std::cout << "child_derived_ptr->parent_ptr_->getName() " << child_derived_ptr->parent_ptr_->getName() << std::endl; - //std::cout << "parent_derived_ptr->child_list_.front()->getName() " << parent_derived_ptr->child_list_.front()->getName() << std::endl; + // std::cout << "parent_derived_ptr->getName() " << parent_derived_ptr->getName() << std::endl; + // std::cout << "child_derived_ptr->getName() " << child_derived_ptr->getName() << std::endl; + // std::cout << "child_derived_ptr->parent_ptr_->getName() " << child_derived_ptr->parent_ptr_->getName() << + // std::endl; std::cout << "parent_derived_ptr->child_list_.front()->getName() " << + // parent_derived_ptr->child_list_.front()->getName() << std::endl; -// PRINTF("All good at TestTest::DummyTestExample !\n"); + // PRINTF("All good at TestTest::DummyTestExample !\n"); } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp index a01a0f0eeac8e6fc34ca5120d88b80d59ce40333..ac48b773da7371e7d14396fa866835b0603b8d04 100644 --- a/test/gtest_solver_ceres.cpp +++ b/test/gtest_solver_ceres.cpp @@ -67,8 +67,8 @@ FactorBasePtr createFactor(NodeStateBlocksPtr node_ptr) } TEST(SolverCeres, Create) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // check pointers EXPECT_EQ(P, solver_ptr->getProblem()); @@ -81,14 +81,15 @@ TEST(SolverCeres, Create) // FLOATING STATE BLOCKS TEST(SolverCeres, FloatingStateBlock_Add) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -111,14 +112,15 @@ TEST(SolverCeres, FloatingStateBlock_Add) TEST(SolverCeres, FloatingStateBlock_DoubleAdd) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -134,7 +136,7 @@ TEST(SolverCeres, FloatingStateBlock_DoubleAdd) EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // notify stateblock again - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); @@ -156,14 +158,15 @@ TEST(SolverCeres, FloatingStateBlock_DoubleAdd) TEST(SolverCeres, FloatingStateBlock_AddFix) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -225,14 +228,15 @@ TEST(SolverCeres, FloatingStateBlock_AddFix) TEST(SolverCeres, FloatingStateBlock_AddFixed) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -274,14 +278,15 @@ TEST(SolverCeres, FloatingStateBlock_AddFixed) TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -326,7 +331,7 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam) // check stateblock fixed EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); - EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr)); EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); EXPECT_TRUE(solver_ptr->check()); @@ -334,11 +339,12 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam) TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Local param LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>(); @@ -350,7 +356,7 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam) EXPECT_TRUE(sb_ptr->localParamUpdated()); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -366,7 +372,7 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam) EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check stateblock localparam - EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr)); // check flags EXPECT_FALSE(sb_ptr->stateUpdated()); @@ -397,14 +403,15 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam) TEST(SolverCeres, FloatingStateBlock_Remove) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -420,7 +427,7 @@ TEST(SolverCeres, FloatingStateBlock_Remove) EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr, REMOVE); // check notifications EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); @@ -441,14 +448,15 @@ TEST(SolverCeres, FloatingStateBlock_Remove) TEST(SolverCeres, FloatingStateBlock_AddRemove) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -457,7 +465,7 @@ TEST(SolverCeres, FloatingStateBlock_AddRemove) EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD + P->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD // check notifications EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); @@ -477,14 +485,15 @@ TEST(SolverCeres, FloatingStateBlock_AddRemove) TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -493,14 +502,14 @@ TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd) EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD + P->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD // check notifications EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list + P->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list // check notifications EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); @@ -521,26 +530,27 @@ TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd) TEST(SolverCeres, FloatingStateBlock_DoubleRemove) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // update solver solver_ptr->update(); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr, REMOVE); // update solver solver_ptr->update(); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr, REMOVE); // check notifications Notification notif; @@ -561,17 +571,18 @@ TEST(SolverCeres, FloatingStateBlock_DoubleRemove) TEST(SolverCeres, FloatingStateBlock_AddUpdated) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Fix sb_ptr->fix(); // Set State - sb_ptr->setState(2*sb_ptr->getState()); + sb_ptr->setState(2 * sb_ptr->getState()); // Check flags have been set true EXPECT_TRUE(sb_ptr->fixUpdated()); @@ -580,30 +591,30 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated) // == When an ADD is notified: a ADD notification should be stored == // notify state block - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications // check notifications Notification notif; - EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); EXPECT_EQ(notif, ADD); // == Insert OTHER notifications == // Set State --> FLAG - sb_ptr->setState(2*sb_ptr->getState()); + sb_ptr->setState(2 * sb_ptr->getState()); // Fix --> FLAG sb_ptr->unfix(); // Check flag has been set true EXPECT_TRUE(sb_ptr->fixUpdated()); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb) + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); // No new notifications (fix and set state are flags in sb) // == consume empties the notification map == - solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); // Check flags have been reset EXPECT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated()); @@ -611,49 +622,51 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated) // == When an REMOVE is notified: a REMOVE notification should be stored == // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr, REMOVE); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); EXPECT_EQ(notif, REMOVE); // == REMOVE + ADD: notification map should be empty == - P->notifyStateBlock(sb_ptr,ADD); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + P->notifyStateBlock(sb_ptr, ADD); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); // == UPDATES should clear the list of notifications == // notify state block - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // update solver solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty + EXPECT_EQ(P->getStateBlockNotificationMapSize(), + 0); // After solver_manager->update, notifications should be empty EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // == ADD + REMOVE: notification map should be empty == - P->notifyStateBlock(sb_ptr,ADD); - P->notifyStateBlock(sb_ptr,REMOVE); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + P->notifyStateBlock(sb_ptr, ADD); + P->notifyStateBlock(sb_ptr, REMOVE); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); } //////////////////////////////////////////////////////// // STATE BLOCKS AND FACTOR TEST(SolverCeres, StateBlockAndFactor_Add) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock (floating for the moment) - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // check notifications Notification notif; @@ -669,7 +682,7 @@ TEST(SolverCeres, StateBlockAndFactor_Add) EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr)); // notify factor (state block now not floating) - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); @@ -680,7 +693,7 @@ TEST(SolverCeres, StateBlockAndFactor_Add) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // checks @@ -692,20 +705,21 @@ TEST(SolverCeres, StateBlockAndFactor_Add) TEST(SolverCeres, StateBlockAndFactor_DoubleAdd) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -719,14 +733,14 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // notify stateblock again - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); @@ -738,7 +752,7 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check stateblock @@ -750,20 +764,21 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd) TEST(SolverCeres, StateBlockAndFactor_Fix) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -781,7 +796,7 @@ TEST(SolverCeres, StateBlockAndFactor_Fix) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check flags @@ -802,14 +817,14 @@ TEST(SolverCeres, StateBlockAndFactor_Fix) EXPECT_FALSE(sb_ptr->localParamUpdated()); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // update solver manager solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check flags @@ -824,20 +839,21 @@ TEST(SolverCeres, StateBlockAndFactor_Fix) TEST(SolverCeres, StateBlockAndFactor_AddFixed) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -863,7 +879,7 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check flags @@ -879,20 +895,21 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed) TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -922,7 +939,7 @@ TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check flags @@ -933,17 +950,18 @@ TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam) // check stateblock fixed EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr)); - EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr)); EXPECT_TRUE(solver_ptr->check()); } TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); @@ -958,10 +976,10 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam) EXPECT_TRUE(sb_ptr->localParamUpdated()); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -974,13 +992,13 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // check solver EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr)); - EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); + EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr)); EXPECT_TRUE(solver_ptr->check()); // check flags @@ -1007,20 +1025,21 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam) TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -1033,11 +1052,11 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr, REMOVE); // check notifications EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); @@ -1045,7 +1064,7 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock) EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // update solver - solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list) + solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list) // check notifications EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); @@ -1060,20 +1079,21 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock) TEST(SolverCeres, StateBlockAndFactor_RemoveFactor) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -1086,11 +1106,11 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // remove state_block - P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating + P->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating // check notifications EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); @@ -1101,7 +1121,7 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor) solver_ptr->update(); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // checks @@ -1113,20 +1133,21 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor) TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -1136,18 +1157,19 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock) EXPECT_EQ(ADD, notif); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification + P->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); // update solver - solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added) + solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD + // notification added) // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); @@ -1159,20 +1181,21 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock) TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -1182,18 +1205,18 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor) EXPECT_EQ(ADD, notif); // remove state_block - P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification + P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification // check notifications - EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); EXPECT_EQ(ADD, notif); // update solver - solver_ptr->update(); // state block should be automatically stored as floating + solver_ptr->update(); // state block should be automatically stored as floating // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // checks @@ -1205,20 +1228,21 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor) TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -1228,23 +1252,23 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock) EXPECT_EQ(ADD, notif); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD + P->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); // update solver - solver_ptr->update(); // factor ADD is posponed + solver_ptr->update(); // factor ADD is posponed // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr, REMOVE); // check notifications EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); @@ -1253,10 +1277,10 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock) EXPECT_EQ(ADD, notif); // update solver manager - solver_ptr->update(); // repeated REMOVE should be ignored + solver_ptr->update(); // repeated REMOVE should be ignored // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); @@ -1268,20 +1292,21 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock) TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify stateblock - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -1291,30 +1316,30 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor) EXPECT_EQ(ADD, notif); // remove state_block - P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD + P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD // check notifications - EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); EXPECT_EQ(ADD, notif); // update solver - solver_ptr->update(); // state block should be automatically stored as floating + solver_ptr->update(); // state block should be automatically stored as floating // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); - EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // remove state_block - P->notifyFactor(fac_ptr,REMOVE); + P->notifyFactor(fac_ptr, REMOVE); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(REMOVE, notif); // update solver - solver_ptr->update(); // repeated REMOVE should be ignored + solver_ptr->update(); // repeated REMOVE should be ignored // checks EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr)); @@ -1325,11 +1350,12 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor) TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); @@ -1338,7 +1364,7 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock) sb_ptr->fix(); // Change State - sb_ptr->setState(2*sb_ptr->getState()); + sb_ptr->setState(2 * sb_ptr->getState()); // Check flags have been set true EXPECT_TRUE(sb_ptr->fixUpdated()); @@ -1347,10 +1373,10 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock) // == When an ADD is notified: a ADD notification should be stored == // notify state block - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // notify factor - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; @@ -1362,18 +1388,18 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock) // == Insert OTHER notifications == // Set State --> FLAG - sb_ptr->setState(2*sb_ptr->getState()); + sb_ptr->setState(2 * sb_ptr->getState()); // Fix --> FLAG sb_ptr->unfix(); // Check flag has been set true EXPECT_TRUE(sb_ptr->fixUpdated()); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb) + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); // No new notifications (fix and set state are flags in sb) // == consume empties the notification map == - solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); // Check flags have been reset EXPECT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated()); @@ -1381,58 +1407,60 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock) // == When an REMOVE is notified: a REMOVE notification should be stored == // remove state_block - P->notifyStateBlock(sb_ptr,REMOVE); + P->notifyStateBlock(sb_ptr, REMOVE); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); EXPECT_EQ(notif, REMOVE); // == REMOVE + ADD: notification map should be empty == - P->notifyStateBlock(sb_ptr,ADD); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + P->notifyStateBlock(sb_ptr, ADD); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); // == UPDATES should clear the list of notifications == // notify state block - P->notifyStateBlock(sb_ptr,ADD); + P->notifyStateBlock(sb_ptr, ADD); // update solver solver_ptr->update(); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty + EXPECT_EQ(P->getStateBlockNotificationMapSize(), + 0); // After solver_manager->update, notifications should be empty // == ADD + REMOVE: notification map should be empty == - P->notifyStateBlock(sb_ptr,ADD); - P->notifyStateBlock(sb_ptr,REMOVE); - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + P->notifyStateBlock(sb_ptr, ADD); + P->notifyStateBlock(sb_ptr, REMOVE); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); } //////////////////////////////////////////////////////// // ONLY FACTOR TEST(SolverCeres, OnlyFactor_Add) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify factor (state block has not been notified) - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); // update solver - solver_ptr->update(); // factor ADD should be posponed (in the notification list again) + solver_ptr->update(); // factor ADD should be posponed (in the notification list again) // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); @@ -1444,45 +1472,46 @@ TEST(SolverCeres, OnlyFactor_Add) TEST(SolverCeres, OnlyFactor_Remove) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify factor (state block has not been notified) - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); // update solver - solver_ptr->update(); // ADD factor should be posponed (in the notification list again) + solver_ptr->update(); // ADD factor should be posponed (in the notification list again) // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); // notify factor - P->notifyFactor(fac_ptr,REMOVE); + P->notifyFactor(fac_ptr, REMOVE); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); - EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // update solver - solver_ptr->update(); // nothing to update + solver_ptr->update(); // nothing to update // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); - EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // checks EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); @@ -1492,37 +1521,38 @@ TEST(SolverCeres, OnlyFactor_Remove) TEST(SolverCeres, OnlyFactor_AddRemove) { - ProblemPtr P = Problem::create("PO", 3); - auto solver_ptr = std::make_shared<SolverCeres>(P); + ProblemPtr P = Problem::create("PO", 3); + auto solver_ptr = std::make_shared<SolverCeres>(P); // Create State block - auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I'); + auto node_ptr = createStateBlock(); + auto sb_ptr = node_ptr->getStateBlock('I'); // Create Factor auto fac_ptr = createFactor(node_ptr); // notify factor (state block has not been notified) - P->notifyFactor(fac_ptr,ADD); + P->notifyFactor(fac_ptr, ADD); // check notifications Notification notif; - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif)); EXPECT_EQ(ADD, notif); // notify factor - P->notifyFactor(fac_ptr,REMOVE); + P->notifyFactor(fac_ptr, REMOVE); // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); - EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // update solver - solver_ptr->update(); // nothing to update + solver_ptr->update(); // nothing to update // check notifications - EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); - EXPECT_EQ(P->getFactorNotificationMapSize(),0); + EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0); + EXPECT_EQ(P->getFactorNotificationMapSize(), 0); // checks EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr)); @@ -1532,7 +1562,6 @@ TEST(SolverCeres, OnlyFactor_AddRemove) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp index 0f3933f66e4b187180165f2716911392bfcedd55..aa8a2380de60aa8f68dfa6b6266a05fd5c6ff7bd 100644 --- a/test/gtest_solver_manager_multithread.cpp +++ b/test/gtest_solver_manager_multithread.cpp @@ -20,7 +20,6 @@ #include "core/utils/utils_gtest.h" - #include "core/problem/problem.h" #include "core/sensor/sensor_base.h" #include "core/state_block/state_block.h" @@ -43,38 +42,43 @@ using namespace Eigen; TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications) { - double Dt = 5.0; - ProblemPtr P = Problem::create("PO", 2); - auto solver_ptr = std::make_shared<SolverManagerDummy>(P); + double Dt = 5.0; + ProblemPtr P = Problem::create("PO", 2); + auto solver_ptr = std::make_shared<SolverManagerDummy>(P); // loop updating (without sleep) - std::thread t([&](){ + std::thread t([&]() { auto start_t = std::chrono::high_resolution_clock::now(); while (true) { solver_ptr->update(); ASSERT_TRUE(solver_ptr->check()); - if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt) + if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - + start_t) + .count() > Dt) break; - }}); + } + }); // loop emplacing and removing frames (window of 10 KF) - auto start = std::chrono::high_resolution_clock::now(); + auto start = std::chrono::high_resolution_clock::now(); TimeStamp ts(0); while (true) { // Emplace Frame, Capture, feature and factor pose 2d - FrameBasePtr F = P->emplaceFrame(ts, P->stateZero()); - auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr); - auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity()); - auto c = FactorBase::emplace<FactorPose2d>(f, f->getMeasurement(), f->getMeasurementSquareRootInformationUpper(), F, nullptr, false); + FrameBasePtr F = P->emplaceFrame(ts, P->stateZero()); + auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr); + auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity()); + auto c = FactorBase::emplace<FactorPose2d>( + f, f->getMeasurement(), f->getMeasurementSquareRootInformationUpper(), F, nullptr, false); ts += 1.0; - if (P->getTrajectory()->size() > 10) - P->getTrajectory()->getFirstFrame()->remove(); + if (P->getTrajectory()->size() > 10) P->getTrajectory()->getFirstFrame()->remove(); - if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt) + if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - + start) + .count() > Dt) break; } @@ -83,6 +87,6 @@ TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp index b19c1ac201024279ac5d3f6bb5ed65eec1a16b72..ac8e2f852bdcdcce78227d73a2f4d0fef6c6a395 100644 --- a/test/gtest_time_stamp.cpp +++ b/test/gtest_time_stamp.cpp @@ -46,189 +46,189 @@ TEST(WolfTestTimeStamp, TimeStampInvalid) TEST(WolfTestTimeStamp, TimeStampInitNow) { - wolf::TimeStamp start = wolf::TimeStamp::Now(); + wolf::TimeStamp start = wolf::TimeStamp::Now(); - // If we don't sleep, start == time_stamp sometimes. - // And sometimes start <= time_stamp ... - std::this_thread::sleep_for(std::chrono::microseconds(1)); + // If we don't sleep, start == time_stamp sometimes. + // And sometimes start <= time_stamp ... + std::this_thread::sleep_for(std::chrono::microseconds(1)); - ASSERT_NE(start.get(), 0); + ASSERT_NE(start.get(), 0); - wolf::TimeStamp time_stamp = wolf::TimeStamp::Now(); + wolf::TimeStamp time_stamp = wolf::TimeStamp::Now(); -// std::cout << std::fixed; -// std::cout << std::setprecision(15); -// std::cout << start.get() << " | " << time_stamp.get() << std::endl; + // std::cout << std::fixed; + // std::cout << std::setprecision(15); + // std::cout << start.get() << " | " << time_stamp.get() << std::endl; - ASSERT_NE(time_stamp.get(), start.get()); + ASSERT_NE(time_stamp.get(), start.get()); - ASSERT_LT(start.get(), time_stamp.get()); + ASSERT_LT(start.get(), time_stamp.get()); -// PRINTF("All good at WolfTestTimeStamp::TimeStampInitNow !\n"); + // PRINTF("All good at WolfTestTimeStamp::TimeStampInitNow !\n"); } TEST(WolfTestTimeStamp, TimeStampInitScalar) { - double val(101010); + double val(101010); - wolf::TimeStamp start(val); + wolf::TimeStamp start(val); - ASSERT_EQ(start.get(), val); - ASSERT_EQ(start.getSeconds(), val); - ASSERT_EQ(start.getNanoSeconds(), (unsigned int) 0); + ASSERT_EQ(start.get(), val); + ASSERT_EQ(start.getSeconds(), val); + ASSERT_EQ(start.getNanoSeconds(), (unsigned int)0); - std::stringstream ss; - start.print(ss); + std::stringstream ss; + start.print(ss); - ASSERT_STREQ("101010.000000000", ss.str().c_str()); + ASSERT_STREQ("101010.000000000", ss.str().c_str()); -// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalar !\n"); + // PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalar !\n"); } TEST(WolfTestTimeStamp, TimeStampInitScalarSecNano) { - double sec(101010); - double nano(202020); - double val(101010.000202020); + double sec(101010); + double nano(202020); + double val(101010.000202020); - wolf::TimeStamp start(sec, nano); + wolf::TimeStamp start(sec, nano); - // start.get -> 101010.000202020004508 + // start.get -> 101010.000202020004508 - ASSERT_EQ(start.get(), val); - ASSERT_EQ(start.getSeconds(), sec); - ASSERT_EQ(start.getNanoSeconds(), nano); + ASSERT_EQ(start.get(), val); + ASSERT_EQ(start.getSeconds(), sec); + ASSERT_EQ(start.getNanoSeconds(), nano); - std::stringstream ss; - start.print(ss); + std::stringstream ss; + start.print(ss); - ASSERT_STREQ("101010.000202020", ss.str().c_str()); + ASSERT_STREQ("101010.000202020", ss.str().c_str()); -// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalarSecNano !\n"); + // PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalarSecNano !\n"); } TEST(WolfTestTimeStamp, TimeStampSetNow) { - wolf::TimeStamp t1; - wolf::TimeStamp t2(t1); + wolf::TimeStamp t1; + wolf::TimeStamp t2(t1); - ASSERT_EQ(t1,t2); + ASSERT_EQ(t1, t2); - // If we don't sleep, start == time_stamp sometimes. - // And sometimes start <= time_stamp ... - std::this_thread::sleep_for(std::chrono::microseconds(1)); + // If we don't sleep, start == time_stamp sometimes. + // And sometimes start <= time_stamp ... + std::this_thread::sleep_for(std::chrono::microseconds(1)); - t2.setToNow(); + t2.setToNow(); - ASSERT_LT(t1,t2); + ASSERT_LT(t1, t2); } TEST(WolfTestTimeStamp, TimeStampSetScalar) { - double val(101010.000202020); + double val(101010.000202020); - wolf::TimeStamp start; - start.set(val); + wolf::TimeStamp start; + start.set(val); - ASSERT_EQ(start.get(), val); - ASSERT_EQ(start.getSeconds(), 101010); - ASSERT_EQ(start.getNanoSeconds(), 202020); + ASSERT_EQ(start.get(), val); + ASSERT_EQ(start.getSeconds(), 101010); + ASSERT_EQ(start.getNanoSeconds(), 202020); - std::stringstream ss; - start.print(ss); + std::stringstream ss; + start.print(ss); - ASSERT_STREQ("101010.000202020", ss.str().c_str()); + ASSERT_STREQ("101010.000202020", ss.str().c_str()); } TEST(WolfTestTimeStamp, TimeStampSetSecNano) { - unsigned long int sec(101010); - unsigned long int nano(202020); + unsigned long int sec(101010); + unsigned long int nano(202020); - wolf::TimeStamp start; - start.set(sec,nano); + wolf::TimeStamp start; + start.set(sec, nano); - // start.get -> 101010.000202020004508 + // start.get -> 101010.000202020004508 - ASSERT_EQ(start.getSeconds(), sec); - ASSERT_EQ(start.getNanoSeconds(), nano); + ASSERT_EQ(start.getSeconds(), sec); + ASSERT_EQ(start.getNanoSeconds(), nano); } TEST(WolfTestTimeStamp, TimeStampEquality) { - wolf::TimeStamp start; + wolf::TimeStamp start; - wolf::TimeStamp time_stamp(start); + wolf::TimeStamp time_stamp(start); - ASSERT_EQ(time_stamp, start); + ASSERT_EQ(time_stamp, start); - ASSERT_EQ(time_stamp.get(), start.get()); + ASSERT_EQ(time_stamp.get(), start.get()); - std::this_thread::sleep_for(std::chrono::microseconds(1)); - time_stamp.setToNow(); + std::this_thread::sleep_for(std::chrono::microseconds(1)); + time_stamp.setToNow(); - ASSERT_NE(time_stamp.get(), start.get()); + ASSERT_NE(time_stamp.get(), start.get()); - time_stamp = start; + time_stamp = start; - ASSERT_EQ(time_stamp.get(), start.get()); + ASSERT_EQ(time_stamp.get(), start.get()); -// PRINTF("All good at WolfTestTimeStamp::TimeStampEquality !\n"); + // PRINTF("All good at WolfTestTimeStamp::TimeStampEquality !\n"); } TEST(WolfTestTimeStamp, TimeStampInequality) { - wolf::TimeStamp start = wolf::TimeStamp::Now(); + wolf::TimeStamp start = wolf::TimeStamp::Now(); - std::this_thread::sleep_for(std::chrono::microseconds(1)); + std::this_thread::sleep_for(std::chrono::microseconds(1)); - wolf::TimeStamp time_stamp = wolf::TimeStamp::Now(); + wolf::TimeStamp time_stamp = wolf::TimeStamp::Now(); - // error: no match for ‘operator!=’ - //ASSERT_NE(time_stamp, start); + // error: no match for ‘operator!=’ + // ASSERT_NE(time_stamp, start); - ASSERT_LT(start, time_stamp); - ASSERT_LE(start, time_stamp); - ASSERT_LE(start, start); + ASSERT_LT(start, time_stamp); + ASSERT_LE(start, time_stamp); + ASSERT_LE(start, start); - // error: no match for ‘operator>’ - ASSERT_GT(time_stamp, start); - ASSERT_GE(time_stamp, start); - ASSERT_GE(start, start); + // error: no match for ‘operator>’ + ASSERT_GT(time_stamp, start); + ASSERT_GE(time_stamp, start); + ASSERT_GE(start, start); -// PRINTF("All good at WolfTestTimeStamp::TimeStampInequality !\n"); + // PRINTF("All good at WolfTestTimeStamp::TimeStampInequality !\n"); } TEST(WolfTestTimeStamp, TimeStampSubstraction) { - wolf::TimeStamp t1; - wolf::TimeStamp t2(t1); - double dt(1e-5); + wolf::TimeStamp t1; + wolf::TimeStamp t2(t1); + double dt(1e-5); - t2+=dt; + t2 += dt; - ASSERT_LT(t1, t2); - ASSERT_EQ(t2-t1, dt); - ASSERT_EQ(t1-t2, -dt); + ASSERT_LT(t1, t2); + ASSERT_EQ(t2 - t1, dt); + ASSERT_EQ(t1 - t2, -dt); } TEST(WolfTestTimeStamp, TimeStampAdding) { - wolf::TimeStamp t1,t3; - wolf::TimeStamp t2(t1); - double dt(1e-5); + wolf::TimeStamp t1, t3; + wolf::TimeStamp t2(t1); + double dt(1e-5); - t2 +=dt; - t3 = t1+dt; + t2 += dt; + t3 = t1 + dt; - ASSERT_EQ(t2, t3); + ASSERT_EQ(t2, t3); } TEST(WolfTestTimeStamp, TimeStampOperatorOstream) { wolf::TimeStamp t(5); - double dt = 1e-4; - t+=dt; + double dt = 1e-4; + t += dt; std::ostringstream ss1, ss2; t.print(ss1); @@ -237,24 +237,24 @@ TEST(WolfTestTimeStamp, TimeStampOperatorOstream) ASSERT_EQ(ss1.str(), ss2.str()); -// PRINTF("All good at WolfTestTimeStamp::TimeStampOperatorOstream !\n"); + // PRINTF("All good at WolfTestTimeStamp::TimeStampOperatorOstream !\n"); } TEST(WolfTestTimeStamp, TimeStampSecNanoSec) { - unsigned long int sec = 5; + unsigned long int sec = 5; unsigned long int nano = 1e5; - wolf::TimeStamp t1(double(sec)+double(nano)*1e-9); - wolf::TimeStamp t2(sec,nano); + wolf::TimeStamp t1(double(sec) + double(nano) * 1e-9); + wolf::TimeStamp t2(sec, nano); - ASSERT_EQ(t1.getSeconds(),sec); - ASSERT_EQ(t2.getSeconds(),sec); - ASSERT_EQ(t1.getNanoSeconds(),nano); - ASSERT_EQ(t2.getNanoSeconds(),nano); + ASSERT_EQ(t1.getSeconds(), sec); + ASSERT_EQ(t2.getSeconds(), sec); + ASSERT_EQ(t1.getNanoSeconds(), nano); + ASSERT_EQ(t2.getNanoSeconds(), nano); } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp index e3b10aee3cd3ee9c2fdf9d414c1fd3c3db12fb54..df47a4cf9eaad7acee4f39b5a606e25fe0c7f1cd 100644 --- a/test/gtest_track_matrix.cpp +++ b/test/gtest_track_matrix.cpp @@ -26,56 +26,56 @@ using namespace wolf; class TrackMatrixTest : public testing::Test { - public: - TrackMatrix track_matrix; - - Eigen::Vector2d m; - Eigen::Matrix2d m_cov = Eigen::Matrix2d::Identity()*0.01; - - FrameBasePtr F0, F1, F2, F3, F4; - CaptureBasePtr C0, C1, C2, C3, C4; - FeatureBasePtr f0, f1, f2, f3, f4, f5; - ProblemPtr problem; - - void SetUp() override - { - problem = Problem::create("PO", 2); - // unlinked captures - // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer - C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0); - C1 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 1.0); - C2 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 2.0); - C3 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 3.0); - C4 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 4.0); - - // unlinked frames - // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer - F0 = std::make_shared<FrameBase>(0.0, nullptr); - F1 = std::make_shared<FrameBase>(1.0, nullptr); - F2 = std::make_shared<FrameBase>(2.0, nullptr); - F3 = std::make_shared<FrameBase>(3.0, nullptr); - F4 = std::make_shared<FrameBase>(4.0, nullptr); - - // unlinked features - // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer - f0 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); - f1 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); - f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); - f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); - f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); - f5 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); - - // F0 and F4 are keyframes - F0->link(problem); - F4->link(problem); - - // link captures - C0->link(F0); - C1->link(F1); - C2->link(F2); - C3->link(F3); - C4->link(F4); - } + public: + TrackMatrix track_matrix; + + Eigen::Vector2d m; + Eigen::Matrix2d m_cov = Eigen::Matrix2d::Identity() * 0.01; + + FrameBasePtr F0, F1, F2, F3, F4; + CaptureBasePtr C0, C1, C2, C3, C4; + FeatureBasePtr f0, f1, f2, f3, f4, f5; + ProblemPtr problem; + + void SetUp() override + { + problem = Problem::create("PO", 2); + // unlinked captures + // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer + C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0); + C1 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 1.0); + C2 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 2.0); + C3 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 3.0); + C4 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 4.0); + + // unlinked frames + // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer + F0 = std::make_shared<FrameBase>(0.0, nullptr); + F1 = std::make_shared<FrameBase>(1.0, nullptr); + F2 = std::make_shared<FrameBase>(2.0, nullptr); + F3 = std::make_shared<FrameBase>(3.0, nullptr); + F4 = std::make_shared<FrameBase>(4.0, nullptr); + + // unlinked features + // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer + f0 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); + f1 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); + f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); + f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); + f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); + f5 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov); + + // F0 and F4 are keyframes + F0->link(problem); + F4->link(problem); + + // link captures + C0->link(F0); + C1->link(F1); + C2->link(F2); + C3->link(F3); + C4->link(F4); + } }; TEST_F(TrackMatrixTest, newTrack) @@ -86,16 +86,16 @@ TEST_F(TrackMatrixTest, newTrack) f3->link(C1); track_matrix.newTrack(f0); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); track_matrix.newTrack(f1); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2); track_matrix.newTrack(f2); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 3); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)3); track_matrix.newTrack(f3); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 4); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)4); } TEST_F(TrackMatrixTest, add) @@ -105,23 +105,23 @@ TEST_F(TrackMatrixTest, add) f2->link(C2); track_matrix.newTrack(f0); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); track_matrix.add(f0->trackId(), f1); /* KC0 C1 snapshots * * f0---f1 trk 0 */ - ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2); + ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int)2); ASSERT_EQ(f1->trackId(), f0->trackId()); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); track_matrix.add(f0->trackId(), f2); /* KC0 C1 C2 snapshots * * f0---f1---f2 trk 0 */ - ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3); + ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int)3); ASSERT_EQ(f2->trackId(), f0->trackId()); } @@ -139,7 +139,7 @@ TEST_F(TrackMatrixTest, add2) * * f0---f1 trk 0 */ - ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2); + ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int)2); ASSERT_EQ(f1->trackId(), f0->trackId()); track_matrix.add(f1, f2); @@ -147,9 +147,9 @@ TEST_F(TrackMatrixTest, add2) * * f0---f1---f2 trk 0 */ - ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3); + ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int)3); ASSERT_EQ(f2->trackId(), f0->trackId()); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); track_matrix.newTrack(f3); /* KC0 C1 C2 snapshots @@ -157,9 +157,9 @@ TEST_F(TrackMatrixTest, add2) * f0---f1---f2 trk 0 * f3 trk 1 */ - ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int) 1); + ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int)1); ASSERT_NE(f3->trackId(), f0->trackId()); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2); } TEST_F(TrackMatrixTest, numTracks) @@ -176,11 +176,11 @@ TEST_F(TrackMatrixTest, numTracks) * f0---f1 trk 0 */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); track_matrix.add(f0->trackId(), f2); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); } TEST_F(TrackMatrixTest, trackSize) @@ -200,8 +200,8 @@ TEST_F(TrackMatrixTest, trackSize) * f2 trk 1 */ - ASSERT_EQ(track_matrix.trackSize(f0->trackId()), (unsigned int) 2); - ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 1); + ASSERT_EQ(track_matrix.trackSize(f0->trackId()), (unsigned int)2); + ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int)1); } TEST_F(TrackMatrixTest, first_last_Feature) @@ -222,10 +222,10 @@ TEST_F(TrackMatrixTest, first_last_Feature) track_matrix.newTrack(f2); ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f0); - ASSERT_EQ(track_matrix.lastFeature (f0->trackId()), f1); - ASSERT_EQ(track_matrix.feature (f0->trackId(), C0 ), f0); - ASSERT_EQ(track_matrix.feature (f0->trackId(), C1 ), f1); - ASSERT_EQ(track_matrix.feature (f2->trackId(), C1 ), f2); + ASSERT_EQ(track_matrix.lastFeature(f0->trackId()), f1); + ASSERT_EQ(track_matrix.feature(f0->trackId(), C0), f0); + ASSERT_EQ(track_matrix.feature(f0->trackId(), C1), f1); + ASSERT_EQ(track_matrix.feature(f2->trackId(), C1), f2); } TEST_F(TrackMatrixTest, remove_ftr) @@ -245,7 +245,7 @@ TEST_F(TrackMatrixTest, remove_ftr) * f2 trk 1 */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2); track_matrix.remove(f0); /* C0 C1 C2 snapshots @@ -254,12 +254,12 @@ TEST_F(TrackMatrixTest, remove_ftr) * * f2 trk 1 */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2); ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f1); ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2); - ASSERT_EQ(track_matrix.snapshot(C0).size(), (unsigned int) 1); + ASSERT_EQ(track_matrix.snapshot(C0).size(), (unsigned int)1); ASSERT_EQ(track_matrix.snapshot(C0).at(f2->trackId()), f2); - ASSERT_EQ(track_matrix.snapshot(C1).size(), (unsigned int) 1); + ASSERT_EQ(track_matrix.snapshot(C1).size(), (unsigned int)1); ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1); track_matrix.remove(f1); @@ -267,14 +267,14 @@ TEST_F(TrackMatrixTest, remove_ftr) * * f2 trk 1 */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2); track_matrix.remove(f2); /* C0 C1 C2 snapshots * */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)0); } TEST_F(TrackMatrixTest, remove_trk) @@ -294,14 +294,14 @@ TEST_F(TrackMatrixTest, remove_trk) * f2 trk 1 */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2); track_matrix.remove(f0->trackId()); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2); track_matrix.remove(f2->trackId()); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)0); } TEST_F(TrackMatrixTest, remove_snapshot) @@ -321,18 +321,18 @@ TEST_F(TrackMatrixTest, remove_snapshot) * f2 trk 1 */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2); track_matrix.remove(C0); /* C1 C2 snapshots * * f1 trk 0 */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1); ASSERT_EQ(track_matrix.firstFeature(f1->trackId()), f1); track_matrix.remove(C1); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int)0); } TEST_F(TrackMatrixTest, track) @@ -355,11 +355,11 @@ TEST_F(TrackMatrixTest, track) Track t0 = track_matrix.track(f0->trackId()); Track t2 = track_matrix.track(f2->trackId()); - ASSERT_EQ(t0.size(), (unsigned int) 2); + ASSERT_EQ(t0.size(), (unsigned int)2); ASSERT_EQ(t0.at(C0->getTimeStamp()), f0); ASSERT_EQ(t0.at(C1->getTimeStamp()), f1); - ASSERT_EQ(t2.size(), (unsigned int) 1); + ASSERT_EQ(t2.size(), (unsigned int)1); ASSERT_EQ(t2.at(C0->getTimeStamp()), f2); } @@ -383,11 +383,11 @@ TEST_F(TrackMatrixTest, snapshot) Snapshot s0 = track_matrix.snapshot(C0); Snapshot s1 = track_matrix.snapshot(C1); - ASSERT_EQ(s0.size(), (unsigned int) 2); + ASSERT_EQ(s0.size(), (unsigned int)2); ASSERT_EQ(s0.at(f0->trackId()), f0); ASSERT_EQ(s0.at(f2->trackId()), f2); - ASSERT_EQ(s1.size(), (unsigned int) 1); + ASSERT_EQ(s1.size(), (unsigned int)1); ASSERT_EQ(s1.at(f1->trackId()), f1); } @@ -408,9 +408,9 @@ TEST_F(TrackMatrixTest, trackAsVector) * f2 trk 1 */ - std::vector<FeatureBasePtr> vt0 = track_matrix.trackAsVector(f0->trackId()); // get track 0 as vector + std::vector<FeatureBasePtr> vt0 = track_matrix.trackAsVector(f0->trackId()); // get track 0 as vector - ASSERT_EQ(vt0.size(), (unsigned int) 2); + ASSERT_EQ(vt0.size(), (unsigned int)2); ASSERT_EQ(vt0[0], f0); ASSERT_EQ(vt0.at(1), f1); } @@ -432,11 +432,11 @@ TEST_F(TrackMatrixTest, snapshotAsList) * f2 trk 1 */ - std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapture()); // get track 0 as vector + std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapture()); // get track 0 as vector - ASSERT_EQ(lt0.size() , (unsigned int) 2); + ASSERT_EQ(lt0.size(), (unsigned int)2); ASSERT_EQ(lt0.front(), f0); - ASSERT_EQ(lt0.back() , f2); + ASSERT_EQ(lt0.back(), f2); } TEST_F(TrackMatrixTest, matches) @@ -462,13 +462,13 @@ TEST_F(TrackMatrixTest, matches) TrackMatches pairs = track_matrix.matches(C0, C2); - ASSERT_EQ(pairs.size(), (unsigned int) 1); + ASSERT_EQ(pairs.size(), (unsigned int)1); ASSERT_EQ(pairs.at(f0->trackId()).first, f0); ASSERT_EQ(pairs.at(f0->trackId()).second, f2); pairs = track_matrix.matches(C2, C3); - ASSERT_EQ(pairs.size(), (unsigned int) 0); + ASSERT_EQ(pairs.size(), (unsigned int)0); } TEST_F(TrackMatrixTest, trackAtKeyframes) @@ -515,7 +515,7 @@ TEST_F(TrackMatrixTest, trackIds) track_matrix.newTrack(f3); track_matrix.add(f0->trackId(), f4); track_matrix.add(f2->trackId(), f5); - + /* KC0 C1 C2 * * f0---f1---f4 trk 0 @@ -525,16 +525,14 @@ TEST_F(TrackMatrixTest, trackIds) * f3 trk 2 */ - ASSERT_EQ(track_matrix.trackIds().size(), 3); + ASSERT_EQ(track_matrix.trackIds().size(), 3); ASSERT_EQ(track_matrix.trackIds(C0).size(), 1); ASSERT_EQ(track_matrix.trackIds(C1).size(), 3); ASSERT_EQ(track_matrix.trackIds(C2).size(), 2); } - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index 885b2a0fd68d02fb9fd864fb36495f9f671a6e4e..d5782545551208bb1ce6ee48d1dde46713f03f30 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -20,7 +20,6 @@ #include "core/utils/utils_gtest.h" - #include "core/problem/problem.h" #include "core/trajectory/trajectory_base.h" #include "core/frame/frame_base.h" @@ -36,8 +35,7 @@ bool debug = false; TEST(TrajectoryBase, ClosestKeyFrame) { - - ProblemPtr P = Problem::create("PO", 2); + ProblemPtr P = Problem::create("PO", 2); TrajectoryBasePtr T = P->getTrajectory(); // Trajectory status: @@ -45,41 +43,40 @@ TEST(TrajectoryBase, ClosestKeyFrame) // 1 2 3 4 time stamps // --+-----+-----+-----+---> time - FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero() ); - FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero() ); - FrameBasePtr F3 = std::make_shared<FrameBase>(3, - P->getFrameTypes(), - VectorComposite("PO", {Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) ); - FrameBasePtr F4 = std::make_shared<FrameBase>(4, - P->getFrameTypes(), - VectorComposite("PO", {Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) ); + FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero()); + FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero()); + FrameBasePtr F3 = std::make_shared<FrameBase>( + 3, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()})); + FrameBasePtr F4 = std::make_shared<FrameBase>( + 4, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()})); - FrameBasePtr KF; // closest key-frame queried + FrameBasePtr KF; // closest key-frame queried - KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1 - ASSERT_EQ(KF->id(), F1->id()); // same id! + KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1 + ASSERT_EQ(KF->id(), F1->id()); // same id! - KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1 - ASSERT_EQ(KF->id(), F1->id()); // same id! + KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1 + ASSERT_EQ(KF->id(), F1->id()); // same id! - KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! + KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! - KF = T->closestFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! + KF = T->closestFrameToTimeStamp( + 2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! - KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! + KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! - KF = T->closestFrameToTimeStamp(4.2); // after the last frame --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! + KF = T->closestFrameToTimeStamp(4.2); // after the last frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! } TEST(TrajectoryBase, Add_Remove_Frame) { using std::make_shared; - ProblemPtr P = Problem::create("PO", 2); + ProblemPtr P = Problem::create("PO", 2); TrajectoryBasePtr T = P->getTrajectory(); SolverManagerDummy N(P); @@ -92,66 +89,66 @@ TEST(TrajectoryBase, Add_Remove_Frame) std::cout << __LINE__ << std::endl; // add F1 - FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero()); // 2 non-fixed - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); + FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero()); // 2 non-fixed + if (debug) P->print(2, 0, 0, 0); + ASSERT_EQ(T->getFrameMap().size(), (SizeStd)1); + ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)2); std::cout << __LINE__ << std::endl; // add F2 - FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero()); // 1 fixed, 1 not - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4); + FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero()); // 1 fixed, 1 not + if (debug) P->print(2, 0, 0, 0); + ASSERT_EQ(T->getFrameMap().size(), (SizeStd)2); + ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)4); std::cout << __LINE__ << std::endl; // add F3 - FrameBasePtr F3 = std::make_shared<FrameBase>(3, - P->getFrameTypes(), - VectorComposite("PO", {Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()})); - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4); + FrameBasePtr F3 = std::make_shared<FrameBase>( + 3, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()})); + if (debug) P->print(2, 0, 0, 0); + ASSERT_EQ(T->getFrameMap().size(), (SizeStd)2); + ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)4); std::cout << __LINE__ << std::endl; ASSERT_EQ(T->getLastFrame()->id(), F2->id()); std::cout << __LINE__ << std::endl; N.update(); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 0); // consumeStateBlockNotificationMap was called in update() so it should be empty + ASSERT_EQ(P->getStateBlockNotificationMapSize(), + (SizeStd)0); // consumeStateBlockNotificationMap was called in update() so it should be empty std::cout << __LINE__ << std::endl; // remove frames and keyframes - F2->remove(); // KF - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); + F2->remove(); // KF + if (debug) P->print(2, 0, 0, 0); + ASSERT_EQ(T->getFrameMap().size(), (SizeStd)1); + ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)2); std::cout << __LINE__ << std::endl; ASSERT_EQ(T->getLastFrame()->id(), F1->id()); std::cout << __LINE__ << std::endl; - F3->remove(); // F - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1); + F3->remove(); // F + if (debug) P->print(2, 0, 0, 0); + ASSERT_EQ(T->getFrameMap().size(), (SizeStd)1); std::cout << __LINE__ << std::endl; ASSERT_EQ(T->getLastFrame()->id(), F1->id()); - F1->remove(); // KF - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 0); + F1->remove(); // KF + if (debug) P->print(2, 0, 0, 0); + ASSERT_EQ(T->getFrameMap().size(), (SizeStd)0); std::cout << __LINE__ << std::endl; N.update(); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 0); // consumeStateBlockNotificationMap was called in update() so it should be empty + ASSERT_EQ(P->getStateBlockNotificationMapSize(), + (SizeStd)0); // consumeStateBlockNotificationMap was called in update() so it should be empty std::cout << __LINE__ << std::endl; } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp index cfcd59307d86bdbe0a08609c7fa7c76207ce2931..666fae18bb85167c8b6bfbc3af417baf94a4aa0c 100644 --- a/test/gtest_tree_manager.cpp +++ b/test/gtest_tree_manager.cpp @@ -47,9 +47,8 @@ TEST(TreeManager, createNode) { ProblemPtr P = Problem::create("PO", 2); - auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir}, - wolf_dir + "/test/yaml/tree_manager_dummy.yaml"); - + auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir}, wolf_dir + "/test/yaml/tree_manager_dummy.yaml"); + WOLF_INFO_COND(not yaml_server.applySchema("TreeManagerDummy"), yaml_server.getLog()); ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy")); @@ -81,8 +80,7 @@ TEST(TreeManager, FactoryParam) { ProblemPtr P = Problem::create("PO", 2); - auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir}, - wolf_dir + "/test/yaml/tree_manager_dummy.yaml"); + auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir}, wolf_dir + "/test/yaml/tree_manager_dummy.yaml"); ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy")); auto GM = FactoryTreeManager::create("TreeManagerDummy", yaml_server.getNode()); @@ -99,9 +97,8 @@ TEST(TreeManager, FactoryYaml) { ProblemPtr P = Problem::create("PO", 2); - auto GM = FactoryTreeManagerYaml::create("TreeManagerDummy", - wolf_dir + "/test/yaml/tree_manager_dummy.yaml", - {wolf_dir}); + auto GM = FactoryTreeManagerYaml::create( + "TreeManagerDummy", wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir}); ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr); @@ -117,7 +114,7 @@ TEST(TreeManager, autoConf) P->applyPriorOptions(0); ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr); - ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF + ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF } TEST(TreeManager, autoConfNone) @@ -125,7 +122,7 @@ TEST(TreeManager, autoConfNone) ProblemPtr P = Problem::autoSetup(wolf_dir + "/test/yaml/problem_tree_manager2.yaml", {wolf_dir}); P->applyPriorOptions(0); - ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None + ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None } TEST(TreeManager, keyFrameCallback) @@ -140,8 +137,9 @@ TEST(TreeManager, keyFrameCallback) ASSERT_EQ(GM->n_KF_, 0); - Vector7d x; x << 1,2,3,0,0,0,1; - auto F0 = P->emplaceFrame(0, "PO", x ); + Vector7d x; + x << 1, 2, 3, 0, 0, 0, 1; + auto F0 = P->emplaceFrame(0, "PO", x); P->keyFrameCallback(F0, nullptr); ASSERT_EQ(GM->n_KF_, 1); @@ -149,6 +147,6 @@ TEST(TreeManager, keyFrameCallback) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index 074f3f1c3f2a972aed266160cd49cffc203d4ea3..e92f2185848ad7b00ff739393f384c4261ec6af3 100644 --- a/test/gtest_tree_manager_sliding_window.cpp +++ b/test/gtest_tree_manager_sliding_window.cpp @@ -69,8 +69,7 @@ TEST(TreeManagerSlidingWindow, createYaml) { ProblemPtr P = Problem::create("PO", 2); - auto GM = - TreeManagerSlidingWindow::create(wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir}); + auto GM = TreeManagerSlidingWindow::create(wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir}); EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr); diff --git a/wolf_scripts/templates/class_template.cpp b/wolf_scripts/templates/class_template.cpp index 22a6cbe179c6c1b9cfd751153ab341e6566a102a..6306b63f52e70bac278fcb4d83f0b376efe0d425 100644 --- a/wolf_scripts/templates/class_template.cpp +++ b/wolf_scripts/templates/class_template.cpp @@ -19,20 +19,17 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. #include "header_file" - -namespace wolf { +namespace wolf +{ // Constructor // TODO Modify this default API to suit your class needs -class_name::class_name() : - base_class() +class_name::class_name() : base_class() { // TODO Add your code or remove this comment } // Destructor -class_name::~class_name() -{ -} +class_name::~class_name() {} -} // namespace wolf +} // namespace wolf diff --git a/wolf_scripts/templates/class_template.h b/wolf_scripts/templates/class_template.h index 368f1bc347ed0347de13a67d66c99f1b70f113ab..849cbc5deea779a88b6c87b27efac77aacc9b7c5 100644 --- a/wolf_scripts/templates/class_template.h +++ b/wolf_scripts/templates/class_template.h @@ -20,27 +20,25 @@ #pragma once -//Wolf includes +// Wolf includes #include "wolf.h" #include "base_header_file" namespace wolf { - WOLF_PTR_TYPEDEFS(class_name); class class_name : public base_class { - public: - - /** \brief Class constructor - */ - // TODO Modify this default API to suit your class needs - class_name(); - - /** \brief Class Destructor - */ - virtual ~class_name(); + public: + /** \brief Class constructor + */ + // TODO Modify this default API to suit your class needs + class_name(); + + /** \brief Class Destructor + */ + virtual ~class_name(); }; -} // namespace wolf +} // namespace wolf diff --git a/wolf_scripts/templates/gtest_template.cpp b/wolf_scripts/templates/gtest_template.cpp index a15aadaff4fda914c2448c6e383889b67f8b3f96..38e00df79eef976f02a18f4860e3cdd55c20c249 100644 --- a/wolf_scripts/templates/gtest_template.cpp +++ b/wolf_scripts/templates/gtest_template.cpp @@ -30,7 +30,7 @@ using namespace wolf; using std::static_pointer_cast; // Use the following in case you want to initialize tests with predefines variables or methods. -//class class_name_class : public testing::Test{ +// class class_name_class : public testing::Test{ // public: // virtual void SetUp() // { @@ -54,4 +54,3 @@ int main(int argc, char **argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } -