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

writeBALfromValues input made const

release/4.3a0
Luca 2014-03-13 01:36:38 -04:00
parent 6e0bfa1f5b
commit afb5bac2f7
2 changed files with 21 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/slam/dataset.cpp
View File

@ -669,28 +669,30 @@ bool writeBAL(const string& filename, SfM_data &data)
return true; return true;
} }
bool writeBALfromValues(const string& filename, 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 // Store poses or cameras in SfM_data
Values valuesPoses = values.filter<Pose3>(); Values valuesPoses = values.filter<Pose3>();
if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera for (size_t i = 0; i < dataValues.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);
Cal3Bundler K = data.cameras[i].calibration(); Cal3Bundler K = dataValues.cameras[i].calibration();
PinholeCamera<Cal3Bundler> camera(pose, K); PinholeCamera<Cal3Bundler> camera(pose, K);
data.cameras[i] = camera; dataValues.cameras[i] = camera;
} }
} else { } else {
Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >(); Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
if ( valuesCameras.size() == data.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 < data.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);
data.cameras[i] = camera; dataValues.cameras[i] = camera;
} }
}else{ }else{
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras()
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
return false; return false;
} }
@ -698,26 +700,26 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
// 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() != data.number_tracks()){ if( valuesPoints.size() != dataValues.number_tracks()){
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks()
<<") and values (#points " << valuesPoints.size() << ")!!" << endl; <<") 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 < dataValues.number_tracks(); j++){ // for each point
Key pointKey = symbol('l',j); Key pointKey = symbol('l',j);
if(values.exists(pointKey)){ if(values.exists(pointKey)){
Point3 point = values.at<Point3>(pointKey); Point3 point = values.at<Point3>(pointKey);
data.tracks[j].p = point; dataValues.tracks[j].p = point;
}else{ }else{
data.tracks[j].r = 1.0; dataValues.tracks[j].r = 1.0;
data.tracks[j].g = 0.0; dataValues.tracks[j].g = 0.0;
data.tracks[j].b = 0.0; dataValues.tracks[j].b = 0.0;
data.tracks[j].p = Point3(); dataValues.tracks[j].p = Point3();
} }
} }
// Write SfM_data to file // Write SfM_data to file
return writeBAL(filename, data); return writeBAL(filename, dataValues);
} }
Values initialCamerasEstimate(const SfM_data& db) { Values initialCamerasEstimate(const SfM_data& db) {

2
gtsam/slam/dataset.h
View File

@ -146,7 +146,7 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 * 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, const SfM_data &data, Values& values);
/** /**
* @brief This function converts an openGL camera pose to an GTSAM camera pose * @brief This function converts an openGL camera pose to an GTSAM camera pose