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

extended g2o parser to nondiagonal noise

release/4.3a0
Luca 2014-08-19 20:10:42 -04:00
parent 881514a45f
commit bc205cf6a4
5 changed files with 111 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/Data/pose3example-offdiagonal-rewritten.txt Normal file
View File

@ -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

3
examples/Data/pose3example-offdiagonal.txt Normal file
View File

@ -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

11
examples/Data/pose3example-rewritten.txt Normal file
View File

@ -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

62
gtsam/slam/dataset.cpp
View File

@ -400,18 +400,22 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (factor){
SharedNoiseModel model = factor->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw invalid_argument(
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(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<Pose3> > factor3D =
@ -419,24 +423,28 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
if (factor3D){
SharedNoiseModel model = factor3D->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw invalid_argument(
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(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<Pose3>(id1, id2, Pose3(R,t), model));
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);
}
}

59
gtsam/slam/tests/testDataset.cpp
View File

@ -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<Pose3>(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)
{