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;
}
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
Values valuesPoses = values.filter<Pose3>();
if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
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<Pose3>(poseKey);
Cal3Bundler K = data.cameras[i].calibration();
Cal3Bundler K = dataValues.cameras[i].calibration();
PinholeCamera<Cal3Bundler> camera(pose, K);
data.cameras[i] = camera;
dataValues.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
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<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
data.cameras[i] = camera;
dataValues.cameras[i] = camera;
}
}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;
return false;
}
@ -698,26 +700,26 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
// 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()
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 < 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);
if(values.exists(pointKey)){
Point3 point = values.at<Point3>(pointKey);
data.tracks[j].p = point;
dataValues.tracks[j].p = point;
}else{
data.tracks[j].r = 1.0;
data.tracks[j].g = 0.0;
data.tracks[j].b = 0.0;
data.tracks[j].p = Point3();
dataValues.tracks[j].r = 1.0;
dataValues.tracks[j].g = 0.0;
dataValues.tracks[j].b = 0.0;
dataValues.tracks[j].p = Point3();
}
}
// Write SfM_data to file
return writeBAL(filename, data);
return writeBAL(filename, dataValues);
}
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
* @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