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

- Fix threshold in Rot3 Logmap for Linux 32bit. Should be 1e-5 instead of 1e-10 

- The fix makes PriorFactor and PosePrior in vSLAM work.
- Now vSLAMexample can use PosePrior. It doesn't need hard constraints. Also, the gaussNewton can converge.
release/4.3a0
Duy-Nguyen Ta 2010-10-21 17:29:29 +00:00
parent 200ac4e862
commit fbcbea5f61
2 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/vSLAMexample/vSLAMexample.cpp
View File

@ -121,7 +121,7 @@ int main(int argc, char* argv[])
g_dataFolder = string(argv[1]);
readAllData();
// Create a graph using the 2D measurements (features) and calibration data
// Create a graph using the 2D measurements (features) and the calibration data
shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates
@ -129,17 +129,15 @@ int main(int argc, char* argv[])
cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: ");
// Add hard constraint on the first pose, used as fixed prior.
graph->addPoseConstraint(g_poses.begin()->first, g_poses.begin()->second);
// Add prior factor for all poses in the graph
// for (map<int, Pose3>::iterator poseit = g_poses.begin(); poseit != g_poses.end(); poseit++)
// graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(10));
map<int, Pose3>::iterator poseit = g_poses.begin();
for (; poseit != g_poses.end(); poseit++)
graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
// Optimize the graph
cout << "*******************************************************" << endl;
Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED;
Optimizer::shared_values result = Optimizer::optimizeLM( graph, initialEstimates, verborsity );
Optimizer::shared_values result = Optimizer::optimizeGN( graph, initialEstimates, verborsity );
// Print final results
cout << "*******************************************************" << endl;

8
geometry/Rot3.cpp
View File

@ -155,10 +155,10 @@ namespace gtsam {
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3::Logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (fabs(tr-3.0) < 1e-10) { // when theta = 0, +-2pi, +-4pi, etc.
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (fabs(tr-3.0) < 1e-5) { // when theta = 0, +-2pi, +-4pi, etc.
return zero(3);
} else if (tr==-1.0) { // when theta = +-pi, +-3pi, +-5pi, etc.
} else if (fabs(tr - -1.0) < 1e-5) { // when theta = +-pi, +-3pi, +-5pi, etc.
if(R.r3().z() != -1.0)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
@ -168,7 +168,7 @@ namespace gtsam {
else // if(R.r1().x() != -1.0) TODO: fix this?
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
} else {
} else {
double theta = acos((tr-1.0)/2.0);
return (theta/2.0/sin(theta))*Vector_(3,
R.r2().z()-R.r3().y(),