fix Vector_() in rest of gtsam folder
parent
5e9540470a
commit
90786c0203
|
@ -24,7 +24,7 @@ const double tol = 1e-5;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST(testSampler, basic) {
|
TEST(testSampler, basic) {
|
||||||
Vector sigmas = Vector_(3, 1.0, 0.1, 0.0);
|
Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0);
|
||||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
|
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
|
||||||
char seed = 'A';
|
char seed = 'A';
|
||||||
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);
|
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);
|
||||||
|
|
|
@ -113,7 +113,7 @@ namespace imuBias {
|
||||||
//
|
//
|
||||||
// return measurement - biasGyro_ - w_earth_rate_I;
|
// return measurement - biasGyro_ - w_earth_rate_I;
|
||||||
//
|
//
|
||||||
//// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
|
//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
|
||||||
//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
|
//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
|
||||||
// }
|
// }
|
||||||
|
|
||||||
|
|
|
@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params {
|
||||||
* entries would be added with:
|
* entries would be added with:
|
||||||
* \code
|
* \code
|
||||||
FastMap<char,Vector> thresholds;
|
FastMap<char,Vector> thresholds;
|
||||||
thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
|
thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
|
||||||
thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
|
thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
|
||||||
params.relinearizeThreshold = thresholds;
|
params.relinearizeThreshold = thresholds;
|
||||||
\endcode
|
\endcode
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -19,7 +19,7 @@ using namespace std;
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0));
|
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0));
|
||||||
const double tol = 1e-5;
|
const double tol = 1e-5;
|
||||||
|
|
||||||
gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;
|
gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;
|
||||||
|
|
|
@ -96,7 +96,7 @@ namespace gtsam {
|
||||||
Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
|
Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
|
||||||
|
|
||||||
double y2 = pose.range(point, H21_, H22_);
|
double y2 = pose.range(point, H21_, H22_);
|
||||||
Vector e2 = Vector_(1, y2 - measuredRange_);
|
Vector e2 = (Vector(1) << y2 - measuredRange_);
|
||||||
|
|
||||||
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
|
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
|
||||||
if (H2) *H2 = gtsam::stack(2, &H12, &H22);
|
if (H2) *H2 = gtsam::stack(2, &H12, &H22);
|
||||||
|
|
|
@ -73,7 +73,7 @@ namespace gtsam {
|
||||||
} else {
|
} else {
|
||||||
hx = pose.range(point, H1, H2);
|
hx = pose.range(point, H1, H2);
|
||||||
}
|
}
|
||||||
return Vector_(1, hx - measured_);
|
return (Vector(1) << hx - measured_);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** return the measured */
|
/** return the measured */
|
||||||
|
|
Loading…
Reference in New Issue