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

Fix some tests

release/4.3a0
dellaert 2014-10-22 16:26:56 +02:00
parent 48a6777935
commit be676b22cf
2 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/geometry/PinholeCamera.h
View File

@ -426,6 +426,15 @@ public:
typedef Eigen::Matrix<double,2,6+DimK> Matrix2K6; typedef Eigen::Matrix<double,2,6+DimK> Matrix2K6;
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
*/
Point2 project2(const Point3& pw) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn);
}
/** project a point from world coordinate to the image, fixed Jacobians /** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate * @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
@ -433,8 +442,8 @@ public:
*/ */
Point2 project2( Point2 project2(
const Point3& pw, // const Point3& pw, //
boost::optional<Matrix2K6&> Dcamera = boost::none, boost::optional<Matrix2K6&> Dcamera,
boost::optional<Matrix23&> Dpoint = boost::none) const { boost::optional<Matrix23&> Dpoint) const {
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc); const Point2 pn = project_to_camera(pc);
@ -465,10 +474,8 @@ public:
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
*/ */
Point2 project2( Point2 project2(const Point3& pw, //
const Point3& pw, // boost::optional<Matrix&> Dcamera, boost::optional<Matrix&> Dpoint) const {
boost::optional<Matrix&> Dcamera = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc); const Point2 pn = project_to_camera(pc);

5
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -14,7 +14,6 @@
* @brief Unit tests for transform derivatives * @brief Unit tests for transform derivatives
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
@ -32,7 +31,9 @@ static Point2 p(2,3);
TEST( Cal3Bundler, vector) TEST( Cal3Bundler, vector)
{ {
Cal3Bundler K; Cal3Bundler K;
CHECK(assert_equal((Vector(3)<<1,0,0),K.vector())); Vector expected(3);
expected << 1, 0, 0;
CHECK(assert_equal(expected,K.vector()));
} }
/* ************************************************************************* */ /* ************************************************************************* */