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

remove noise sampler in visualSLAM examples

release/4.3a0
Duy-Nguyen Ta 2012-06-06 09:36:10 +00:00
parent 8037c44b17
commit a8ffa407ae
3 changed files with 17 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
examples/VisualISAMExample.cpp
View File

@ -53,10 +53,8 @@ int main(int argc, char* argv[]) {
// First pose with prior factor // First pose with prior factor
newFactors.addPosePrior(X(0), data.poses[0], data.noiseX); newFactors.addPosePrior(X(0), data.poses[0], data.noiseX);
// Second pose with odometry measurement, simulated by adding Gaussian noise to the ground-truth. // Second pose with odometry measurement
Pose3 odoMeasurement = data.odometry*Pose3::Expmap(data.noiseX->sample()); newFactors.addOdometry(X(0), X(1), data.odometry, data.noiseX);
newFactors.push_back( boost::shared_ptr<BetweenFactor<Pose3> >(
new BetweenFactor<Pose3>(X(0), X(1), odoMeasurement, data.noiseX)));
// Visual measurements at both poses // Visual measurements at both poses
for (size_t i=0; i<2; ++i) { 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 // Initial values for the first two poses, simulated with Gaussian noise
Values initials; Values initials;
Pose3 pose0Init = data.poses[0]*Pose3::Expmap(data.noiseX->sample()); initials.insert(X(0), data.poses[0]);
initials.insert(X(0), pose0Init); initials.insert(X(1), data.poses[0]*data.odometry);
initials.insert(X(1), pose0Init*odoMeasurement);
// Initial values for the landmarks, simulated with Gaussian noise // Initial values for the landmarks
for (size_t j=0; j<data.points.size(); ++j) for (size_t j=0; j<data.points.size(); ++j)
initials.insert(L(j), data.points[j] + Point3(data.noiseL->sample())); initials.insert(L(j), data.points[j]);
// Update ISAM the first time and obtain the current estimate // Update ISAM the first time and obtain the current estimate
isam.update(newFactors, initials); isam.update(newFactors, initials);
@ -87,9 +84,8 @@ int main(int argc, char* argv[]) {
for (size_t i=2; i<data.poses.size(); ++i) { for (size_t i=2; i<data.poses.size(); ++i) {
visualSLAM::Graph newFactors; visualSLAM::Graph newFactors;
// Factor for odometry measurements, simulated by adding Gaussian noise to the ground-truth. // Factor for odometry measurements, simulated by adding Gaussian noise to the ground-truth.
Pose3 odoMeasurement = data.odometry*Pose3::Expmap(data.noiseX->sample()); Pose3 odoMeasurement = data.odometry;
newFactors.push_back( boost::shared_ptr<BetweenFactor<Pose3> >( newFactors.addOdometry(X(i-1), X(i), data.odometry, data.noiseX);
new BetweenFactor<Pose3>(X(i-1), X(i), odoMeasurement, data.noiseX)));
// Factors for visual measurements // Factors for visual measurements
for (size_t j=0; j<data.z[i].size(); ++j) { for (size_t j=0; j<data.z[i].size(); ++j) {
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK); newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
@ -97,7 +93,7 @@ int main(int argc, char* argv[]) {
// Initial estimates for the new node Xi, simulated by Gaussian noises // Initial estimates for the new node Xi, simulated by Gaussian noises
Values initials; Values initials;
initials.insert(X(i), currentEstimate.at<Pose3>(X(i-1))*odoMeasurement); initials.insert(X(i), currentEstimate.at<Pose3>(X(i-1))*data.odometry);
// update ISAM // update ISAM
isam.update(newFactors, initials); isam.update(newFactors, initials);

13
examples/VisualSLAMData.h
View File

@ -66,20 +66,19 @@ struct VisualSLAMExampleData {
double theta = 0.0; double theta = 0.0;
double r = 30.0; double r = 30.0;
for (int i=0; i<n; ++i, theta += 2*M_PI/n) { for (int i=0; i<n; ++i, theta += 2*M_PI/n) {
data.poses.push_back(gtsam::Pose3( Point3 C = gtsam::Point3(r*cos(theta), r*sin(theta), 0.0);
gtsam::Rot3(-sin(theta), 0.0, -cos(theta), SimpleCamera camera = SimpleCamera::lookat(C, Point3(), Point3(0,0,1));
cos(theta), 0.0, -sin(theta), data.poses.push_back(camera.pose());
0.0, -1.0, 0.0),
gtsam::Point3(r*cos(theta), r*sin(theta), 0.0)));
} }
data.odometry = data.poses[0].between(data.poses[1]); data.odometry = data.poses[0].between(data.poses[1]);
// Simulated measurements with Gaussian noise // Simulated measurements, possibly with Gaussian noise
data.noiseZ = gtsam::sharedSigma(2, 1.0); data.noiseZ = gtsam::sharedSigma(2, 1.0);
for (size_t i=0; i<data.poses.size(); ++i) { for (size_t i=0; i<data.poses.size(); ++i) {
for (size_t j=0; j<data.points.size(); ++j) { for (size_t j=0; j<data.points.size(); ++j) {
gtsam::SimpleCamera camera(data.poses[i], *data.sK); gtsam::SimpleCamera camera(data.poses[i], *data.sK);
data.z[i].push_back(camera.project(data.points[j]) + gtsam::Point2(data.noiseZ->sample())); 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)); data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1));

4
examples/VisualSLAMExample.cpp
View File

@ -49,9 +49,9 @@ int main(int argc, char* argv[]) {
/* 3. Initial estimates for variable nodes, simulated by Gaussian noises */ /* 3. Initial estimates for variable nodes, simulated by Gaussian noises */
Values initial; Values initial;
for (size_t i=0; i<data.poses.size(); ++i) for (size_t i=0; i<data.poses.size(); ++i)
initial.insert(X(i), data.poses[i]*Pose3::Expmap(data.noiseX->sample())); initial.insert(X(i), data.poses[i]/* *Pose3::Expmap(data.noiseX->sample())*/); // you can add noise if you want
for (size_t j=0; j<data.points.size(); ++j) for (size_t j=0; j<data.points.size(); ++j)
initial.insert(L(j), data.points[j] + Point3(data.noiseL->sample())); initial.insert(L(j), data.points[j] /*+ Point3(data.noiseL->sample())*/); // you can add noise if you want
initial.print("Intial Estimates: "); initial.print("Intial Estimates: ");
/* 4. Optimize the graph and print results */ /* 4. Optimize the graph and print results */