i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged multicamera branch into devel branch

Browse Source
This commit is contained in:
Mathieu Labbe
2015-06-16 17:41:51 -04:00
parent 6a7a9fb9b0 039ae45d57
commit 50eeda5b95
61 changed files with 3383 additions and 2918 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
corelib/src/Graph.cpp
View File

@@ -263,20 +263,23 @@ std::map<int, Transform> TOROOptimizer::optimize(
AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
AISNavigation::TreePoseGraph2::InformationMatrix inf;
//Identity:
inf.values[0][0] = 1.0f; inf.values[0][1] = 0.0f; inf.values[0][2] = 0.0f; // x
inf.values[1][0] = 0.0f; inf.values[1][1] = 1.0f; inf.values[1][2] = 0.0f; // y
inf.values[2][0] = 0.0f; inf.values[2][1] = 0.0f; inf.values[2][2] = 1.0f; // theta
if(!isCovarianceIgnored())
if(isCovarianceIgnored())
{
if(iter->second.transVariance()>0)
{
inf.values[0][0] = 1.0f/iter->second.transVariance(); // x
inf.values[1][1] = 1.0f/iter->second.transVariance(); // y
}
if(iter->second.rotVariance()>0)
{
inf.values[2][2] = 1.0f/iter->second.rotVariance(); // theta
}
inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x
inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y
inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw
}
else
{
inf.values[0][0] = iter->second.infMatrix().at<double>(0,0); // x-x
inf.values[0][1] = iter->second.infMatrix().at<double>(0,1); // x-y
inf.values[0][2] = iter->second.infMatrix().at<double>(0,5); // x-theta
inf.values[1][0] = iter->second.infMatrix().at<double>(1,0); // y-x
inf.values[1][1] = iter->second.infMatrix().at<double>(1,1); // y-y
inf.values[1][2] = iter->second.infMatrix().at<double>(1,5); // y-theta
inf.values[2][0] = iter->second.infMatrix().at<double>(5,0); // theta-x
inf.values[2][1] = iter->second.infMatrix().at<double>(5,1); // theta-y
inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
int id1 = iter->first;
@@ -304,18 +307,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
if(!isCovarianceIgnored())
{
if(iter->second.rotVariance()>0)
{
inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
}
if(iter->second.transVariance()>0)
{
inf[3][3] = 1.0f/iter->second.transVariance(); // x
inf[4][4] = 1.0f/iter->second.transVariance(); // y
inf[5][5] = 1.0f/iter->second.transVariance(); // z
}
memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
int id1 = iter->first;
@@ -491,7 +483,7 @@ bool TOROOptimizer::saveGraph(
{
float x,y,z, yaw,pitch,roll;
pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll, pitch, yaw);
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
iter->first,
iter->second.to(),
x,
@@ -500,12 +492,27 @@ bool TOROOptimizer::saveGraph(
roll,
pitch,
yaw,
iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f);
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(0,1),
iter->second.infMatrix().at<double>(0,2),
iter->second.infMatrix().at<double>(0,3),
iter->second.infMatrix().at<double>(0,4),
iter->second.infMatrix().at<double>(0,5),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(1,2),
iter->second.infMatrix().at<double>(1,3),
iter->second.infMatrix().at<double>(1,4),
iter->second.infMatrix().at<double>(1,5),
iter->second.infMatrix().at<double>(2,2),
iter->second.infMatrix().at<double>(2,3),
iter->second.infMatrix().at<double>(2,4),
iter->second.infMatrix().at<double>(2,5),
iter->second.infMatrix().at<double>(3,3),
iter->second.infMatrix().at<double>(3,4),
iter->second.infMatrix().at<double>(3,5),
iter->second.infMatrix().at<double>(4,4),
iter->second.infMatrix().at<double>(4,5),
iter->second.infMatrix().at<double>(5,5));
}
UINFO("Graph saved to %s", fileName.c_str());
fclose(file);
@@ -689,15 +696,15 @@ std::map<int, Transform> G2OOptimizer::optimize(
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
if(iter->second.transVariance()>0)
{
information(0,0) = 1.0f/iter->second.transVariance(); // x
information(1,1) = 1.0f/iter->second.transVariance(); // y
}
if(iter->second.rotVariance()>0)
{
information(2,2) = 1.0f/iter->second.rotVariance(); // theta
}
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
@@ -716,18 +723,7 @@ std::map<int, Transform> G2OOptimizer::optimize(
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
if(iter->second.transVariance()>0)
{
information(0,0) = 1.0f/iter->second.transVariance(); // x
information(1,1) = 1.0f/iter->second.transVariance(); // y
information(2,2) = 1.0f/iter->second.transVariance(); // z
}
if(iter->second.rotVariance()>0)
{
information(3,3) = 1.0f/iter->second.rotVariance(); // roll
information(4,4) = 1.0f/iter->second.rotVariance(); // pitch
information(5,5) = 1.0f/iter->second.rotVariance(); // yaw
}
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
Eigen::Affine3d a = iter->second.transform().toEigen3d();