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()); }