Vector3d CSkeleton::fitToLocation(Joint_handle hJoint, Vector3d target_loc)
{
Joint_handle hRoot = rootOf(hJoint);
updateGlobalPostures(hRoot);
if(hRoot == parents[hJoint])
{
joints[hJoint].setOffset(target_loc);
updateGlobalPostures(hRoot);
return globals[hJoint].getOffset();
}
//updateGlobalPostures(0);
Joint_handle hParent = parents[hJoint];
Vector3d dJoint, dTarget;
Vector3d pJoint, pParent;
pJoint = getGlobalPosition(hJoint);
pParent = getGlobalPosition(hParent);
dJoint = (pJoint - pParent).Normalize();
dTarget = (target_loc - pParent).Normalize();
Quaterniond qParent = getGlobalRotation(hParent);
// dJoint = cast_to_vector(!qParent*dJoint*qParent);
// dTarget = cast_to_vector(!qParent*dTarget*qParent);
Vector3d axis = Vector3d::Cross(dJoint, dTarget);
double dot = Vector3d::Dot(dJoint, dTarget);
double angle = acos(Vector3d::Dot(dJoint, dTarget));
Quaterniond qtemp = !qParent*Quaterniond(0, axis.X(), axis.Y(), axis.Z())*qParent;
axis = Vector3d(qtemp.X(), qtemp.Y(), qtemp.Z());
Quaterniond rot;
rot.FromAxisAngle(angle, axis.X(), axis.Y(), axis.Z());
qtemp = rot*Quaterniond(0, dJoint.X(), dJoint.Y(), dJoint.Z())*!rot;
Vector3d pos(qtemp.X(), qtemp.Y(), qtemp.Z());
Vector3d err = pos - dTarget;
if( angle == 0.f ||
(axis.X() == 0 && axis.Y() == 0 && axis.Z() ==0) ||
dJoint == dTarget)
return globals[hJoint].getOffset();
joints[hParent].Rotate(rot);
//rotateJoint(hParent, rot);
updateGlobalPostures(hParent);
//updateGlobalPostures(0);
return globals[hJoint].getOffset();
}
void inline
EstimatorFull::ApplyCorrection( const Eigen::Matrix<double,28,1> &x_error )
{
// TODO: look at quaternion error propagation. it smells
p_i_w += x_error.segment<3>(0);
v_i_w += x_error.segment<3>(3);
q_i_w = QuaternionAd(Quaterniond( 1, x_error(6)/2.0, x_error(7)/2.0, x_error(8)/2.0 ).conjugate()*q_i_w.toQuat()); q_i_w.normalize();
b_omega += x_error.segment<3>(9);
b_a += x_error.segment<3>(12);
lambda += x_error(15);
p_c_i += x_error.segment<3>(16);
q_c_i = QuaternionAd(Quaterniond( 1, x_error(19)/2.0, x_error(20)/2.0, x_error(21)/2.0 ).conjugate()*q_c_i.toQuat()); q_c_i.normalize();
p_w_v += x_error.segment<3>(22);
q_w_v = QuaternionAd(Quaterniond( 1, x_error(25)/2.0, x_error(26)/2.0, x_error(27)/2.0 ).conjugate()*q_w_v.toQuat()); q_w_v.normalize();
}
/*
void ContactDynamics::updateNormalMatrix() {
static Timer t1("t1");
static Timer t2("t2");
static Timer t3("t3");
mN = MatrixXd::Zero(getNumTotalDofs(), getNumContacts());
for (int i = 0; i < getNumContacts(); i++) {
ContactPoint& c = mCollisionChecker->getContact(i);
Vector3d p = c.point;
int skelID1 = mBodyIndexToSkelIndex[c.bdID1];
int skelID2 = mBodyIndexToSkelIndex[c.bdID2];
if (!mSkels[skelID1]->getImmobileState()) {
int index1 = mIndices[skelID1];
int NDOF1 = c.bd1->getSkel()->getNumDofs();
Vector3d N21 = c.normal;
MatrixXd J21 = getJacobian(c.bd1, p);
mN.block(index1, i, NDOF1, 1) = J21.transpose() * N21;
}
if (!mSkels[skelID2]->getImmobileState()) {
t1.startTimer();
int index2 = mIndices[skelID2];
int NDOF2 = c.bd2->getSkel()->getNumDofs();
Vector3d N12 = -c.normal;
int nDofs = c.bd2->getSkel()->getNumDofs();
MatrixXd J12( MatrixXd::Zero(3, nDofs) );
VectorXd invP = math::xformHom(c.bd2->getWorldInvTransform(), p);
t2.startTimer();
for(int dofIndex = 0; dofIndex < c.bd2->getNumDependentDofs(); dofIndex++) {
int index = c.bd2->getDependentDof(dofIndex);
VectorXd Jcol = math::xformHom(c.bd2->getDerivWorldTransform(dofIndex), (Vector3d)invP);
J12.col(index) = Jcol;
}
t2.stopTimer();
if (t2.getCount() == 1500)
t2.printScreen();
// MatrixXd J12 = getJacobian(c.bd2, p);
mN.block(index2, i, NDOF2, 1) = J12.transpose() * N12;
t1.stopTimer();
if (t1.getCount() == 1500) {
t1.printScreen();
cout << t2.getAve() / t1.getAve() * 100 << "%" << endl;
}
}
}
return;
}
*/
MatrixXd ContactDynamics::getTangentBasisMatrix(const Vector3d& p, const Vector3d& n) {
MatrixXd T(MatrixXd::Zero(3, mNumDir));
// Pick an arbitrary vector to take the cross product of (in this case, Z-axis)
Vector3d tangent = Vector3d::UnitZ().cross(n);
// If they're too close, pick another tangent (use X-axis as arbitrary vector)
if (tangent.norm() < EPSILON) {
tangent = Vector3d::UnitX().cross(n);
}
tangent.normalize();
// Rotate the tangent around the normal to compute bases.
// Note: a possible speedup is in place for mNumDir % 2 = 0
// Each basis and its opposite belong in the matrix, so we iterate half as many times.
double angle = (2 * M_PI) / mNumDir;
int iter = (mNumDir % 2 == 0) ? mNumDir / 2 : mNumDir;
for (int i = 0; i < iter; i++) {
Vector3d basis = Quaterniond(AngleAxisd(i * angle, n)) * tangent;
T.block<3,1>(0, i) = basis;
if (mNumDir % 2 == 0) {
T.block<3,1>(0, i + iter) = -basis;
}
}
return T;
}
void test_stdvector_overload()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Affine2f())); // does not need the specialization (2+1)^2 = 9
CALL_SUBTEST_4(check_stdvector_transform(Affine3f()));
CALL_SUBTEST_4(check_stdvector_transform(Affine3d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
SO3 SO3
::expAndTheta(const Vector3d & omega, double * theta)
{
*theta = omega.norm();
double half_theta = 0.5*(*theta);
double imag_factor;
double real_factor = cos(half_theta);
if((*theta)<SMALL_EPS)
{
double theta_sq = (*theta)*(*theta);
double theta_po4 = theta_sq*theta_sq;
imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4;
}
else
{
double sin_half_theta = sin(half_theta);
imag_factor = sin_half_theta/(*theta);
}
return SO3(Quaterniond(real_factor,
imag_factor*omega.x(),
imag_factor*omega.y(),
imag_factor*omega.z()));
}
void test_newstdvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST(check_stdvector_transform(Transform2f()));
CALL_SUBTEST(check_stdvector_transform(Transform3f()));
CALL_SUBTEST(check_stdvector_transform(Transform3d()));
//CALL_SUBTEST(check_stdvector_transform(Transform4d()));
// some Quaternion
CALL_SUBTEST(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST(check_stdvector_quaternion(Quaterniond()));
}
void test_stdvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Projective2f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3d()));
//CALL_SUBTEST(heck_stdvector_transform(Projective4d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
Matrix3d fromCompactQuaternion(const Vector3d& v) {
double w = 1-v.squaredNorm();
if (w<0)
return Matrix3d::Identity();
else
w=sqrt(w);
return Quaterniond(w, v[0], v[1], v[2]).toRotationMatrix();
}
void FlipHandsGoalRegion::sample(std::vector<double> &xyz_rpy)
{
// compute the approach pose so that the gripper is perpendicular to the given direction with a random slant with
// respect to the horizontal plane
boost::uniform_real<double> dist(-0.5, 0.5) ;
Vector3d pos2_offset = Vector3d(planner->x_tol * dist(gen), planner->y_tol * dist(gen), planner->z_tol * dist(gen)) ;
// compute the pose of the grasping arm (arm 2)
Vector3d pos2 = pos2_offset + p1.translation() ;
double d = (p1.translation() - p2.translation()).norm() ;
Vector3d pos1 = p2.translation() + pos2_offset +
Vector3d(boost::uniform_real<double>(-0.05, 0.05)(gen), boost::uniform_real<double>(-0.05, 0.05)(gen), boost::uniform_real<double>(0, 0.2)(gen));
double roll1, pitch1, yaw1, x1, y1, z1 ;
x1 = pos1.x(), y1 = pos1.y(), z1 = pos1.z() ;
if ( planner->arm == "r1" )
rpyFromQuat(lookAt(Vector3d(1, 0, 0)), roll1, pitch1, yaw1) ;
else
rpyFromQuat(lookAt(Vector3d(-1, 0, 0)), roll1, pitch1, yaw1) ;
double roll2, pitch2, yaw2, x2, y2, z2 ;
x2 = pos2.x() ; y2 = pos2.y() ; z2 = pos2.z() ;
rpyFromQuat(Quaterniond(p1.rotation()), roll2, pitch2, yaw2) ;
double pitch = boost::uniform_real<double>(planner->pitch_tol_min, planner->pitch_tol_max)(gen) ;
double yaw = boost::uniform_real<double>(planner->yaw_tol_min, planner->yaw_tol_max)(gen) ;
double roll = boost::uniform_real<double>(planner->roll_tol_min, planner->roll_tol_max)(gen) ;
roll1 += roll ; yaw1 += yaw ; pitch1 += pitch ;
if ( planner->arm == "r1" )
{
xyz_rpy.push_back(x1) ; xyz_rpy.push_back(y1) ; xyz_rpy.push_back(z1) ;
xyz_rpy.push_back(roll1) ; xyz_rpy.push_back(pitch1) ; xyz_rpy.push_back(yaw1) ;
xyz_rpy.push_back(x2) ; xyz_rpy.push_back(y2) ; xyz_rpy.push_back(z2) ;
xyz_rpy.push_back(roll2) ; xyz_rpy.push_back(pitch2) ; xyz_rpy.push_back(yaw2) ;
}
else
{
xyz_rpy.push_back(x2) ; xyz_rpy.push_back(y2) ; xyz_rpy.push_back(z2) ;
xyz_rpy.push_back(roll2) ; xyz_rpy.push_back(pitch2) ; xyz_rpy.push_back(yaw2) ;
xyz_rpy.push_back(x1) ; xyz_rpy.push_back(y1) ; xyz_rpy.push_back(z1) ;
xyz_rpy.push_back(roll1) ; xyz_rpy.push_back(pitch1) ; xyz_rpy.push_back(yaw1) ;
}
}
void GLWidget::rotateSelected(float x, float y, float z, float angle)
{
obj.rotateSelected(x, y, z, angle);
QQuaternion rotate = QQuaternion::fromAxisAndAngle(x, y, z, angle);
QVector4D qrotate = rotate.toVector4D();
Matrix3d mat3 = Quaterniond(qrotate.w(),qrotate.x(), qrotate.y(), qrotate.z()).toRotationMatrix();
Matrix4d mat4 = Matrix4d::Identity();
mat4.block(0,0,3,3) = mat3;
pd.InterTransform(obj.getSelectVertexIds(),rotate);
qDebug() << "update!!";
update();
}
void test_geo_quaternion()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1(( quaternion<float,AutoAlign>() ));
CALL_SUBTEST_1( check_const_correctness(Quaternionf()) );
CALL_SUBTEST_2(( quaternion<double,AutoAlign>() ));
CALL_SUBTEST_2( check_const_correctness(Quaterniond()) );
CALL_SUBTEST_3(( quaternion<float,DontAlign>() ));
CALL_SUBTEST_4(( quaternion<double,DontAlign>() ));
CALL_SUBTEST_5(( quaternionAlignment<float>() ));
CALL_SUBTEST_6(( quaternionAlignment<double>() ));
CALL_SUBTEST_1( mapQuaternion<float>() );
CALL_SUBTEST_2( mapQuaternion<double>() );
}
}
void
EditorIkSolver::_checkHingeZ(ArRef<Joint> joint, Vector3d /* requestedTranslation */, Quaterniond requestedRotation) {
double prx, pry, prz;
joint->getDeltaRotation().getEulerAngles(prx, pry, prz);
requestedRotation = joint->getDeltaRotation() * requestedRotation;
double rx, ry, rz;
requestedRotation.getEulerAngles(rx, ry, rz);
double min, max;
ar_down_cast<JointConstraint1DOF>(joint->getConstraint())->getLimitValues(min, max);
rz = _clampWithPreviousValue(min, max, prz, rz);
joint->setDeltaRotation(Quaterniond(Vector3d(0.0, 0.0, 1.0), rz));
}
Quaterniond lookAt(const Eigen::Vector3d &dir, double roll)
{
Vector3d nz = dir, na, nb ;
nz.normalize() ;
double q = sqrt(nz.x() * nz.x() + nz.y() * nz.y()) ;
if ( q < 1.0e-4 )
{
na = Vector3d(1, 0, 0) ;
nb = nz.cross(na) ;
}
else {
na = Vector3d(-nz.y()/q, nz.x()/q, 0) ;
nb = Vector3d(-nz.x() * nz.z()/q, -nz.y() * nz.z()/q, q) ;
}
Matrix3d r ;
r << na, nb, nz ;
return Quaterniond(r) * AngleAxisd(roll, Eigen::Vector3d::UnitZ()) ;
}
void CSkeleton::updateGlobalPostures(Joint_handle hJoint)
{
if(isRoot(hJoint))
globals[hJoint] = joints[hJoint];
else
{
size_t parent_idx = parents[hJoint];
globals[hJoint].setOffset(globals[parent_idx].getOffset());
Quaterniond q = globals[parent_idx].getRotation();
Vector3d offset = joints[hJoint].getOffset();
Quaterniond qtemp = q*Quaterniond(0, offset.X(), offset.Y(), offset.Z())*!q;
Vector3d pos(qtemp.X(), qtemp.Y(), qtemp.Z());
globals[hJoint].Translate(pos);
globals[hJoint].setRotation(q*joints[hJoint].getRotation());
}
Joint_handle_iterator jh_it, jh_end = children[hJoint].end();
for(jh_it = children[hJoint].begin(); jh_it != jh_end; ++jh_it)
{
updateGlobalPostures(*jh_it);
}
}
Isometry3d fromVectorQT(const Vector7d& v) {
Isometry3d t;
t=Quaterniond(v[6], v[3], v[4], v[5]).toRotationMatrix();
t.translation() = v.head<3>();
return t;
}
vector<Marker> Visualization::visualizeGripper(const string& ns,
const string& frame_id, const geometry_msgs::Pose& pose, double gripper_state) const
{
Vector3d trans(pose.position.x, pose.position.y, pose.position.z);
Quaterniond rot(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
Marker l_finger = visualizeGripperPart("package://pr2_description/meshes/gripper_v0"
"/l_finger_tip.dae", ns, frame_id, 0);
Marker r_finger = visualizeGripperPart("package://pr2_description/meshes/gripper_v0"
"/l_finger_tip.dae", ns, frame_id, 1);
Marker palm;
palm = visualizeGripperPart("package://pr2_description/meshes/gripper_v0"
"/gripper_palm.dae", ns, frame_id, 2);
Marker bounding_box = visualizeGripperBox("bounding_box", frame_id, 4);
bounding_box.pose = pose;
Vector3d position(-0.051, 0, -0.025);
Quaterniond rotation = Quaterniond::Identity();
position = rot * position + trans;
rotation = rotation * rot;
palm.pose.position.x = position.x();
palm.pose.position.y = position.y();
palm.pose.position.z = position.z();
palm.pose.orientation.w = rotation.w();
palm.pose.orientation.x = rotation.x();
palm.pose.orientation.y = rotation.y();
palm.pose.orientation.z = rotation.z();
double finger_y_delta = gripper_state * 0.8 / 2;
double finger_x_delta = 0;
position = Vector3d(0.1175 - finger_x_delta, 0.015 + finger_y_delta, -0.025);
rotation = Quaterniond::Identity();
position = rot * position + trans;
rotation = rotation * rot;
l_finger.pose.position.x = position.x();
l_finger.pose.position.y = position.y();
l_finger.pose.position.z = position.z();
l_finger.pose.orientation.w = rotation.w();
l_finger.pose.orientation.x = rotation.x();
l_finger.pose.orientation.y = rotation.y();
l_finger.pose.orientation.z = rotation.z();
position = Vector3d(0.1175 - finger_x_delta, -0.015 - finger_y_delta, -0.025);
rotation = Quaterniond(AngleAxisd(M_PI, Vector3d(1, 0, 0)));
position = rot * position + trans;
rotation = rot * rotation;
r_finger.pose.position.x = position.x();
r_finger.pose.position.y = position.y();
r_finger.pose.position.z = position.z();
r_finger.pose.orientation.w = rotation.w();
r_finger.pose.orientation.x = rotation.x();
r_finger.pose.orientation.y = rotation.y();
r_finger.pose.orientation.z = rotation.z();
vector < Marker > result;
result.push_back(palm);
result.push_back(l_finger);
result.push_back(r_finger);
/* palm marker */
getTemplateExtractionPoint(position);
rotation = Quaterniond::Identity();
position = rot * position + trans;
rotation = rotation * rot;
Marker palm_marker;
palm_marker.header.frame_id = frame_id;
palm_marker.header.stamp = ros::Time::now();
palm_marker.ns = ns;
palm_marker.id = 3;
palm_marker.type = Marker::SPHERE;
palm_marker.action = Marker::ADD;
palm_marker.pose.position.x = position.x();
palm_marker.pose.position.y = position.y();
palm_marker.pose.position.z = position.z();
palm_marker.pose.orientation.x = 0.0;
palm_marker.pose.orientation.y = 0.0;
palm_marker.pose.orientation.z = 0.0;
palm_marker.pose.orientation.w = 1.0;
palm_marker.scale.x = 0.01;
palm_marker.scale.y = 0.01;
palm_marker.scale.z = 0.01;
palm_marker.color.a = 1.0;
palm_marker.color.r = 0.0;
palm_marker.color.g = 1.0;
result.push_back(palm_marker);
return result;
}
void Estimator::double2vector()
{
Vector3d origin_R0 = Utility::R2ypr(Rs[0]);
Vector3d origin_P0 = Ps[0];
if (failure_occur)
{
origin_R0 = Utility::R2ypr(last_R0);
origin_P0 = last_P0;
failure_occur = 0;
}
Vector3d origin_R00 = Utility::R2ypr(Quaterniond(para_Pose[0][6],
para_Pose[0][3],
para_Pose[0][4],
para_Pose[0][5]).toRotationMatrix());
double y_diff = origin_R0.x() - origin_R00.x();
//TODO
Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));
for (int i = 0; i <= WINDOW_SIZE; i++)
{
Rs[i] = rot_diff * Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();
Ps[i] = rot_diff * Vector3d(para_Pose[i][0] - para_Pose[0][0],
para_Pose[i][1] - para_Pose[0][1],
para_Pose[i][2] - para_Pose[0][2]) + origin_P0;
Vs[i] = rot_diff * Vector3d(para_SpeedBias[i][0],
para_SpeedBias[i][1],
para_SpeedBias[i][2]);
Bas[i] = Vector3d(para_SpeedBias[i][3],
para_SpeedBias[i][4],
para_SpeedBias[i][5]);
Bgs[i] = Vector3d(para_SpeedBias[i][6],
para_SpeedBias[i][7],
para_SpeedBias[i][8]);
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
tic[i] = Vector3d(para_Ex_Pose[i][0],
para_Ex_Pose[i][1],
para_Ex_Pose[i][2]);
ric[i] = Quaterniond(para_Ex_Pose[i][6],
para_Ex_Pose[i][3],
para_Ex_Pose[i][4],
para_Ex_Pose[i][5]).toRotationMatrix();
}
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < f_manager.getFeatureCount(); i++)
dep(i) = para_Feature[i][0];
f_manager.setDepth(dep);
if (LOOP_CLOSURE && relocalize && retrive_data_vector[0].relative_pose && !retrive_data_vector[0].relocalized)
{
for (int i = 0; i < (int)retrive_data_vector.size();i++)
retrive_data_vector[i].relocalized = true;
Matrix3d vio_loop_r;
Vector3d vio_loop_t;
vio_loop_r = rot_diff * Quaterniond(retrive_data_vector[0].loop_pose[6], retrive_data_vector[0].loop_pose[3], retrive_data_vector[0].loop_pose[4], retrive_data_vector[0].loop_pose[5]).normalized().toRotationMatrix();
vio_loop_t = rot_diff * Vector3d(retrive_data_vector[0].loop_pose[0] - para_Pose[0][0],
retrive_data_vector[0].loop_pose[1] - para_Pose[0][1],
retrive_data_vector[0].loop_pose[2] - para_Pose[0][2]) + origin_P0;
Quaterniond vio_loop_q(vio_loop_r);
double relocalize_yaw;
relocalize_yaw = Utility::R2ypr(retrive_data_vector[0].R_old).x() - Utility::R2ypr(vio_loop_r).x();
relocalize_r = Utility::ypr2R(Vector3d(relocalize_yaw, 0, 0));
relocalize_t = retrive_data_vector[0].P_old- relocalize_r * vio_loop_t;
}
}
void Estimator::optimization()
{
ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
if (!ESTIMATE_EXTRINSIC)
{
ROS_DEBUG("fix extinsic param");
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
else
ROS_DEBUG("estimate extinsic param");
}
TicToc t_whole, t_prepare;
vector2double();
if (last_marginalization_info)
{
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
if (pre_integrations[j]->sum_dt > 10.0)
continue;
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
int f_m_cnt = 0;
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i == imu_j)
{
continue;
}
Vector3d pts_j = it_per_frame.point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
f_m_cnt++;
}
}
relocalize = false;
//loop close factor
if(LOOP_CLOSURE)
{
int loop_constraint_num = 0;
for (int k = 0; k < (int)retrive_data_vector.size(); k++)
{
for(int i = 0; i < WINDOW_SIZE; i++)
{
if(retrive_data_vector[k].header == Headers[i].stamp.toSec())
{
relocalize = true;
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(retrive_data_vector[k].loop_pose, SIZE_POSE, local_parameterization);
loop_window_index = i;
loop_constraint_num++;
int retrive_feature_index = 0;
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
int start = it_per_id.start_frame;
if(start <= i)
{
while(retrive_data_vector[k].features_ids[retrive_feature_index] < it_per_id.feature_id)
{
retrive_feature_index++;
}
if(retrive_data_vector[k].features_ids[retrive_feature_index] == it_per_id.feature_id)
{
Vector3d pts_j = Vector3d(retrive_data_vector[k].measurements[retrive_feature_index].x, retrive_data_vector[k].measurements[retrive_feature_index].y, 1.0);
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[start], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);
retrive_feature_index++;
}
}
}
}
}
}
ROS_DEBUG("loop constraint num: %d", loop_constraint_num);
}
ROS_DEBUG("visual measurement count: %d", f_m_cnt);
ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//cout << summary.BriefReport() << endl;
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
ROS_DEBUG("solver costs: %f", t_solver.toc());
// relative info between two loop frame
if(LOOP_CLOSURE && relocalize)
{
for (int k = 0; k < (int)retrive_data_vector.size(); k++)
{
for(int i = 0; i< WINDOW_SIZE; i++)
{
if(retrive_data_vector[k].header == Headers[i].stamp.toSec())
{
retrive_data_vector[k].relative_pose = true;
Matrix3d Rs_i = Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();
Vector3d Ps_i = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]);
Quaterniond Qs_loop;
Qs_loop = Quaterniond(retrive_data_vector[k].loop_pose[6], retrive_data_vector[k].loop_pose[3], retrive_data_vector[k].loop_pose[4], retrive_data_vector[k].loop_pose[5]).normalized().toRotationMatrix();
Matrix3d Rs_loop = Qs_loop.toRotationMatrix();
Vector3d Ps_loop = Vector3d( retrive_data_vector[k].loop_pose[0], retrive_data_vector[k].loop_pose[1], retrive_data_vector[k].loop_pose[2]);
retrive_data_vector[k].relative_t = Rs_loop.transpose() * (Ps_i - Ps_loop);
retrive_data_vector[k].relative_q = Rs_loop.transpose() * Rs_i;
retrive_data_vector[k].relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs_i).x() - Utility::R2ypr(Rs_loop).x());
if (abs(retrive_data_vector[k].relative_yaw) > 30.0 || retrive_data_vector[k].relative_t.norm() > 20.0)
retrive_data_vector[k].relative_pose = false;
}
}
}
}
double2vector();
TicToc t_whole_marginalization;
if (marginalization_flag == MARGIN_OLD)
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
if (last_marginalization_info)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
drop_set.push_back(i);
}
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
{
if (pre_integrations[1]->sum_dt < 10.0)
{
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
vector<int>{0, 1});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
{
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
if (imu_i != 0)
continue;
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i == imu_j)
continue;
Vector3d pts_j = it_per_frame.point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
vector<int>{0, 3});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
}
TicToc t_pre_margin;
marginalization_info->preMarginalize();
ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
TicToc t_margin;
marginalization_info->marginalize();
ROS_DEBUG("marginalization %f ms", t_margin.toc());
std::unordered_map<long, double *> addr_shift;
for (int i = 1; i <= WINDOW_SIZE; i++)
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
for (int i = 0; i < NUM_OF_CAM; i++)
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
if (last_marginalization_info)
delete last_marginalization_info;
last_marginalization_info = marginalization_info;
last_marginalization_parameter_blocks = parameter_blocks;
}
else
{
if (last_marginalization_info &&
std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
if (last_marginalization_info)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])
drop_set.push_back(i);
}
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
TicToc t_pre_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->preMarginalize();
ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());
TicToc t_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->marginalize();
ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
std::unordered_map<long, double *> addr_shift;
for (int i = 0; i <= WINDOW_SIZE; i++)
{
if (i == WINDOW_SIZE - 1)
continue;
else if (i == WINDOW_SIZE)
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
else
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
}
}
for (int i = 0; i < NUM_OF_CAM; i++)
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
if (last_marginalization_info)
delete last_marginalization_info;
last_marginalization_info = marginalization_info;
last_marginalization_parameter_blocks = parameter_blocks;
}
}
ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc());
ROS_DEBUG("whole time for ceres: %f", t_whole.toc());
}
Quaterniond Rotation::GetQuaternion() const
{
OTL_ASSERT(m_type == RotationType::Quaternion, "Invalid rotation type");
return Quaterniond(m_matrix);
}
void Visualizer::publishMinimal(
const cv::Mat& img,
const FramePtr& frame,
const FrameHandlerMono& slam,
const double timestamp)
{
++trace_id_;
std_msgs::Header header_msg;
header_msg.frame_id = "/camera";
header_msg.seq = trace_id_;
header_msg.stamp = ros::Time(timestamp);
// publish info msg.
if(pub_info_.getNumSubscribers() > 0)
{
svo_msgs::Info msg_info;
msg_info.header = header_msg;
msg_info.processing_time = slam.lastProcessingTime();
msg_info.keyframes.resize(slam.map().keyframes_.size());
for(list<FramePtr>::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it)
msg_info.keyframes.push_back((*it)->id_);
msg_info.stage = static_cast<int>(slam.stage());
msg_info.tracking_quality = static_cast<int>(slam.trackingQuality());
if(frame != NULL)
msg_info.num_matches = slam.lastNumObservations();
else
msg_info.num_matches = 0;
pub_info_.publish(msg_info);
}
if(frame == NULL)
{
if(pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_PAUSED)
{
// Display image when slam is not running.
cv_bridge::CvImage img_msg;
img_msg.header.stamp = ros::Time::now();
img_msg.header.frame_id = "/image";
img_msg.image = img;
img_msg.encoding = sensor_msgs::image_encodings::MONO8;
pub_images_.publish(img_msg.toImageMsg());
}
return;
}
// Publish pyramid-image every nth-frame.
if(img_pub_nth_ > 0 && trace_id_%img_pub_nth_ == 0 && pub_images_.getNumSubscribers() > 0)
{
const int scale = (1<<img_pub_level_);
cv::Mat img_rgb(frame->img_pyr_[img_pub_level_].size(), CV_8UC3);
cv::cvtColor(frame->img_pyr_[img_pub_level_], img_rgb, CV_GRAY2RGB);
if(slam.stage() == FrameHandlerBase::STAGE_SECOND_FRAME)
{
// During initialization, draw lines.
const vector<cv::Point2f>& px_ref(slam.initFeatureTrackRefPx());
const vector<cv::Point2f>& px_cur(slam.initFeatureTrackCurPx());
for(vector<cv::Point2f>::const_iterator it_ref=px_ref.begin(), it_cur=px_cur.begin();
it_ref != px_ref.end(); ++it_ref, ++it_cur)
cv::line(img_rgb,
cv::Point2f(it_cur->x/scale, it_cur->y/scale),
cv::Point2f(it_ref->x/scale, it_ref->y/scale), cv::Scalar(0,255,0), 2);
}
if(img_pub_level_ == 0)
{
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->type == Feature::EDGELET)
cv::line(img_rgb,
cv::Point2f((*it)->px[0]+3*(*it)->grad[1], (*it)->px[1]-3*(*it)->grad[0]),
cv::Point2f((*it)->px[0]-3*(*it)->grad[1], (*it)->px[1]+3*(*it)->grad[0]),
cv::Scalar(255,0,255), 2);
else
cv::rectangle(img_rgb,
cv::Point2f((*it)->px[0]-2, (*it)->px[1]-2),
cv::Point2f((*it)->px[0]+2, (*it)->px[1]+2),
cv::Scalar(0,255,0), CV_FILLED);
}
}
if(img_pub_level_ == 1)
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
cv::rectangle(img_rgb,
cv::Point2f((*it)->px[0]/scale-1, (*it)->px[1]/scale-1),
cv::Point2f((*it)->px[0]/scale+1, (*it)->px[1]/scale+1),
cv::Scalar(0,255,0), CV_FILLED);
else
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
cv::rectangle(img_rgb,
cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
cv::Scalar(0,255,0), CV_FILLED);
cv_bridge::CvImage img_msg;
img_msg.header = header_msg;
img_msg.image = img_rgb;
img_msg.encoding = sensor_msgs::image_encodings::BGR8;
pub_images_.publish(img_msg.toImageMsg());
}
if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
{
Quaterniond q;
Vector3d p;
Matrix<double,6,6> Cov;
if(publish_world_in_cam_frame_)
{
// publish world in cam frame
SE3 T_cam_from_world(frame->T_f_w_* T_world_from_vision_);
q = Quaterniond(T_cam_from_world.rotation_matrix());
p = T_cam_from_world.translation();
Cov = frame->Cov_;
}
else
{
// publish cam in world frame
SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
p = T_world_from_cam.translation();
Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
}
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose->header = header_msg;
msg_pose->pose.pose.position.x = p[0];
msg_pose->pose.pose.position.y = p[1];
msg_pose->pose.pose.position.z = p[2];
msg_pose->pose.pose.orientation.x = q.x();
msg_pose->pose.pose.orientation.y = q.y();
msg_pose->pose.pose.orientation.z = q.z();
msg_pose->pose.pose.orientation.w = q.w();
for(size_t i=0; i<36; ++i)
msg_pose->pose.covariance[i] = Cov(i%6, i/6);
pub_pose_.publish(msg_pose);
}
}
void PoseGraph::optimize6DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
while(!optimize_buf.empty())
{
cur_index = optimize_buf.front();
first_looped_index = earliest_loop_index;
optimize_buf.pop();
}
m_optimize_buf.unlock();
if (cur_index != -1)
{
printf("optimize pose graph \n");
TicToc tmp_t;
m_keyframelist.lock();
KeyFrame* cur_kf = getKeyFrame(cur_index);
int max_length = cur_index + 1;
// w^t_i w^q_i
double t_array[max_length][3];
double q_array[max_length][4];
double sequence_array[max_length];
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//ptions.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(0.1);
//loss_function = new ceres::CauchyLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
list<KeyFrame*>::iterator it;
int i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
(*it)->local_index = i;
Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
(*it)->getVioPose(tmp_t, tmp_r);
tmp_q = tmp_r;
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
q_array[i][0] = tmp_q.w();
q_array[i][1] = tmp_q.x();
q_array[i][2] = tmp_q.y();
q_array[i][3] = tmp_q.z();
sequence_array[i] = (*it)->sequence;
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
problem.SetParameterBlockConstant(q_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
//add edge
for (int j = 1; j < 5; j++)
{
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
Quaterniond q_i_j = Quaterniond(q_array[i-j][0], q_array[i-j][1], q_array[i-j][2], q_array[i-j][3]);
Quaterniond q_i = Quaterniond(q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]);
relative_t = q_i_j.inverse() * relative_t;
Quaterniond relative_q = q_i_j.inverse() * q_i;
ceres::CostFunction* vo_function = RelativeRTError::Create(relative_t.x(), relative_t.y(), relative_t.z(),
relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
0.1, 0.01);
problem.AddResidualBlock(vo_function, NULL, q_array[i-j], t_array[i-j], q_array[i], t_array[i]);
}
}
//add loop edge
if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index);
int connected_index = getKeyFrame((*it)->loop_index)->local_index;
Vector3d relative_t;
relative_t = (*it)->getLoopRelativeT();
Quaterniond relative_q;
relative_q = (*it)->getLoopRelativeQ();
ceres::CostFunction* loop_function = RelativeRTError::Create(relative_t.x(), relative_t.y(), relative_t.z(),
relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
0.1, 0.01);
problem.AddResidualBlock(loop_function, loss_function, q_array[connected_index], t_array[connected_index], q_array[i], t_array[i]);
}
if ((*it)->index == cur_index)
break;
i++;
}
m_keyframelist.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
//printf("pose optimization time: %f \n", tmp_t.toc());
/*
for (int j = 0 ; j < i; j++)
{
printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );
}
*/
m_keyframelist.lock();
i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
Quaterniond tmp_q(q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]);
Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
Matrix3d tmp_r = tmp_q.toRotationMatrix();
(*it)-> updatePose(tmp_t, tmp_r);
if ((*it)->index == cur_index)
break;
i++;
}
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);
cur_kf->getVioPose(vio_t, vio_r);
m_drift.lock();
r_drift = cur_r * vio_r.transpose();
t_drift = cur_t - r_drift * vio_t;
m_drift.unlock();
//cout << "t_drift " << t_drift.transpose() << endl;
//cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;
it++;
for (; it != keyframelist.end(); it++)
{
Vector3d P;
Matrix3d R;
(*it)->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
(*it)->updatePose(P, R);
}
m_keyframelist.unlock();
updatePath();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}
void setTransform( const base::Affine3d& trans )
{
this->translation = trans.translation();
this->orientation = Quaterniond(trans.rotation());
}
