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

writeBALfromValues can now write either cameras or poses

release/4.3a0
Luca 2014-01-24 17:36:41 -05:00
parent d954cab884
commit 8755e08dad
2 changed files with 31 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam/slam/dataset.cpp
View File

@ -671,27 +671,9 @@ bool writeBAL(const string& filename, SfM_data &data)
bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
// CHECKS // Store poses or cameras in SfM_data
Values valuesPoses = values.filter<Pose3>(); Values valuesPoses = values.filter<Pose3>();
if( valuesPoses.size() != data.number_cameras()){ if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
<<") and values (#cameras " << valuesPoses.size() << ")!!" << endl;
return false;
}
Values valuesPoints = values.filter<Point3>();
if( valuesPoints.size() != data.number_tracks()){
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
}
if(valuesPoints.size() + valuesPoses.size() != values.size()){
cout << "writeBALfromValues write only poses and points values!!" << endl;
return false;
}
if(valuesPoints.size()==0 || valuesPoses.size()==0){
cout << "writeBALfromValues: No point or pose in values!!" << endl;
return false;
}
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
Key poseKey = symbol('x',i); Key poseKey = symbol('x',i);
Pose3 pose = values.at<Pose3>(poseKey); Pose3 pose = values.at<Pose3>(poseKey);
@ -699,6 +681,27 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
PinholeCamera<Cal3Bundler> camera(pose, K); PinholeCamera<Cal3Bundler> camera(pose, K);
data.cameras[i] = camera; data.cameras[i] = camera;
} }
} else {
Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
if ( valuesCameras.size() == data.number_cameras() ){ // we only estimated camera poses and calibration
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
Key cameraKey = symbol('c',i);
PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
data.cameras[i] = camera;
}
}else{
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
return false;
}
}
// Store 3D points in SfM_data
Values valuesPoints = values.filter<Point3>();
if( valuesPoints.size() != data.number_tracks()){
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
}
for (size_t j = 0; j < data.number_tracks(); j++){ // for each point for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
Key pointKey = symbol('l',j); Key pointKey = symbol('l',j);
@ -713,8 +716,8 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
} }
} }
// Write SfM_data to file
return writeBAL(filename, data); return writeBAL(filename, data);
} }
} // \namespace gtsam } // \namespace gtsam

4
gtsam/slam/dataset.h
View File

@ -141,7 +141,9 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
* while camera poses and values are read from Values) * while camera poses and values are read from Values)
* @param filename The name of the BAL file to write * @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored * @param data SfM structure where the data is stored
* @param values structure where the graph values are stored * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the
* cameras, and should be Point3 for the 3D points). Note that the current version
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
* @return true if the parsing was successful, false otherwise * @return true if the parsing was successful, false otherwise
*/ */
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values); GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values);