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

Merge branch 'develop' of git@bitbucket.org:gtborg/gtsam.git into develop

release/4.3a0
Yong-Dian Jian 2014-06-16 00:46:36 -04:00
parent 38dc807f32 9f796565d0
commit 78fcfdc5ef
11 changed files with 44 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -34,7 +34,7 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
endif()
# See whether gtsam_unstable is available (it will be present only if we're using an SVN checkout)
# See whether gtsam_unstable is available (it will be present only if we're using a git checkout)
if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable")
set(GTSAM_UNSTABLE_AVAILABLE 1)
else()
@ -94,7 +94,7 @@ if(MSVC)
# Disable autolinking
if(NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_NO_LIB)
add_definitions(-DBOOST_ALL_DYN_LINK)
add_definitions(-DBOOST_ALL_DYN_LINK)
endif()
endif()

3
cmake/GTSAMCMakeToolsConfig.cmake
View File

@ -1,3 +1,4 @@
# This config file modifies CMAKE_MODULE_PATH so that the GTSAM-CMakeTools files may be included
set(GTSAM_CMAKE_TOOLS_DIR "${CMAKE_CURRENT_LIST_DIR}")
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}")

2
examples/Pose2SLAMExample_g2o.cpp
View File

@ -31,7 +31,7 @@ int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = "../../examples/Data/noisyToyGraph.txt";
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];

3
examples/Pose2SLAMExample_graph.cpp
View File

@ -32,7 +32,8 @@ int main (int argc, char** argv) {
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model);
string graph_file = findExampleDataFile("w100.graph");
boost::tie(graph, initial) = load2D(graph_file, model);
initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses

2
examples/Pose2SLAMExample_lago.cpp
View File

@ -32,7 +32,7 @@ int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = "../../examples/Data/noisyToyGraph.txt";
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];

11
examples/RangeISAMExample_plaza2.cpp
View File

@ -40,6 +40,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
@ -59,9 +60,8 @@ namespace NM = gtsam::noiseModel;
typedef pair<double, Pose2> TimedOdometry;
list<TimedOdometry> readOdometry() {
list<TimedOdometry> odometryList;
ifstream is("../../examples/Data/Plaza2_DR.txt");
if (!is)
throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found");
string data_file = findExampleDataFile("Plaza2_DR.txt");
ifstream is(data_file.c_str());
while (is) {
double t, distance_traveled, delta_heading;
@ -78,9 +78,8 @@ list<TimedOdometry> readOdometry() {
typedef boost::tuple<double, size_t, double> RangeTriple;
vector<RangeTriple> readTriples() {
vector<RangeTriple> triples;
ifstream is("../../examples/Data/Plaza2_TD.txt");
if (!is)
throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found");
string data_file = findExampleDataFile("Plaza2_TD.txt");
ifstream is(data_file.c_str());
while (is) {
double t, sender, receiver, range;

25
examples/StereoVOExample.cpp
View File

@ -24,47 +24,48 @@
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose = Pose3();
graph.push_back(NonlinearEquality<Pose3>(1, first_pose));
Pose3 first_pose;
graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
//create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
//create and add stereo factors between second pose and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
//create Values object to contain initial estimates of camera poses and landmark locations
Values initial_estimate = Values();
Values initial_estimate;
//create and add iniital estimates
initial_estimate.insert(1, first_pose);
initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
initial_estimate.insert(3, Point3(1, 1, 5));
initial_estimate.insert(4, Point3(-1, 1, 5));
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
@ -72,4 +73,4 @@ int main(int argc, char** argv){
result.print("Final result:\n");
return 0;
}
}

8
gtsam/geometry/tests/testUnit3.cpp
View File

@ -26,7 +26,7 @@
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/thread.hpp>
//#include <boost/thread.hpp>
#include <boost/assign/std/vector.hpp>
#include <cmath>
@ -234,7 +234,7 @@ TEST(Unit3, localCoordinates_retract) {
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
boost::this_thread::sleep(boost::posix_time::milliseconds(0));
// boost::this_thread::sleep(boost::posix_time::milliseconds(0));
// Create the two Unit3s.
// NOTE: You can not create two totally random Unit3's because you cannot always compute
@ -264,10 +264,10 @@ TEST(Unit3, localCoordinates_retract_expmap) {
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
boost::this_thread::sleep(boost::posix_time::milliseconds(0));
// boost::this_thread::sleep(boost::posix_time::milliseconds(0));
// Create the two Unit3s.
// Unlike the above case, we can use any two sphers.
// Unlike the above case, we can use any two Unit3's.
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
Vector v12 = randomVector(minXiLimit, maxXiLimit);

10
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -288,7 +288,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
// result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3),1e-8));
if(isDebugTest) tictoc_print_();
}
@ -365,7 +365,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
// result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-7));
if(isDebugTest) tictoc_print_();
}
@ -428,7 +428,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-8));
}
/* *************************************************************************/
@ -631,7 +631,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-8));
}
/* *************************************************************************/
@ -697,7 +697,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){
Values result = optimizer.optimize();
if(isDebugTest) result.at<Pose3>(x3).print("Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-7));
}
/* *************************************************************************/

10
gtsam_unstable/CMakeLists.txt
View File

@ -28,7 +28,7 @@ set (excluded_headers # "")
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
)
# assemble core libaries
# assemble core libraries
foreach(subdir ${gtsam_unstable_subdirs})
# Build convenience libraries
file(GLOB subdir_srcs "${subdir}/*.cpp")
@ -63,7 +63,7 @@ set(gtsam_unstable_soversion ${GTSAM_VERSION_MAJOR})
message(STATUS "GTSAM_UNSTABLE Version: ${gtsam_unstable_version}")
message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
# build shared and static versions of the library
# build shared or static versions of the library
if (GTSAM_BUILD_STATIC_LIBRARY)
message(STATUS "Building GTSAM_UNSTABLE - static")
add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs})
@ -116,8 +116,6 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}")
endif(GTSAM_INSTALL_MATLAB_TOOLBOX)
# Build examples
if (GTSAM_BUILD_EXAMPLES)
add_subdirectory(examples)
endif(GTSAM_BUILD_EXAMPLES)
add_subdirectory(examples)

6
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -108,9 +108,13 @@ int main(int argc, char** argv){
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;