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

Print for MATLAB

release/4.3a0
dellaert 2014-12-10 21:44:56 +01:00
parent dc84b6589e
commit bb00e375da
2 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam_unstable/geometry/Event.h
View File

@ -50,6 +50,9 @@ public:
time_(t), location_(x, y, z) { time_(t), location_(x, y, z) {
} }
double time() const { return time_;}
Point3 location() const { return location_;}
/** print with optional string */ /** print with optional string */
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
std::cout << s << "time = " << time_; std::cout << s << "time = " << time_;

12
gtsam_unstable/slam/tests/testTOAFactor.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam_unstable/nonlinear/ExpressionFactor.h> #include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam_unstable/geometry/Event.h> #include <gtsam_unstable/geometry/Event.h>
#include <boost/format.hpp>
namespace gtsam { namespace gtsam {
@ -177,7 +178,7 @@ TEST( TOAFactor, RealExperiment1 ) {
data[i++] << 7.8286, 7.8286, 7.8302, 7.8353; data[i++] << 7.8286, 7.8286, 7.8302, 7.8353;
// Create unknowns and initial estimate // Create unknowns and initial estimate
Event nullEvent; Event nullEvent(3, 403 / 2 * cm, 403 / 2 * cm, (212 - 45) * cm);
Values initialEstimate; Values initialEstimate;
vector<Expression<Event> > eventExpressions; vector<Expression<Event> > eventExpressions;
for (size_t j = 0; j < 15; j++) { for (size_t j = 0; j < 15; j++) {
@ -209,7 +210,14 @@ TEST( TOAFactor, RealExperiment1 ) {
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
if (verbose) if (verbose)
result.print("Final Result:\n"); for (size_t j = 0; j < 15; j++) {
Event event = result.at<Event>(j);
double t = event.time();
Point3 p = event.location();
cout
<< boost::format("t(%1%) = %2%;\tlocation(%1%,:) = [%3%, %4%, %5%];")
% (j + 1) % t % p.x() % p.y() % p.z() << endl;
}
} }
//***************************************************************************** //*****************************************************************************