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
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Point2.h>
#include <cmath>
@ -53,10 +53,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
/* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
OptionalJacobian<2,10> Dcal,
Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal,
OptionalJacobian<2, 2> Dp) const {
// this part of code is modified from Cal3DS2,
// since the second part of this model (after project to normalized plane)
// 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 {
const double x = p.x(), y = p.y();
const double xy2 = x * x + y * y;
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 {
const double x = p.x(), y = p.y();
const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);