diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 67cc2646e..52899fe45 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -164,11 +164,11 @@ public: protected: - // implicit assignment operator for (const GenericValue& rhs) works fine here /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. GenericValue& operator=(const GenericValue& rhs) { - // Nothing to do, do not call base class assignment operator + Value::operator=(static_cast(rhs)); + value_ = rhs.value_; return *this; } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bf17f1f0d..16f8eba16 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -385,6 +385,17 @@ namespace gtsam { ConstFiltered filter(const boost::function& filterFcn = &_truePredicate) const; + // Count values of given type \c ValueType + template + size_t count() const { + size_t i = 0; + for (const auto& key_value : *this) { + if (dynamic_cast*>(&key_value.value)) + ++i; + } + return i; + } + private: // Filters based on ValueType (if not Value) and also based on the user- // supplied \c filter function. diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index bcf01eff5..b3c557b32 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -383,6 +383,8 @@ TEST(Values, filter) { ++ i; } EXPECT_LONGS_EQUAL(2, (long)i); + EXPECT_LONGS_EQUAL(2, (long)values.count()); + EXPECT_LONGS_EQUAL(2, (long)values.count()); // construct a values with the view Values actualSubValues2(pose_filtered); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index ba51864f1..6efd01feb 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -432,23 +432,27 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, fstream stream(filename.c_str(), fstream::out); // save 2D & 3D poses - Values::ConstFiltered viewPose2 = estimate.filter(); - for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose2) { - stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " - << key_value.value.y() << " " << key_value.value.theta() << endl; + for (const auto& key_value : estimate) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + const Pose2& pose = p->value(); + stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + << pose.y() << " " << pose.theta() << endl; } - Values::ConstFiltered viewPose3 = estimate.filter(); - for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose3) { - Point3 p = key_value.value.translation(); - Rot3 R = key_value.value.rotation(); - stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() + for(const auto& key_value: estimate) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + const Pose3& pose = p->value(); + Point3 t = pose.translation(); + Rot3 R = pose.rotation(); + stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " << t.y() << " " << t.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w() << endl; } // save edges (2D or 3D) - for(boost::shared_ptr factor_: graph) { + for(const auto& factor_: graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (factor){ @@ -857,48 +861,47 @@ bool writeBAL(const string& filename, SfM_data &data) { bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values) { - + using Camera = PinholeCamera; SfM_data dataValues = data; // Store poses or cameras in SfM_data - Values valuesPoses = values.filter(); - if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses + size_t nrPoses = values.count(); + if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera Key poseKey = symbol('x', i); Pose3 pose = values.at(poseKey); Cal3Bundler K = dataValues.cameras[i].calibration(); - PinholeCamera camera(pose, K); + Camera camera(pose, K); dataValues.cameras[i] = camera; } } else { - Values valuesCameras = values.filter >(); - if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration - for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera + size_t nrCameras = values.count(); + if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration + for (size_t i = 0; i < nrCameras; i++) { // for each camera Key cameraKey = i; // symbol('c',i); - PinholeCamera camera = - values.at >(cameraKey); + Camera camera = values.at(cameraKey); dataValues.cameras[i] = camera; } } else { cout - << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " + << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras " << dataValues.number_cameras() << ") and values (#cameras " - << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" + << nrPoses << ", #poses " << nrCameras << ")!!" << endl; return false; } } // Store 3D points in SfM_data - Values valuesPoints = values.filter(); - if (valuesPoints.size() != dataValues.number_tracks()) { + size_t nrPoints = values.count(), nrTracks = dataValues.number_tracks(); + if (nrPoints != nrTracks) { cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " - << dataValues.number_tracks() << ") and values (#points " - << valuesPoints.size() << ")!!" << endl; + << nrTracks << ") and values (#points " + << nrPoints << ")!!" << endl; } - for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point + for (size_t j = 0; j < nrTracks; j++) { // for each point Key pointKey = P(j); if (values.exists(pointKey)) { Point3 point = values.at(pointKey); diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp new file mode 100644 index 000000000..1ced2af23 --- /dev/null +++ b/tests/testVisualISAM2.cpp @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testVisualISAM2.cpp + * @brief Test convergence of visualSLAM example. + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(testVisualISAM2, all) +{ + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + + // Create ground truth data + vector points = createPoints(); + vector poses = createPoses(); + + // Set the parameters + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + ISAM2 isam(parameters); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + // Loop over the poses, adding the observations to iSAM incrementally + for (size_t i = 0; i < poses.size(); ++i) + { + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) + { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.emplace_shared>( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); + + // Treat first iteration as special case + if (i == 0) + { + // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw + static auto kPosePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)) + .finished()); + graph.emplace_shared>(Symbol('x', 0), poses[0], + kPosePrior); + + // Add a prior on landmark l0 + static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared>(Symbol('l', 0), points[0], + kPointPrior); + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + static Point3 kDeltaPoint(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j] + kDeltaPoint); + } + else + { + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + + // Do an extra update to converge withing these 8 iterations + isam.update(); + + // Optimize + Values currentEstimate = isam.calculateEstimate(); + + // reset for next iteration + graph.resize(0); + initialEstimate.clear(); + } + } // for loop + + auto result = isam.calculateEstimate(); + EXPECT_LONGS_EQUAL(16, result.size()); + for (size_t j = 0; j < points.size(); ++j) + { + Symbol key('l', j); + EXPECT(assert_equal(points[j], result.at(key), 0.01)); + } +} + +/* ************************************************************************* */ +int main() +{ + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */