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

Comments/formatting only

release/4.3a0
Frank Dellaert 2010-01-14 05:59:33 +00:00
parent 8088aea598
commit a552ba4539
2 changed files with 3 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/BearingFactor.h
View File

@ -27,9 +27,7 @@ namespace gtsam {
* Calculate relative bearing and optional derivative * Calculate relative bearing and optional derivative
*/ */
Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H) { Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
double x = d.x(), y = d.y(); double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
double d2 = x * x + y * y;
double n = sqrt(d2);
if (H) *H = Matrix_(1, 2, -y / d2, x / d2); if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
return Rot2(x / n, y / n); return Rot2(x / n, y / n);
} }

5
cpp/testBearingFactor.cpp
View File

@ -1,12 +1,11 @@
/** /**
* @file testBearingFactor.cpp * @file testBearingFactor.cpp
* @brief Unit tests for BearingFactor Class * @brief Unit tests for BearingFactor Class
* @authors Frank Dellaert, Viorela Ila * @authors Frank Dellaert
**/ **/
#include <boost/bind.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "Key.h"
#include "numericalDerivative.h" #include "numericalDerivative.h"
#include "BearingFactor.h" #include "BearingFactor.h"
#include "TupleConfig.h" #include "TupleConfig.h"