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

Printing now transpose

release/4.3a0
Frank dellaert 2020-10-12 15:37:02 -04:00
parent 1708162723
commit 5df3eebd2e
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/OrientedPlane3.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/geometry/OrientedPlane3.h> #include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <iostream> #include <iostream>
using namespace std; using namespace std;
@ -28,7 +29,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void OrientedPlane3::print(const string& s) const { void OrientedPlane3::print(const string& s) const {
Vector4 coeffs = planeCoefficients(); Vector4 coeffs = planeCoefficients();
cout << s << " : " << coeffs << endl; cout << s << " : " << coeffs.transpose() << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -42,15 +43,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
double pred_d = n_.unitVector().dot(xr.translation()) + d_; double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) { if (Hr) {
*Hr = Matrix::Zero(3,6); Hr->setZero();
Hr->block<2, 3>(0, 0) = D_rotated_plane; Hr->block<2, 3>(0, 0) = D_rotated_plane;
Hr->block<1, 3>(2, 3) = unit_vec; Hr->block<1, 3>(2, 3) = unit_vec;
} }
if (Hp) { if (Hp) {
Vector2 hpp = n_.basis().transpose() * xr.translation(); Hp->setZero();
*Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = D_rotated_pose; Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp; Hp->block<1, 2>(2, 0) = n_.basis().transpose() * xr.translation();
(*Hp)(2, 2) = 1; (*Hp)(2, 2) = 1;
} }