From fbcbea5f61331966e2d41c896de89065d0a6cb8d Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Thu, 21 Oct 2010 17:29:29 +0000 Subject: [PATCH] - 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. --- examples/vSLAMexample/vSLAMexample.cpp | 12 +++++------- geometry/Rot3.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/examples/vSLAMexample/vSLAMexample.cpp b/examples/vSLAMexample/vSLAMexample.cpp index 4a3e29b4a..c5842889e 100644 --- a/examples/vSLAMexample/vSLAMexample.cpp +++ b/examples/vSLAMexample/vSLAMexample.cpp @@ -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(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::iterator poseit = g_poses.begin(); poseit != g_poses.end(); poseit++) -// graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(10)); + map::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; diff --git a/geometry/Rot3.cpp b/geometry/Rot3.cpp index b34fce069..3fbc14463 100644 --- a/geometry/Rot3.cpp +++ b/geometry/Rot3.cpp @@ -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() / 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() / 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(),