12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Formatting

release/4.3a0
dellaert 2014-05-31 18:08:02 -04:00
parent a84a9a67d6
commit 204ddbee5e
1 changed files with 186 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

213
gtsam/slam/dataset.cpp
View File

@ -61,7 +61,8 @@ string findExampleDataFile(const string& name) {
} }
// If we did not return already, then we did not find the file // If we did not return already, then we did not find the file
throw std::invalid_argument( throw
std::invalid_argument(
"gtsam::findExampleDataFile could not find a matching file in\n" "gtsam::findExampleDataFile could not find a matching file in\n"
SOURCE_TREE_DATASET_DIR " or\n" SOURCE_TREE_DATASET_DIR " or\n"
INSTALLED_DATASET_DIR " named\n" + INSTALLED_DATASET_DIR " named\n" +
@ -78,7 +79,8 @@ string createRewrittenFileName(const string& name) {
} }
fs::path p(name); fs::path p(name);
fs::path newpath = fs::path(p.parent_path().string()) / fs::path(p.stem().string() + "-rewritten.txt" ); 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<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
/* ************************************************************************* */ /* ************************************************************************* */
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D( pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const string& filename, boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID, const string& filename,
boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID,
bool addNoise, bool smart) { bool addNoise, bool smart) {
cout << "Will try to read " << filename << endl; cout << "Will try to read " << filename << endl;
ifstream is(filename.c_str()); ifstream is(filename.c_str());
@ -145,19 +148,17 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// Try to guess covariance matrix layout // Try to guess covariance matrix layout
Matrix m(3, 3); Matrix m(3, 3);
if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) 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 ] // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
m << 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
else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) && v6 != 0.0) {
{
// Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
m << v1, v2, v3, v2, v4, v5, v3, v5, v6; m << v1, v2, v3, v2, v4, v5, v3, v5, v6;
} } else {
else throw std::invalid_argument(
{ "load2D: unrecognized covariance matrix format in dataset file");
throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file");
} }
// optional filter // optional filter
@ -207,18 +208,17 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// In our experience, the x-y covariance on landmark sightings is not very good, so assume // 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. // 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); bearing_std = sqrt(v1 / 10.0);
range_std = sqrt(v1); range_std = sqrt(v1);
} } else {
else
{
bearing_std = 1; bearing_std = 1;
range_std = 1; range_std = 1;
if (!haveLandmark) { if (!haveLandmark) {
cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" cout
"non-uniform covariance on LANDMARK measurements in this file." << endl; << "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; haveLandmark = true;
} }
} }
@ -265,18 +265,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save poses // save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
{
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value); const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
<< pose.y() << " " << pose.theta() << endl; << " " << pose.theta() << endl;
} }
// save edges // save edges
Matrix R = model->R(); Matrix R = model->R();
Matrix RR = trans(R) * R; //prod(trans(R),R); Matrix RR = trans(R) * R; //prod(trans(R),R);
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
{
boost::shared_ptr<BetweenFactor<Pose2> > factor = boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor) if (!factor)
@ -284,9 +282,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
Pose2 pose = factor->measured().inverse(); Pose2 pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0)
<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " "
<< RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; << RR(0, 2) << " " << RR(1, 2) << endl;
} }
stream.close(); stream.close();
@ -411,7 +409,8 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise); *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range,
measurementNoise);
// Insert poses or points if they do not exist yet // Insert poses or points if they do not exist yet
if (!initial->exists(id1)) if (!initial->exists(id1))
@ -439,13 +438,14 @@ Rot3 openGLFixedRotation(){ // this is due to different convention for cameras i
* 0 0 -1] * 0 0 -1]
*/ */
Matrix3 R_mat = Matrix3::Zero(3, 3); 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; R_mat(0, 0) = 1.0;
R_mat(1, 1) = -1.0;
R_mat(2, 2) = -1.0;
return Rot3(R_mat); 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 R90 = openGLFixedRotation();
Rot3 wRc = (R.inverse()).compose(R90); Rot3 wRc = (R.inverse()).compose(R90);
@ -454,8 +454,7 @@ Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double 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 R90 = openGLFixedRotation();
Rot3 cRw_openGL = R90.compose(R.inverse()); Rot3 cRw_openGL = R90.compose(R.inverse());
Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
@ -463,18 +462,16 @@ Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz)
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 gtsam2openGL(const Pose3& PoseGTSAM) Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
{ return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z()); PoseGTSAM.z());
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool readBundler(const string& filename, SfM_data &data) bool readBundler(const string& filename, SfM_data &data) {
{
// Load the data file // Load the data file
ifstream is(filename.c_str(), ifstream::in); ifstream is(filename.c_str(), ifstream::in);
if(!is) if (!is) {
{
cout << "Error in readBundler: can not find the file!!" << endl; cout << "Error in readBundler: can not find the file!!" << endl;
return false; return false;
} }
@ -488,8 +485,7 @@ bool readBundler(const string& filename, SfM_data &data)
is >> nrPoses >> nrPoints; is >> nrPoses >> nrPoints;
// Get the information for the camera poses // 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 // Get the focal length and the radial distortion parameters
float f, k1, k2; float f, k1, k2;
is >> f >> k1 >> k2; is >> f >> k1 >> k2;
@ -499,20 +495,15 @@ bool readBundler(const string& filename, SfM_data &data)
float r11, r12, r13; float r11, r12, r13;
float r21, r22, r23; float r21, r22, r23;
float r31, r32, r33; float r31, r32, r33;
is >> r11 >> r12 >> r13 is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
>> r21 >> r22 >> r23
>> r31 >> r32 >> r33;
// Bundler-OpenGL rotation matrix // Bundler-OpenGL rotation matrix
Rot3 R( Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
r11, r12, r13,
r21, r22, r23,
r31, r32, r33);
// Check for all-zero R, in which case quit // Check for all-zero R, in which case quit
if(r11==0 && r12==0 && r13==0) if (r11 == 0 && r12 == 0 && r13 == 0) {
{ cout << "Error in readBundler: zero rotation matrix for pose " << i
cout << "Error in readBundler: zero rotation matrix for pose " << i << endl; << endl;
return false; return false;
} }
@ -526,8 +517,7 @@ bool readBundler(const string& filename, SfM_data &data)
} }
// Get the information for the 3D points // 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; SfM_Track track;
// Get the 3D position // Get the 3D position
@ -546,8 +536,7 @@ bool readBundler(const string& filename, SfM_data &data)
size_t nvisible = 0; size_t nvisible = 0;
is >> nvisible; 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; size_t cam_idx = 0, point_idx = 0;
float u, v; float u, v;
is >> cam_idx >> point_idx >> u >> v; is >> cam_idx >> point_idx >> u >> v;
@ -562,8 +551,8 @@ bool readBundler(const string& filename, SfM_data &data)
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph,
const kernelFunctionType kernelFunction){ Values& initial, const kernelFunctionType kernelFunction) {
ifstream is(g2oFile.c_str()); ifstream is(g2oFile.c_str());
if (!is) { if (!is) {
@ -602,23 +591,35 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
is >> id1 >> id2 >> x >> y >> yaw; is >> id1 >> id2 >> x >> y >> yaw;
is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33; is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33;
Pose2 l1Xl2(x, y, yaw); 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) { switch (kernelFunction) {
{case QUADRATIC: {
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model)); case QUADRATIC:
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph.add(factor); graph.add(factor);
break;} break;
{case HUBER: }
NonlinearFactor::shared_ptr huberFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, {
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); case HUBER:
NonlinearFactor::shared_ptr huberFactor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), model)));
graph.add(huberFactor); graph.add(huberFactor);
break;} break;
{case TUKEY: }
NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, {
noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model))); case TUKEY:
NonlinearFactor::shared_ptr tukeyFactor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(
noiseModel::mEstimator::Tukey::Create(4.6851), model)));
graph.add(tukeyFactor); graph.add(tukeyFactor);
break;} break;
}
} }
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
@ -628,29 +629,30 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
case QUADRATIC: case QUADRATIC:
break; break;
case HUBER: case HUBER:
std::cout << "Robust kernel: Huber" << std::endl; break; std::cout << "Robust kernel: Huber" << std::endl;
break;
case TUKEY: case TUKEY:
std::cout << "Robust kernel: Tukey" << std::endl; break; std::cout << "Robust kernel: Tukey" << std::endl;
break;
} }
return true; 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); fstream stream(filename.c_str(), fstream::out);
// save poses // save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
{
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value); const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
<< pose.y() << " " << pose.theta() << endl; << pose.y() << " " << pose.theta() << endl;
} }
// save edges // save edges
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
{
boost::shared_ptr<BetweenFactor<Pose2> > factor = boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor) if (!factor)
@ -660,25 +662,25 @@ bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, co
boost::shared_ptr<noiseModel::Diagonal> diagonalModel = boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model); boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel) 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(); Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " << 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(); stream.close();
return true; return true;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool readBAL(const string& filename, SfM_data &data) bool readBAL(const string& filename, SfM_data &data) {
{
// Load the data file // Load the data file
ifstream is(filename.c_str(), ifstream::in); ifstream is(filename.c_str(), ifstream::in);
if(!is) if (!is) {
{
cout << "Error in readBAL: can not find the file!!" << endl; cout << "Error in readBAL: can not find the file!!" << endl;
return false; return false;
} }
@ -690,8 +692,7 @@ bool readBAL(const string& filename, SfM_data &data)
data.tracks.resize(nrPoints); data.tracks.resize(nrPoints);
// Get the information for the observations // 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; size_t i = 0, j = 0;
float u, v; float u, v;
is >> i >> j >> u >> v; is >> i >> j >> u >> v;
@ -699,8 +700,7 @@ bool readBAL(const string& filename, SfM_data &data)
} }
// Get the information for the camera poses // 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 // Get the rodriguez vector
float wx, wy, wz; float wx, wy, wz;
is >> wx >> wy >> wz; is >> wx >> wy >> wz;
@ -721,8 +721,7 @@ bool readBAL(const string& filename, SfM_data &data)
} }
// Get the information for the 3D points // 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 // Get the 3D position
float x, y, z; float x, y, z;
is >> x >> y >> z; is >> x >> y >> z;
@ -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 // Open the output file
ofstream os; ofstream os;
os.open(filename.c_str()); os.open(filename.c_str());
@ -756,7 +754,8 @@ bool writeBAL(const string& filename, SfM_data &data)
} }
// Write observations // Write observations
os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl; os << data.number_cameras() << " " << data.number_tracks() << " "
<< nrObservations << endl;
os << 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
@ -767,13 +766,18 @@ bool writeBAL(const string& filename, SfM_data &data)
double u0 = data.cameras[i].calibration().u0(); double u0 = data.cameras[i].calibration().u0();
double v0 = data.cameras[i].calibration().v0(); 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 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); Point2 pixelMeasurement(pixelBALx, pixelBALy);
os << i /*camera id*/<< " " << j /*point id*/<< " " os << i /*camera id*/<< " " << j /*point id*/<< " "
<< pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl; << pixelMeasurement.x() /*u of the pixel*/<< " "
<< pixelMeasurement.y() /*v of the pixel*/<< endl;
} }
} }
os << endl; os << endl;
@ -804,7 +808,8 @@ bool writeBAL(const string& filename, SfM_data &data)
return true; 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; SfM_data dataValues = data;
@ -823,12 +828,16 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va
if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration 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 for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
Key cameraKey = i; // symbol('c',i); Key cameraKey = i; // symbol('c',i);
PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey); PinholeCamera<Cal3Bundler> camera =
values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
dataValues.cameras[i] = camera; dataValues.cameras[i] = camera;
} }
} else { } else {
cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras() cout
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
<< dataValues.number_cameras() << ") and values (#cameras "
<< valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
<< endl;
return false; return false;
} }
} }
@ -836,8 +845,10 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va
// Store 3D points in SfM_data // Store 3D points in SfM_data
Values valuesPoints = values.filter<Point3>(); Values valuesPoints = values.filter<Point3>();
if (valuesPoints.size() != dataValues.number_tracks()) { if (valuesPoints.size() != dataValues.number_tracks()) {
cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks() cout
<<") and values (#points " << valuesPoints.size() << ")!!" << endl; << "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