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

Fixes due to bad merge

release/4.3a0
Alex Cunningham 2011-11-07 21:16:26 +00:00
parent 8592e6b2c6
commit d340e556b5
7 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam.h
View File

@ -40,7 +40,7 @@ class Pose2 {
int dim() const; int dim() const;
Pose2* compose_(const Pose2& p2); Pose2* compose_(const Pose2& p2);
Pose2* between_(const Pose2& p2); Pose2* between_(const Pose2& p2);
Vector logmap(const Pose2& p); Vector localCoordinates(const Pose2& p);
}; };
class SharedGaussian { class SharedGaussian {

6
gtsam/geometry/tests/testPose3.cpp
View File

@ -148,7 +148,7 @@ TEST(Pose3, expmaps_galore)
actual = Pose3::Retract(xi); actual = Pose3::Retract(xi);
EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6)); EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
EXPECT(assert_equal(xi, logmap(actual),1e-6)); EXPECT(assert_equal(xi, localCoordinates(actual),1e-6));
xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6);
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
@ -156,7 +156,7 @@ TEST(Pose3, expmaps_galore)
actual = Pose3::Retract(txi); actual = Pose3::Retract(txi);
EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6)); EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
Vector log = logmap(actual); Vector log = localCoordinates(actual);
EXPECT(assert_equal(actual, Pose3::Retract(log),1e-6)); EXPECT(assert_equal(actual, Pose3::Retract(log),1e-6));
EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
} }
@ -166,7 +166,7 @@ TEST(Pose3, expmaps_galore)
actual = Pose3::Retract(xi); actual = Pose3::Retract(xi);
EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5)); EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
EXPECT(assert_equal(xi, logmap(actual),1e-6)); EXPECT(assert_equal(xi, localCoordinates(actual),1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/nonlinear/DoglegOptimizer.h
View File

@ -15,7 +15,7 @@ namespace gtsam {
* This class is functional, meaning every method is \c const, and returns a new * This class is functional, meaning every method is \c const, and returns a new
* copy of the class. * copy of the class.
* *
* \tparam VALUES The LieValues or TupleValues type to hold the values to be * \tparam VALUES The Values or TupleValues type to hold the values to be
* estimated. * estimated.
* *
* \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration,

6
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
* here matches that in "trustregion.pdf" in gtsam_experimental/doc, see this * here matches that in "trustregion.pdf" in gtsam_experimental/doc, see this
* file for further explanation of the computations performed by this class. * file for further explanation of the computations performed by this class.
* *
* \tparam VALUES The LieValues or TupleValues type to hold the values to be * \tparam VALUES The Values or TupleValues type to hold the values to be
* estimated. * estimated.
* *
* \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration,
@ -60,7 +60,7 @@ struct DoglegOptimizerImpl {
* @tparam M The type of the Bayes' net or tree, currently * @tparam M The type of the Bayes' net or tree, currently
* either BayesNet<GaussianConditional> (or GaussianBayesNet) or BayesTree<GaussianConditional>. * either BayesNet<GaussianConditional> (or GaussianBayesNet) or BayesTree<GaussianConditional>.
* @tparam F For normal usage this will be NonlinearFactorGraph<VALUES>. * @tparam F For normal usage this will be NonlinearFactorGraph<VALUES>.
* @tparam VALUES The LieValues or TupleValues to pass to F::error() to evaluate * @tparam VALUES The Values or TupleValues to pass to F::error() to evaluate
* the error function. * the error function.
* @param initialDelta The initial trust region radius. * @param initialDelta The initial trust region radius.
* @param Rd The Bayes' net or tree as described above. * @param Rd The Bayes' net or tree as described above.
@ -159,7 +159,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(verbose) cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl; if(verbose) cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl;
// Compute expmapped solution // Compute expmapped solution
const VALUES x_d(x0.expmap(result.dx_d, ordering)); const VALUES x_d(x0.retract(result.dx_d, ordering));
// Compute decrease in f // Compute decrease in f
result.f_error = f.error(x_d); result.f_error = f.error(x_d);

4
gtsam/nonlinear/Makefile.am
View File

@ -16,8 +16,8 @@ check_PROGRAMS =
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# Lie Groups # Lie Groups
headers += LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h
check_PROGRAMS += tests/testLieValues tests/testKey tests/testOrdering check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
# Nonlinear nonlinear # Nonlinear nonlinear
headers += Key.h headers += Key.h

2
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -306,7 +306,7 @@ namespace gtsam {
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
*graph_, *values_, *ordering_, error_); *graph_, *values_, *ordering_, error_);
shared_values newValues(new T(values_->expmap(result.dx_d, *ordering_))); shared_values newValues(new T(values_->retract(result.dx_d, *ordering_)));
cout << "newValues: " << newValues.get() << endl; cout << "newValues: " << newValues.get() << endl;
return newValuesErrorLambda_(newValues, result.f_error, result.Delta); return newValuesErrorLambda_(newValues, result.f_error, result.Delta);
} }

2
tests/testDoglegOptimizer.cpp
View File

@ -405,7 +405,7 @@ TEST(DoglegOptimizer, Iterate) {
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config)); DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config));
Delta = result.Delta; Delta = result.Delta;
EXPECT(result.f_error < fg->error(*config)); // Check that error decreases EXPECT(result.f_error < fg->error(*config)); // Check that error decreases
simulated2D::Values newConfig(config->expmap(result.dx_d, *ord)); simulated2D::Values newConfig(config->retract(result.dx_d, *ord));
(*config) = newConfig; (*config) = newConfig;
DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in
} }