8 void Node::setTransform()
21 Eigen::Matrix3d Node::dRidx, Node::dRidy, Node::dRidz;
28 dRidy << 0.0,0.0,-2.0,
40 if (qrot.w() < 0) qrot.coeffs().head<3>() = -qrot.coeffs().head<3>();
41 double sn = qrot.coeffs().head<3>().squaredNorm();
43 qrot.coeffs().head<3>() *= -1.0/(sqrt(sn)*1.0001);
44 qrot.w() = sqrt(1.0 - qrot.coeffs().head<3>().squaredNorm());
45 if (std::isnan(qrot.x()) || std::isnan(qrot.y()) || std::isnan(qrot.z()) || std::isnan(qrot.w()) )
47 printf(
"[NormRot] Bad quaternion: %f %f %f %f\n", qrot.x(), qrot.y(), qrot.z(), qrot.w());
56 void Node::setDr(
bool local)
62 dRdx = w2n.block<3,3>(0,0) * dRidx;
63 dRdy = w2n.block<3,3>(0,0) * dRidy;
64 dRdz = w2n.block<3,3>(0,0) * dRidz;
66 dRdx = dRidx * w2n.block<3,3>(0,0);
67 dRdy = dRidy * w2n.block<3,3>(0,0);
68 dRdz = dRidz * w2n.block<3,3>(0,0);
73 double x2 = qrot.x() * 2.0;
74 double y2 = qrot.y() * 2.0;
75 double z2 = qrot.z() * 2.0;
76 double w2 = qrot.w() * 2.0;
77 double xw = qrot.x()/qrot.w();
78 double yw = qrot.y()/qrot.w();
79 double zw = qrot.z()/qrot.w();
91 dRdx(2,1) = -dRdx(1,2);
92 dRdx(2,2) = dRdx(1,1);
97 dRdy(0,2) = (-w2)+yw*y2;
101 dRdy(1,2) = dRdx(2,0);
103 dRdy(2,0) = -dRdy(0,2);
104 dRdy(2,1) = dRdx(0,2);
105 dRdy(2,2) = dRdy(0,0);
109 dRdz(0,1) = w2-zw*z2;
110 dRdz(0,2) = dRdy(1,0);
112 dRdz(1,0) = -dRdz(0,1);
113 dRdz(1,1) = dRdz(0,0);
114 dRdz(1,2) = dRdx(0,1);
116 dRdz(2,0) = dRdy(0,1);
117 dRdz(2,1) = dRdx(1,0);
122 void Node::normRotLocal()
125 if (qrot.w() < 0) qrot.coeffs().head<3>() = -qrot.coeffs().head<3>();
126 if (std::isnan(qrot.x()) || std::isnan(qrot.y()) || std::isnan(qrot.z()) || std::isnan(qrot.w()) )
128 printf(
"[NormRot] Bad quaternion in normRotLocal(): %f %f %f %f\n", qrot.x(), qrot.y(), qrot.z(), qrot.w());
134 void Node::projectMono(
const Point& point, Eigen::Vector3d& proj)
137 project2im(proj2d, point);
139 proj.head<2>() = proj2d;
142 void Node::projectStereo(
const Point& point, Eigen::Vector3d& proj)
145 Vector3d pc, baseline_vect;
146 project2im(proj2d, point);
149 baseline_vect << baseline, 0, 0;
150 pc = Kcam * (w2n*point - baseline_vect);
151 proj.head<2>() = proj2d;
152 proj(2) = pc(0)/pc(2);
158 const Eigen::Matrix<double,4,1> &trans,
159 const Eigen::Quaternion<double> &qrot)
161 m.block<3,3>(0,0) = qrot.toRotationMatrix().transpose();
167 const Eigen::Matrix<double,4,1> &trans,
168 const Eigen::Quaternion<double> &qrot)
170 m.block<3,3>(0,0) = qrot.toRotationMatrix();
171 m.col(3) = trans.head(3);
178 Eigen::Quaternion<double> &qr,
181 Matrix<double,3,4> tfm;
185 trans.head(3) = tfm*nd1.
trans;
188 qr = q0.inverse()*q1;
191 qr.coeffs() = -qr.coeffs();