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

Fixed testIMUSystem and BetweenFactorEM

release/4.3a0
Richard Roberts 2013-08-11 18:40:47 +00:00
parent 0db8e446d5
commit f240327f24
3 changed files with 789 additions and 789 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -69,13 +69,13 @@ TEST( testIMUSystem, optimize_chain ) {
// assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph;
graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1));
graph.add(IMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
graph.add(IMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
graph.add(IMUFactor<PoseRTV>(imu34, dt, x3, x4, model));
graph.add(VelocityConstraint(x1, x2, dt));
graph.add(VelocityConstraint(x2, x3, dt));
graph.add(VelocityConstraint(x3, x4, dt));
graph += NonlinearEquality<gtsam::PoseRTV>(x1, pose1);
graph += IMUFactor<PoseRTV>(imu12, dt, x1, x2, model);
graph += IMUFactor<PoseRTV>(imu23, dt, x2, x3, model);
graph += IMUFactor<PoseRTV>(imu34, dt, x3, x4, model);
graph += VelocityConstraint(x1, x2, dt);
graph += VelocityConstraint(x2, x3, dt);
graph += VelocityConstraint(x3, x4, dt);
// ground truth values
Values true_values;
@ -116,10 +116,10 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph;
graph.add(NonlinearEquality<gtsam::PoseRTV>(x1, pose1));
graph.add(FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model));
graph.add(FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model));
graph.add(FullIMUFactor<PoseRTV>(imu34, dt, x3, x4, model));
graph += NonlinearEquality<gtsam::PoseRTV>(x1, pose1);
graph += FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model);
graph += FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model);
graph += FullIMUFactor<PoseRTV>(imu34, dt, x3, x4, model);
// ground truth values
Values true_values;
@ -158,7 +158,7 @@ TEST( testIMUSystem, linear_trajectory) {
Values true_traj, init_traj;
NonlinearFactorGraph graph;
graph.add(NonlinearEquality<gtsam::PoseRTV>(x0, start));
graph += NonlinearEquality<gtsam::PoseRTV>(x0, start);
true_traj.insert(x0, start);
init_traj.insert(x0, start);
@ -167,7 +167,7 @@ TEST( testIMUSystem, linear_trajectory) {
for (size_t i=1; i<nrPoses; ++i) {
Key xA = i-1, xB = i;
cur_pose = cur_pose.generalDynamics(accel, gyro, dt);
graph.add(FullIMUFactor<PoseRTV>(accel - g, gyro, dt, xA, xB, model));
graph += FullIMUFactor<PoseRTV>(accel - g, gyro, dt, xA, xB, model);
true_traj.insert(xB, cur_pose);
init_traj.insert(xB, PoseRTV());
}

6
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -68,7 +68,7 @@ namespace gtsam {
BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
const double prior_inlier, const double prior_outlier) :
Base(key1, key2), key1_(key1), key2_(key2), measured_(measured),
Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured),
model_inlier_(model_inlier), model_outlier_(model_outlier),
prior_inlier_(prior_inlier), prior_outlier_(prior_outlier){
}
@ -119,7 +119,7 @@ namespace gtsam {
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
/* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x, const gtsam::Ordering& ordering) const {
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>();
@ -132,7 +132,7 @@ namespace gtsam {
A2 = A[1];
return gtsam::GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(ordering[key1_], A1, ordering[key2_], A2, b, gtsam::noiseModel::Unit::Create(b.size())));
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size())));
}

4
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -104,7 +104,7 @@ TEST( BetweenFactorEM, EvaluateError)
Vector actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
// in case of inlier, inlier-mode whitented error should be dominant
assert(actual_err_wh_inlier.norm() > 1000.0*actual_err_wh_outlier.norm());
CHECK(actual_err_wh_inlier.norm() > 1000.0*actual_err_wh_outlier.norm());
cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier.norm()<<endl;
cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl;
@ -123,7 +123,7 @@ TEST( BetweenFactorEM, EvaluateError)
actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
// in case of outlier, outlier-mode whitented error should be dominant
assert(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm());
CHECK(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm());
cout << "Outlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier<<endl;
cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl;