diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index dca24dba5..38fe1dd35 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -53,10 +53,8 @@ int main(int argc, char* argv[]) { // First pose with prior factor newFactors.addPosePrior(X(0), data.poses[0], data.noiseX); - // Second pose with odometry measurement, simulated by adding Gaussian noise to the ground-truth. - Pose3 odoMeasurement = data.odometry*Pose3::Expmap(data.noiseX->sample()); - newFactors.push_back( boost::shared_ptr >( - new BetweenFactor(X(0), X(1), odoMeasurement, data.noiseX))); + // Second pose with odometry measurement + newFactors.addOdometry(X(0), X(1), data.odometry, data.noiseX); // Visual measurements at both poses for (size_t i=0; i<2; ++i) { @@ -67,13 +65,12 @@ int main(int argc, char* argv[]) { // Initial values for the first two poses, simulated with Gaussian noise Values initials; - Pose3 pose0Init = data.poses[0]*Pose3::Expmap(data.noiseX->sample()); - initials.insert(X(0), pose0Init); - initials.insert(X(1), pose0Init*odoMeasurement); + initials.insert(X(0), data.poses[0]); + initials.insert(X(1), data.poses[0]*data.odometry); - // Initial values for the landmarks, simulated with Gaussian noise + // Initial values for the landmarks for (size_t j=0; jsample())); + initials.insert(L(j), data.points[j]); // Update ISAM the first time and obtain the current estimate isam.update(newFactors, initials); @@ -87,9 +84,8 @@ int main(int argc, char* argv[]) { for (size_t i=2; isample()); - newFactors.push_back( boost::shared_ptr >( - new BetweenFactor(X(i-1), X(i), odoMeasurement, data.noiseX))); + Pose3 odoMeasurement = data.odometry; + newFactors.addOdometry(X(i-1), X(i), data.odometry, data.noiseX); // Factors for visual measurements for (size_t j=0; j(X(i-1))*odoMeasurement); + initials.insert(X(i), currentEstimate.at(X(i-1))*data.odometry); // update ISAM isam.update(newFactors, initials); diff --git a/examples/VisualSLAMData.h b/examples/VisualSLAMData.h index 155a137d8..88fb576fe 100644 --- a/examples/VisualSLAMData.h +++ b/examples/VisualSLAMData.h @@ -66,20 +66,19 @@ struct VisualSLAMExampleData { double theta = 0.0; double r = 30.0; for (int i=0; isample())); + data.z[i].push_back(camera.project(data.points[j]) + /*+ gtsam::Point2(data.noiseZ->sample()))*/); // you can add noise as desired } } data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1)); diff --git a/examples/VisualSLAMExample.cpp b/examples/VisualSLAMExample.cpp index 90391bf90..1fc343203 100644 --- a/examples/VisualSLAMExample.cpp +++ b/examples/VisualSLAMExample.cpp @@ -49,9 +49,9 @@ int main(int argc, char* argv[]) { /* 3. Initial estimates for variable nodes, simulated by Gaussian noises */ Values initial; for (size_t i=0; isample())); + initial.insert(X(i), data.poses[i]/* *Pose3::Expmap(data.noiseX->sample())*/); // you can add noise if you want for (size_t j=0; jsample())); + initial.insert(L(j), data.points[j] /*+ Point3(data.noiseL->sample())*/); // you can add noise if you want initial.print("Intial Estimates: "); /* 4. Optimize the graph and print results */