diff --git a/examples/Data/pose3example-offdiagonal-rewritten.txt b/examples/Data/pose3example-offdiagonal-rewritten.txt new file mode 100644 index 000000000..b99d266aa --- /dev/null +++ b/examples/Data/pose3example-offdiagonal-rewritten.txt @@ -0,0 +1,3 @@ +VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 +VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 +EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000 diff --git a/examples/Data/pose3example-offdiagonal.txt b/examples/Data/pose3example-offdiagonal.txt new file mode 100644 index 000000000..c3eb883a4 --- /dev/null +++ b/examples/Data/pose3example-offdiagonal.txt @@ -0,0 +1,3 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 +VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 +EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000 10000.000000 2.000000 2.000000 2.000000 2.000000 10000.000000 3.000000 3.000000 3.000000 10000.000000 4.000000 4.000000 10000.000000 5.000000 10000.000000 \ No newline at end of file diff --git a/examples/Data/pose3example-rewritten.txt b/examples/Data/pose3example-rewritten.txt new file mode 100644 index 000000000..6d342fcb0 --- /dev/null +++ b/examples/Data/pose3example-rewritten.txt @@ -0,0 +1,11 @@ +VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 +VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 +VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446 +VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024 +VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488 +EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 +EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 +EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 +EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592076 0.30338 -0.513225 0.542222 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 +EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 +EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index ff20ce5da..7d06ad0cf 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -400,18 +400,22 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, boost::dynamic_pointer_cast >(factor_); if (factor){ SharedNoiseModel model = factor->get_noiseModel(); - boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(model); - if (!diagonalModel) - throw invalid_argument( - "writeG2o: invalid noise model (current version assumes diagonal noise model)!"); - + boost::shared_ptr gaussianModel = + boost::dynamic_pointer_cast(model); + if (!gaussianModel){ + model->print("model\n"); + throw invalid_argument("writeG2o: invalid noise model!"); + } + Matrix Info = gaussianModel->R().transpose() * gaussianModel->R(); Pose2 pose = factor->measured(); //.inverse(); stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " - << pose.x() << " " << pose.y() << " " << pose.theta() << " " - << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " - << diagonalModel->precision(1) << " " << 0.0 << " " - << diagonalModel->precision(2) << endl; + << pose.x() << " " << pose.y() << " " << pose.theta(); + for (int i = 0; i < 3; i++){ + for (int j = i; j < 3; j++){ + stream << " " << Info(i, j); + } + } + stream << endl; } boost::shared_ptr< BetweenFactor > factor3D = @@ -419,24 +423,28 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, if (factor3D){ SharedNoiseModel model = factor3D->get_noiseModel(); - boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(model); - if (!diagonalModel) - throw invalid_argument( - "writeG2o: invalid noise model (current version assumes diagonal noise model)!"); + boost::shared_ptr gaussianModel = + boost::dynamic_pointer_cast(model); + if (!gaussianModel){ + model->print("model\n"); + throw invalid_argument("writeG2o: invalid noise model!"); + } + Matrix Info = gaussianModel->R().transpose() * gaussianModel->R(); Pose3 pose3D = factor3D->measured(); Point3 p = pose3D.translation(); Rot3 R = pose3D.rotation(); + stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " " << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() - << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w() << " " - << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " - << diagonalModel->precision(1) << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " - << diagonalModel->precision(2) << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " - << diagonalModel->precision(3) << " " << 0.0 << " " << 0.0 << " " - << diagonalModel->precision(4) << " " << 0.0 << " " - << diagonalModel->precision(5) << endl; + << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w(); + + for (int i = 0; i < 6; i++){ + for (int j = i; j < 6; j++){ + stream << " " << Info(i, j); + } + } + stream << endl; } } stream.close(); @@ -510,14 +518,14 @@ GraphAndValues load3D(const string& filename) { Point3 t = Point3(x, y, z); for (int i = 0; i < 6; i++){ for (int j = i; j < 6; j++){ - ls >> m(i, j); - if( j > i && m(i, j)!= 0) - throw invalid_argument("load3D: current version assumes diagonal noise!"); + double mij; + ls >> mij; + m(i, j) = mij; + m(j, i) = mij; } } SharedNoiseModel model = noiseModel::Gaussian::Information(m); - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, Pose3(R,t), model)); + NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); graph->push_back(factor); } } diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index b581796dc..a743200c9 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -178,6 +178,46 @@ TEST( dataSet, readG2o3D) EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } +/* ************************************************************************* */ +TEST( dataSet, readG2o3DNonDiagonalNoise) +{ + const string g2oFile = findExampleDataFile("pose3example-offdiagonal.txt"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + bool is3D = true; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); + + Values expectedValues; + Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Point3 p0 = Point3(0.000000, 0.000000, 0.000000); + expectedValues.insert(0, Pose3(R0, p0)); + + Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Point3 p1 = Point3(1.001367, 0.015390, 0.004948); + expectedValues.insert(1, Pose3(R1, p1)); + + EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); + + Matrix Info = Matrix(6,6); + for (int i = 0; i < 6; i++){ + for (int j = i; j < 6; j++){ + if(i==j) + Info(i, j) = 10000; + else{ + Info(i, j) = i+1; // arbitrary nonzero number + Info(j, i) = i+1; + } + } + } + noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse()); + NonlinearFactorGraph expectedGraph; + Point3 p01 = Point3(1.001367, 0.015390, 0.004948); + Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); +} + /* ************************************************************************* */ TEST( dataSet, readG2oHuber) { @@ -271,6 +311,25 @@ TEST( dataSet, writeG2o3D) EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4)); } +/* ************************************************************************* */ +TEST( dataSet, writeG2o3DNonDiagonalNoise) +{ + const string g2oFile = findExampleDataFile("pose3example-offdiagonal"); + NonlinearFactorGraph::shared_ptr expectedGraph; + Values::shared_ptr expectedValues; + bool is3D = true; + boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D); + + const string filenameToWrite = createRewrittenFileName(g2oFile); + writeG2o(*expectedGraph, *expectedValues, filenameToWrite); + + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D); + EXPECT(assert_equal(*expectedValues,*actualValues,1e-4)); + EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4)); +} + /* ************************************************************************* */ TEST( dataSet, readBAL_Dubrovnik) {