diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index cb2d44224..3528424fa 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -43,7 +43,7 @@ string findExampleDataFile(const string& name) { // Search source tree and installed location vector rootsToSearch; rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt - rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt + rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt // Search for filename as given, and with .graph and .txt extensions vector namesToSearch; @@ -55,32 +55,34 @@ string findExampleDataFile(const string& name) { // Find first name that exists BOOST_FOREACH(const fs::path& root, rootsToSearch) { BOOST_FOREACH(const fs::path& name, namesToSearch) { - if(fs::is_regular_file(root / name)) + if (fs::is_regular_file(root / name)) return (root / name).string(); } } // If we did not return already, then we did not find the file - throw std::invalid_argument( - "gtsam::findExampleDataFile could not find a matching file in\n" - SOURCE_TREE_DATASET_DIR " or\n" - INSTALLED_DATASET_DIR " named\n" + - name + ", " + name + ".graph, or " + name + ".txt"); + throw +std::invalid_argument( + "gtsam::findExampleDataFile could not find a matching file in\n" + SOURCE_TREE_DATASET_DIR " or\n" + INSTALLED_DATASET_DIR " named\n" + + name + ", " + name + ".graph, or " + name + ".txt"); } /* ************************************************************************* */ string createRewrittenFileName(const string& name) { // Search source tree and installed location - if(!exists(fs::path(name))) { - throw std::invalid_argument( - "gtsam::createRewrittenFileName could not find a matching file in\n" - + name); + if (!exists(fs::path(name))) { + throw std::invalid_argument( + "gtsam::createRewrittenFileName could not find a matching file in\n" + + name); } - fs::path p(name); - fs::path newpath = fs::path(p.parent_path().string()) / fs::path(p.stem().string() + "-rewritten.txt" ); + fs::path p(name); + fs::path newpath = fs::path(p.parent_path().string()) + / fs::path(p.stem().string() + "-rewritten.txt"); - return newpath.string(); + return newpath.string(); } /* ************************************************************************* */ @@ -95,7 +97,8 @@ pair load2D( /* ************************************************************************* */ pair load2D( - const string& filename, boost::optional model, int maxID, + const string& filename, + boost::optional model, int maxID, bool addNoise, bool smart) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); @@ -109,7 +112,7 @@ pair load2D( // load the poses while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; if ((tag == "VERTEX2") || (tag == "VERTEX")) { @@ -133,7 +136,7 @@ pair load2D( int id1, id2; bool haveLandmark = false; while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { @@ -144,20 +147,18 @@ pair load2D( is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; // Try to guess covariance matrix layout - Matrix m(3,3); - if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) - { + Matrix m(3, 3); + if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 == 0.0) { // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] - m << v1, v2, v5, v2, v3, v6, v5, v6, v4; - } - else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) - { + m << v1, v2, v5, v2, v3, v6, v5, v6, v4; + } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 != 0.0) { // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] - m << v1, v2, v3, v2, v4, v5, v3, v5, v6; - } - else - { - throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file"); + m << v1, v2, v3, v2, v4, v5, v3, v5, v6; + } else { + throw std::invalid_argument( + "load2D: unrecognized covariance matrix format in dataset file"); } // optional filter @@ -203,22 +204,21 @@ pair load2D( // Convert x,y to bearing,range bearing = std::atan2(lmy, lmx); - range = std::sqrt(lmx*lmx + lmy*lmy); + range = std::sqrt(lmx * lmx + lmy * lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. - if(std::abs(v1 - v3) < 1e-4) - { + if (std::abs(v1 - v3) < 1e-4) { bearing_std = sqrt(v1 / 10.0); range_std = sqrt(v1); - } - else - { + } else { bearing_std = 1; range_std = 1; - if(!haveLandmark) { - cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" - "non-uniform covariance on LANDMARK measurements in this file." << endl; + if (!haveLandmark) { + cout + << "Warning: load2D is a very simple dataset loader and is ignoring the\n" + "non-uniform covariance on LANDMARK measurements in this file." + << endl; haveLandmark = true; } } @@ -244,7 +244,7 @@ pair load2D( initial->insert(id1, Pose2()); if (!initial->exists(L(id2))) { Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 local(cos(bearing) * range, sin(bearing) * range); Point2 global = pose.transform_from(local); initial->insert(L(id2), global); } @@ -265,18 +265,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, fstream stream(filename.c_str(), fstream::out); // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) - { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { const Pose2& pose = dynamic_cast(key_value.value); - stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " - << pose.y() << " " << pose.theta() << endl; + stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() + << " " << pose.theta() << endl; } // save edges Matrix R = model->R(); Matrix RR = trans(R) * R; //prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr factor_, graph) - { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (!factor) @@ -284,9 +282,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, Pose2 pose = factor->measured().inverse(); stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " - << pose.x() << " " << pose.y() << " " << pose.theta() << " " - << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " - << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; + << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) + << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " + << RR(0, 2) << " " << RR(1, 2) << endl; } stream.close(); @@ -411,14 +409,15 @@ pair load2D_robust( noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); + *graph += BearingRangeFactor(id1, id2, bearing, range, + measurementNoise); // Insert poses or points if they do not exist yet if (!initial->exists(id1)) initial->insert(id1, Pose2()); if (!initial->exists(id2)) { Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 local(cos(bearing) * range, sin(bearing) * range); Point2 global = pose.transform_from(local); initial->insert(id2, global); } @@ -427,69 +426,66 @@ pair load2D_robust( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } /* ************************************************************************* */ -Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL +Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL /* R = [ 1 0 0 * 0 -1 0 * 0 0 -1] */ - Matrix3 R_mat = Matrix3::Zero(3,3); - R_mat(0,0) = 1.0; R_mat(1,1) = -1.0; R_mat(2,2) = -1.0; + Matrix3 R_mat = Matrix3::Zero(3, 3); + R_mat(0, 0) = 1.0; + R_mat(1, 1) = -1.0; + R_mat(2, 2) = -1.0; return Rot3(R_mat); } /* ************************************************************************* */ -Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) -{ +Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) { Rot3 R90 = openGLFixedRotation(); - Rot3 wRc = ( R.inverse() ).compose(R90); + Rot3 wRc = (R.inverse()).compose(R90); // Our camera-to-world translation wTc = -R'*t - return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz))); + return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); } /* ************************************************************************* */ -Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) -{ +Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) { Rot3 R90 = openGLFixedRotation(); - Rot3 cRw_openGL = R90.compose( R.inverse() ); - Point3 t_openGL = cRw_openGL.rotate(Point3(-tx,-ty,-tz)); + Rot3 cRw_openGL = R90.compose(R.inverse()); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); return Pose3(cRw_openGL, t_openGL); } /* ************************************************************************* */ -Pose3 gtsam2openGL(const Pose3& PoseGTSAM) -{ - return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z()); +Pose3 gtsam2openGL(const Pose3& PoseGTSAM) { + return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), + PoseGTSAM.z()); } /* ************************************************************************* */ -bool readBundler(const string& filename, SfM_data &data) -{ +bool readBundler(const string& filename, SfM_data &data) { // Load the data file - ifstream is(filename.c_str(),ifstream::in); - if(!is) - { + ifstream is(filename.c_str(), ifstream::in); + if (!is) { cout << "Error in readBundler: can not find the file!!" << endl; return false; } // Ignore the first line char aux[500]; - is.getline(aux,500); + is.getline(aux, 500); // Get the number of camera poses and 3D points size_t nrPoses, nrPoints; is >> nrPoses >> nrPoints; // Get the information for the camera poses - for( size_t i = 0; i < nrPoses; i++ ) - { + for (size_t i = 0; i < nrPoses; i++) { // Get the focal length and the radial distortion parameters float f, k1, k2; is >> f >> k1 >> k2; @@ -499,20 +495,15 @@ bool readBundler(const string& filename, SfM_data &data) float r11, r12, r13; float r21, r22, r23; float r31, r32, r33; - is >> r11 >> r12 >> r13 - >> r21 >> r22 >> r23 - >> r31 >> r32 >> r33; + is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; // Bundler-OpenGL rotation matrix - Rot3 R( - r11, r12, r13, - r21, r22, r23, - r31, r32, r33); + Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); // Check for all-zero R, in which case quit - if(r11==0 && r12==0 && r13==0) - { - cout << "Error in readBundler: zero rotation matrix for pose " << i << endl; + if (r11 == 0 && r12 == 0 && r13 == 0) { + cout << "Error in readBundler: zero rotation matrix for pose " << i + << endl; return false; } @@ -520,38 +511,36 @@ bool readBundler(const string& filename, SfM_data &data) float tx, ty, tz; is >> tx >> ty >> tz; - Pose3 pose = openGL2gtsam(R,tx,ty,tz); + Pose3 pose = openGL2gtsam(R, tx, ty, tz); - data.cameras.push_back(SfM_Camera(pose,K)); + data.cameras.push_back(SfM_Camera(pose, K)); } // Get the information for the 3D points - for( size_t j = 0; j < nrPoints; j++ ) - { + for (size_t j = 0; j < nrPoints; j++) { SfM_Track track; // Get the 3D position float x, y, z; is >> x >> y >> z; - track.p = Point3(x,y,z); + track.p = Point3(x, y, z); // Get the color information float r, g, b; is >> r >> g >> b; - track.r = r/255.f; - track.g = g/255.f; - track.b = b/255.f; + track.r = r / 255.f; + track.g = g / 255.f; + track.b = b / 255.f; // Now get the visibility information size_t nvisible = 0; is >> nvisible; - for( size_t k = 0; k < nvisible; k++ ) - { + for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.measurements.push_back(make_pair(cam_idx,Point2(u,-v))); + track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); } data.tracks.push_back(track); @@ -562,11 +551,11 @@ bool readBundler(const string& filename, SfM_data &data) } /* ************************************************************************* */ -bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, - const kernelFunctionType kernelFunction){ +bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, + Values& initial, const kernelFunctionType kernelFunction) { ifstream is(g2oFile.c_str()); - if (!is){ + if (!is) { throw std::invalid_argument("File not found!"); return false; } @@ -574,7 +563,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in // READ INITIAL GUESS FROM G2O FILE string tag; while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; if (tag == "VERTEX_SE2" || tag == "VERTEX2") { @@ -591,7 +580,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in // READ MEASUREMENTS FROM G2O FILE while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; if (tag == "EDGE_SE2" || tag == "EDGE2") { @@ -602,23 +591,35 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in is >> id1 >> id2 >> x >> y >> yaw; is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33; Pose2 l1Xl2(x, y, yaw); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions( + (Vector(3) << I11, I22, I33)); switch (kernelFunction) { - {case QUADRATIC: - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, model)); + { + case QUADRATIC: + NonlinearFactor::shared_ptr factor( + new BetweenFactor(id1, id2, l1Xl2, model)); graph.add(factor); - break;} - {case HUBER: - NonlinearFactor::shared_ptr huberFactor(new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); + break; + } + { + case HUBER: + NonlinearFactor::shared_ptr huberFactor( + new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), model))); graph.add(huberFactor); - break;} - {case TUKEY: - NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model))); + break; + } + { + case TUKEY: + NonlinearFactor::shared_ptr tukeyFactor( + new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create( + noiseModel::mEstimator::Tukey::Create(4.6851), model))); graph.add(tukeyFactor); - break;} + break; + } } } is.ignore(LINESIZE, '\n'); @@ -626,31 +627,32 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in // Output which kernel is used switch (kernelFunction) { case QUADRATIC: - break; + break; case HUBER: - std::cout << "Robust kernel: Huber" << std::endl; break; + std::cout << "Robust kernel: Huber" << std::endl; + break; case TUKEY: - std::cout << "Robust kernel: Tukey" << std::endl; break; + std::cout << "Robust kernel: Tukey" << std::endl; + break; } return true; } /* ************************************************************************* */ -bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ +bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, + const Values& estimate) { fstream stream(filename.c_str(), fstream::out); // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) - { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { const Pose2& pose = dynamic_cast(key_value.value); stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } // save edges - BOOST_FOREACH(boost::shared_ptr factor_, graph) - { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (!factor) @@ -660,25 +662,25 @@ bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, co boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) - throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!"); + throw std::invalid_argument( + "writeG2o: invalid noise model (current version assumes diagonal noise model)!"); Pose2 pose = factor->measured(); //.inverse(); stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " - << diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl; + << diagonalModel->precision(1) << " " << 0.0 << " " + << diagonalModel->precision(2) << endl; } stream.close(); return true; } /* ************************************************************************* */ -bool readBAL(const string& filename, SfM_data &data) -{ +bool readBAL(const string& filename, SfM_data &data) { // Load the data file - ifstream is(filename.c_str(),ifstream::in); - if(!is) - { + ifstream is(filename.c_str(), ifstream::in); + if (!is) { cout << "Error in readBAL: can not find the file!!" << endl; return false; } @@ -690,44 +692,41 @@ bool readBAL(const string& filename, SfM_data &data) data.tracks.resize(nrPoints); // Get the information for the observations - for( size_t k = 0; k < nrObservations; k++ ) - { + for (size_t k = 0; k < nrObservations; k++) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.push_back(make_pair(i,Point2(u,-v))); + data.tracks[j].measurements.push_back(make_pair(i, Point2(u, -v))); } // Get the information for the camera poses - for( size_t i = 0; i < nrPoses; i++ ) - { + for (size_t i = 0; i < nrPoses; i++) { // Get the rodriguez vector float wx, wy, wz; is >> wx >> wy >> wz; - Rot3 R = Rot3::rodriguez(wx, wy, wz);// BAL-OpenGL rotation matrix + Rot3 R = Rot3::rodriguez(wx, wy, wz); // BAL-OpenGL rotation matrix // Get the translation vector float tx, ty, tz; is >> tx >> ty >> tz; - Pose3 pose = openGL2gtsam(R,tx,ty,tz); + Pose3 pose = openGL2gtsam(R, tx, ty, tz); // Get the focal length and the radial distortion parameters float f, k1, k2; is >> f >> k1 >> k2; Cal3Bundler K(f, k1, k2); - data.cameras.push_back(SfM_Camera(pose,K)); + data.cameras.push_back(SfM_Camera(pose, K)); } // Get the information for the 3D points - for( size_t j = 0; j < nrPoints; j++ ) - { + for (size_t j = 0; j < nrPoints; j++) { // Get the 3D position float x, y, z; is >> x >> y >> z; SfM_Track& track = data.tracks[j]; - track.p = Point3(x,y,z); + track.p = Point3(x, y, z); track.r = 0.4f; track.g = 0.4f; track.b = 0.4f; @@ -738,8 +737,7 @@ bool readBAL(const string& filename, SfM_data &data) } /* ************************************************************************* */ -bool writeBAL(const string& filename, SfM_data &data) -{ +bool writeBAL(const string& filename, SfM_data &data) { // Open the output file ofstream os; os.open(filename.c_str()); @@ -750,49 +748,55 @@ bool writeBAL(const string& filename, SfM_data &data) } // Write the number of camera poses and 3D points - size_t nrObservations=0; - for (size_t j = 0; j < data.number_tracks(); j++){ + size_t nrObservations = 0; + for (size_t j = 0; j < data.number_tracks(); j++) { nrObservations += data.tracks[j].number_measurements(); } // Write observations - os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl; + os << data.number_cameras() << " " << data.number_tracks() << " " + << nrObservations << endl; os << endl; - for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j SfM_Track track = data.tracks[j]; - for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j + for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); - if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;} + if (u0 != 0 || v0 != 0) { + cout + << "writeBAL has not been tested for calibration with nonzero (u0,v0)" + << endl; + } double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = - (track.measurements[k].second.y() - v0); // center of image is the origin + double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); - os << i /*camera id*/ << " " << j /*point id*/ << " " - << pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl; + os << i /*camera id*/<< " " << j /*point id*/<< " " + << pixelMeasurement.x() /*u of the pixel*/<< " " + << pixelMeasurement.y() /*v of the pixel*/<< endl; } } os << endl; // Write cameras - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera Pose3 poseGTSAM = data.cameras[i].pose(); Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); - os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().vector() << endl; - os << cameraCalibration.fx() << endl; - os << cameraCalibration.k1() << endl; - os << cameraCalibration.k2() << endl; + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().vector() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; os << endl; } // Write the points - for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j Point3 point = data.tracks[j].p; os << point.x() << endl; os << point.y() << endl; @@ -804,48 +808,55 @@ bool writeBAL(const string& filename, SfM_data &data) return true; } -bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){ +bool writeBALfromValues(const string& filename, const SfM_data &data, + Values& values) { SfM_data dataValues = data; // Store poses or cameras in SfM_data Values valuesPoses = values.filter(); - if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses - for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); + if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera + Key poseKey = symbol('x', i); Pose3 pose = values.at(poseKey); Cal3Bundler K = dataValues.cameras[i].calibration(); PinholeCamera camera(pose, K); dataValues.cameras[i] = camera; } } else { - Values valuesCameras = values.filter< PinholeCamera >(); - if ( valuesCameras.size() == dataValues.number_cameras() ){ // we only estimated camera poses and calibration - for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera + Values valuesCameras = values.filter >(); + if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration + for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera Key cameraKey = i; // symbol('c',i); - PinholeCamera camera = values.at >(cameraKey); + PinholeCamera camera = + values.at >(cameraKey); dataValues.cameras[i] = camera; } - }else{ - cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras() - <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; + } else { + cout + << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " + << dataValues.number_cameras() << ") and values (#cameras " + << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" + << endl; return false; } } // Store 3D points in SfM_data Values valuesPoints = values.filter(); - if( valuesPoints.size() != dataValues.number_tracks()){ - cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks() - <<") and values (#points " << valuesPoints.size() << ")!!" << endl; + if (valuesPoints.size() != dataValues.number_tracks()) { + cout + << "writeBALfromValues: different number of points in SfM_dataValues (#points= " + << dataValues.number_tracks() << ") and values (#points " + << valuesPoints.size() << ")!!" << endl; } - for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point + for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point Key pointKey = P(j); - if(values.exists(pointKey)){ + if (values.exists(pointKey)) { Point3 point = values.at(pointKey); dataValues.tracks[j].p = point; - }else{ + } else { dataValues.tracks[j].r = 1.0; dataValues.tracks[j].g = 0.0; dataValues.tracks[j].b = 0.0; @@ -861,7 +872,7 @@ Values initialCamerasEstimate(const SfM_data& db) { Values initial; size_t i = 0; // NO POINTS: j = 0; BOOST_FOREACH(const SfM_Camera& camera, db.cameras) - initial.insert(i++, camera); + initial.insert(i++, camera); return initial; } @@ -869,9 +880,9 @@ Values initialCamerasAndPointsEstimate(const SfM_data& db) { Values initial; size_t i = 0, j = 0; BOOST_FOREACH(const SfM_Camera& camera, db.cameras) - initial.insert((i++), camera); + initial.insert((i++), camera); BOOST_FOREACH(const SfM_Track& track, db.tracks) - initial.insert(P(j++), track.p); + initial.insert(P(j++), track.p); return initial; }