[Fix] Rot3.ypr -> Rot3.Ypr
[REFACTOR] Fixed including with "" instead of <>release/4.3a0
parent
5fab190194
commit
482144821b
|
@ -18,8 +18,6 @@
|
||||||
//
|
//
|
||||||
///////////////////////////////////////////////////////////////////////////////
|
///////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#ifndef TESTRESULT_H
|
#ifndef TESTRESULT_H
|
||||||
#define TESTRESULT_H
|
#define TESTRESULT_H
|
||||||
|
|
||||||
|
|
|
@ -22,7 +22,7 @@
|
||||||
#include <gtsam_unstable/linear/LPSolver.h>
|
#include <gtsam_unstable/linear/LPSolver.h>
|
||||||
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
|
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
|
||||||
#include <boost/range/adaptor/map.hpp>
|
#include <boost/range/adaptor/map.hpp>
|
||||||
#include "LPInitSolverMatlab.h"
|
#include <gtsam_unstable/linear/LPInitSolverMatlab.h>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
|
|
|
@ -125,15 +125,15 @@ TEST(testlcnlpSolver, poseOnALine) {
|
||||||
|
|
||||||
//Instantiate LinearConstraintNLP
|
//Instantiate LinearConstraintNLP
|
||||||
LinearConstraintNLP lcnlp;
|
LinearConstraintNLP lcnlp;
|
||||||
lcnlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6)));
|
lcnlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6)));
|
||||||
LineConstraintX constraint(X(1), dualKey);
|
LineConstraintX constraint(X(1), dualKey);
|
||||||
lcnlp.linearEqualities.add(constraint);
|
lcnlp.linearEqualities.add(constraint);
|
||||||
|
|
||||||
Values initialValues;
|
Values initialValues;
|
||||||
initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(1,0,0)));
|
initialValues.insert(X(1), Pose3(Rot3::Ypr(0.3, 0.2, 0.3), Point3(1,0,0)));
|
||||||
|
|
||||||
Values expectedSolution;
|
Values expectedSolution;
|
||||||
expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3()));
|
expectedSolution.insert(X(1), Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3()));
|
||||||
|
|
||||||
// Instantiate LinearConstraintSQP
|
// Instantiate LinearConstraintSQP
|
||||||
LinearConstraintSQP lcnlpSolver(lcnlp);
|
LinearConstraintSQP lcnlpSolver(lcnlp);
|
||||||
|
|
|
@ -146,7 +146,7 @@ TEST(PositionUpperBoundX, evaluateError ) {
|
||||||
// Instantiate a class PositionUpperBoundX
|
// Instantiate a class PositionUpperBoundX
|
||||||
PositionUpperBoundX ineq(X(1), 45.6);
|
PositionUpperBoundX ineq(X(1), 45.6);
|
||||||
|
|
||||||
Pose3 pose(Rot3::ypr(0.1, 0.3, 0.2), Point3(43.0, 27.8, 91.1));
|
Pose3 pose(Rot3::Ypr(0.1, 0.3, 0.2), Point3(43.0, 27.8, 91.1));
|
||||||
Matrix H;
|
Matrix H;
|
||||||
Vector error = ineq.evaluateError(pose, H);
|
Vector error = ineq.evaluateError(pose, H);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue