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

release/4.3a0
Pablo Fernandez Alcantarilla 2013-10-21 22:00:12 +00:00
parent d57f61e5ab
commit b8ce816de2
1 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam_unstable/examples/KITTItoBALConverter.cpp
View File

@ -83,10 +83,20 @@ Values::shared_ptr loadPoseValues(const string& filename) {
fin >> pose_matrix[i]; fin >> pose_matrix[i];
} }
if (addNoise) { Pose3 pose_(Matrix_(4, 4, pose_matrix));
Rot3 r_ = pose_.rotation();
Point3 t_ = pose_.translation();
/*if (addNoise) {
values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)).compose(noise_pose)); values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)).compose(noise_pose));
} else { } else {
values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix))); values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)));
}*/
if (addNoise) {
values->insert(Symbol('x',pose_id), Pose3(r_,t_).compose(noise_pose));
} else {
values->insert(Symbol('x',pose_id), Pose3(r_,t_));
} }
} }
@ -199,8 +209,8 @@ int main(int argc, char** argv) {
unsigned int minimumNumViews = 1; unsigned int minimumNumViews = 1;
string HOME = getenv("HOME"); string HOME = getenv("HOME");
string input_dir = HOME + "/data/kitti/loop_closures_merged/"; string input_dir = HOME + "/Research/datasets/kitti/test/";
string output_file = HOME + "/data/kitti/loop_closures_merged/loop_closures_merged_bal.txt"; string output_file = HOME + "/Research/datasets/kitti/test/test_bal.txt";
typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> ProjectionFactor; typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> ProjectionFactor;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -243,10 +253,13 @@ int main(int argc, char** argv) {
if (loaded_values->exists<Point3>(L(l)) == boost::none) { if (loaded_values->exists<Point3>(L(l)) == boost::none) {
Pose3 camera = loaded_values->at<Pose3>(X(r)); Pose3 camera = loaded_values->at<Pose3>(X(r));
// Use transform to to get points in world coordinate frame
Point3 worldPoint = camera.transform_from(Point3(x, y, z)); Point3 worldPoint = camera.transform_from(Point3(x, y, z));
loaded_values->insert(L(l), worldPoint); // add point; loaded_values->insert(L(l), worldPoint); // add point;
npoints++; npoints++;
//cout << "world p " << worldPoint << endl;
// Create a track without observations // Create a track without observations
SfM_Track track; SfM_Track track;
track.p = worldPoint; track.p = worldPoint;
@ -262,13 +275,12 @@ int main(int argc, char** argv) {
fin.close(); fin.close();
// Open again the file to store the measurements // Open again the file to store the measurements
fin.open((input_dir+"sorted_contiguous_stereo_factors.txt").c_str()); fin.open((input_dir+"stereo_factors.txt").c_str());
if(!fin) { if(!fin) {
cerr << "Could not open sorted_contiguous_stereo_factors.txt" << endl; cerr << "Could not stereo_factors.txt" << endl;
exit(1); exit(1);
} }
while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z){ while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z){
SfM_Measurement observation = make_pair(r,Point2(uL,v)); SfM_Measurement observation = make_pair(r,Point2(uL,v));
kitti_sfm.tracks[l].measurements.push_back(observation); kitti_sfm.tracks[l].measurements.push_back(observation);
} }