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

Merged in fix/GenericValue-assignment (pull request #345) 

close Issue #412
Attempt to fix GenericValue assignment operator for windows
release/4.3a0
Chris Beall 2018-12-29 20:28:12 +00:00 committed by Frank Dellaert
parent 94cf67b398 007fd9c3be
commit 8c654de0a7
5 changed files with 171 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/GenericValue.h
View File

@ -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<T>& operator=(const GenericValue<T>& rhs) {
// Nothing to do, do not call base class assignment operator
Value::operator=(static_cast<Value const&>(rhs));
value_ = rhs.value_;
return *this;
}

11
gtsam/nonlinear/Values.h
View File

@ -385,6 +385,17 @@ namespace gtsam {
ConstFiltered<ValueType>
filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
// Count values of given type \c ValueType
template<class ValueType>
size_t count() const {
size_t i = 0;
for (const auto& key_value : *this) {
if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
++i;
}
return i;
}
private:
// Filters based on ValueType (if not Value) and also based on the user-
// supplied \c filter function.

2
gtsam/nonlinear/tests/testValues.cpp
View File

@ -383,6 +383,8 @@ TEST(Values, filter) {
++ i;
}
EXPECT_LONGS_EQUAL(2, (long)i);
EXPECT_LONGS_EQUAL(2, (long)values.count<Pose3>());
EXPECT_LONGS_EQUAL(2, (long)values.count<Pose2>());
// construct a values with the view
Values actualSubValues2(pose_filtered);

55
gtsam/slam/dataset.cpp
View File

@ -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<Pose2> viewPose2 = estimate.filter<Pose2>();
for(const Values::ConstFiltered<Pose2>::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<const GenericValue<Pose2>*>(&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<Pose3> viewPose3 = estimate.filter<Pose3>();
for(const Values::ConstFiltered<Pose3>::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<const GenericValue<Pose3>*>(&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<NonlinearFactor> factor_: graph) {
for(const auto& factor_: graph) {
boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(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<Cal3Bundler>;
SfM_data dataValues = data;
// Store poses or cameras in SfM_data
Values valuesPoses = values.filter<Pose3>();
if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses
size_t nrPoses = values.count<Pose3>();
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<Pose3>(poseKey);
Cal3Bundler K = dataValues.cameras[i].calibration();
PinholeCamera<Cal3Bundler> camera(pose, K);
Camera camera(pose, K);
dataValues.cameras[i] = camera;
}
} else {
Values valuesCameras = values.filter<PinholeCamera<Cal3Bundler> >();
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<Camera>();
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<Cal3Bundler> camera =
values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
Camera camera = values.at<Camera>(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<Point3>();
if (valuesPoints.size() != dataValues.number_tracks()) {
size_t nrPoints = values.count<Point3>(), 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<Point3>(pointKey);

127
tests/testVisualISAM2.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <examples/SFMdata.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <vector>
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<Point3> points = createPoints();
vector<Pose3> 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<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
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<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
kPosePrior);
// Add a prior on landmark l0
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3>>(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<Point3>(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<Point3>(key), 0.01));
}
}
/* ************************************************************************* */
int main()
{
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */