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

additional formatting

release/4.3a0
Varun Agrawal 2020-12-02 17:24:21 -05:00
parent 70bab8e00c
commit 3ddc999f43
1 changed files with 11 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Cal3Unified.cpp
View File

@ -16,10 +16,10 @@
* @author Varun Agrawal * @author Varun Agrawal
*/ */
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3Unified.h> #include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Point2.h>
#include <cmath> #include <cmath>
@ -53,10 +53,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
// todo: make a fixed sized jacobian version of this // todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p, Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal,
OptionalJacobian<2,10> Dcal,
OptionalJacobian<2, 2> Dp) const { OptionalJacobian<2, 2> Dp) const {
// this part of code is modified from Cal3DS2, // this part of code is modified from Cal3DS2,
// since the second part of this model (after project to normalized plane) // since the second part of this model (after project to normalized plane)
// is same as Cal3DS2 // is same as Cal3DS2
@ -116,7 +114,6 @@ Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
const double x = p.x(), y = p.y(); const double x = p.x(), y = p.y();
const double xy2 = x * x + y * y; const double xy2 = x * x + y * y;
const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1); const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1);
@ -126,7 +123,6 @@ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Unified::spaceToNPlane(const Point2& p) const { Point2 Cal3Unified::spaceToNPlane(const Point2& p) const {
const double x = p.x(), y = p.y(); const double x = p.x(), y = p.y();
const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);