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

testing mode: still stuck, getting closer to the problem

release/4.3a0
lcarlone 2021-08-26 21:50:04 -04:00
parent e5677e3805
commit e1c5b50934
2 changed files with 30 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/SphericalCamera.cpp
View File

@ -60,6 +60,21 @@ Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose,
return pu;
}
/* ************************************************************************* */
Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint) const {
Matrix23 Dtf_rot;
Matrix2 Dtf_point; // calculated by transformTo if needed
const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0, Dpoint ? &Dtf_point : 0);
if (Dpose)
*Dpose << Dtf_rot, Matrix::Zero(2,3); //2x6 (translation part is zero)
if (Dpoint)
*Dpoint = Dtf_point; //2x2
return pu;
}
/* ************************************************************************* */
Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
return pose().transformFrom(depth * pu);

17
gtsam/geometry/SphericalCamera.h
View File

@ -31,15 +31,20 @@ namespace gtsam {
class GTSAM_EXPORT EmptyCal {
protected:
double d_ = 0;
Matrix3 K_;
public:
///< shared pointer to calibration object
EmptyCal()
: d_(0) {
: K_(Matrix3::Identity()) {
}
/// Default destructor
virtual ~EmptyCal() = default;
using shared_ptr = boost::shared_ptr<EmptyCal>;
void print(const std::string& s) const {
std::cout << "empty calibration: " << s << std::endl;
}
Matrix3 K() const {return K_;}
};
/**
@ -159,6 +164,14 @@ class GTSAM_EXPORT SphericalCamera {
Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/** Project point into the image
* (note: there is no CheiralityException for a spherical camera)
* @param point 3D direction in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Unit3& p, const double depth) const;