void StateEstimatorKinematic::computeKss(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &C, int zDim ) { Eigen::Matrix<double,6,6> B = C.transpose(); // Eigen::Matrix<double,6,6> P; dare(A.transpose(), B, _P, zDim); // A^T is used here _K.setZero(); Eigen::Matrix<double,6,6> PB; Eigen::Matrix<double,6,6> BtPB_R; if (zDim == 6) { PB = _P * B; BtPB_R = B.transpose() * PB + _R; _K = PB* BtPB_R.inverse() ; } else { PB.topLeftCorner(6,zDim) = _P * B.topLeftCorner(6,zDim); BtPB_R.topLeftCorner(zDim,zDim) = B.topLeftCorner(6,zDim).transpose() * PB.topLeftCorner(6,zDim) + _R.topLeftCorner(zDim,zDim); _K.topLeftCorner(6,zDim) = PB.topLeftCorner(6,zDim)* BtPB_R.topLeftCorner(zDim,zDim).inverse(); } }
void StateEstimatorKinematic::dare(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &B, Eigen::Matrix<double,6,6> &P,int zDim) { Eigen::Matrix<double,6,6> Ainv = A.inverse(); Eigen::Matrix<double,6,6> ABRB; if (zDim == 6) { ABRB = Ainv * B * _R.llt().solve(B.transpose()); } else { ABRB = Ainv * B.topLeftCorner(6,zDim) * _R.topLeftCorner(zDim,zDim).llt().solve(B.topLeftCorner(6,zDim).transpose()); } Eigen::Matrix<double,2*6,2*6> Z; Z.block(0,0,6,6) = Ainv; Z.block(0,6,6,6) = ABRB; Z.block(6,0,6,6) = _Q * Ainv; Z.block(6,6,6,6) = A.transpose() + _Q * ABRB; Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*6,2*6> > ces; ces.compute(Z); Eigen::Matrix<std::complex<double>,2*6,1> eigVal = ces.eigenvalues(); Eigen::Matrix<std::complex<double>,2*6,2*6> eigVec = ces.eigenvectors(); Eigen::Matrix<std::complex<double>,2*6,6> unstableEigVec; int ctr = 0; for (int i = 0; i < 2*6; i++) { if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) { unstableEigVec.col(ctr) = eigVec.col(i); ctr++; if (ctr > 6) break; } } Eigen::Matrix<std::complex<double>,6,6> U21inv = unstableEigVec.block(0,0,6,6).inverse(); Eigen::Matrix<std::complex<double>,6,6> PP = unstableEigVec.block(6,0,6,6) * U21inv; for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { P(i,j) = PP(i,j).real(); } } }
/* static */ bool ocraWbiConversions::wbiToOcraCoMJacobian(const Eigen::MatrixXd &jac, Eigen::Matrix<double,3,Eigen::Dynamic> &J) { if(DIM_T != jac.rows() || jac.cols() != J.cols()) { std::cout<<"ERROR: Input and output matrices dimensions should be the same" <<std::endl; return false; } Eigen::MatrixXd jac3; Eigen::Matrix3d jac1,jac2; jac3.resize(3,jac.cols()-6); jac1 = jac.topLeftCorner(3,3); jac2 = jac.block<3,3>(0,3); jac3 = jac.topRightCorner(3,jac.cols()-6); J.topLeftCorner(3,3) = jac2; J.block<3,3>(0,3) = jac1; J.topRightCorner(3,jac.cols()-6) = jac3; return true; }
void run () { auto input_header = Header::open (argument[0]); Header output_header (input_header); output_header.datatype() = DataType::from_command_line (DataType::from<float> ()); // Linear transform_type linear_transform; bool linear = false; auto opt = get_options ("linear"); if (opt.size()) { linear = true; linear_transform = load_transform (opt[0][0]); } else { linear_transform.setIdentity(); } // Replace const bool replace = get_options ("replace").size(); if (replace && !linear) { INFO ("no linear is supplied so replace with the default (identity) transform"); linear = true; } // Template opt = get_options ("template"); Header template_header; if (opt.size()) { if (replace) throw Exception ("you cannot use the -replace option with the -template option"); template_header = Header::open (opt[0][0]); for (size_t i = 0; i < 3; ++i) { output_header.size(i) = template_header.size(i); output_header.spacing(i) = template_header.spacing(i); } output_header.transform() = template_header.transform(); add_line (output_header.keyval()["comments"], std::string ("regridded to template image \"" + template_header.name() + "\"")); } // Warp 5D warp // TODO add reference to warp format documentation opt = get_options ("warp_full"); Image<default_type> warp; if (opt.size()) { warp = Image<default_type>::open (opt[0][0]).with_direct_io(); if (warp.ndim() != 5) throw Exception ("the input -warp_full image must be a 5D file."); if (warp.size(3) != 3) throw Exception ("the input -warp_full image must have 3 volumes (x,y,z) in the 4th dimension."); if (warp.size(4) != 4) throw Exception ("the input -warp_full image must have 4 volumes in the 5th dimension."); if (linear) throw Exception ("the -warp_full option cannot be applied in combination with -linear since the " "linear transform is already included in the warp header"); } // Warp from image1 or image2 int from = 1; opt = get_options ("from"); if (opt.size()) { from = opt[0][0]; if (!warp.valid()) WARN ("-from option ignored since no 5D warp was input"); } // Warp deformation field opt = get_options ("warp"); if (opt.size()) { if (warp.valid()) throw Exception ("only one warp field can be input with either -warp or -warp_mid"); warp = Image<default_type>::open (opt[0][0]).with_direct_io (Stride::contiguous_along_axis(3)); if (warp.ndim() != 4) throw Exception ("the input -warp file must be a 4D deformation field"); if (warp.size(3) != 3) throw Exception ("the input -warp file must have 3 volumes in the 4th dimension (x,y,z positions)"); } // Inverse const bool inverse = get_options ("inverse").size(); if (inverse) { if (!(linear || warp.valid())) throw Exception ("no linear or warp transformation provided for option '-inverse'"); if (warp.valid()) if (warp.ndim() == 4) throw Exception ("cannot apply -inverse with the input -warp_df deformation field."); linear_transform = linear_transform.inverse(); } // Half const bool half = get_options ("half").size(); if (half) { if (!(linear)) throw Exception ("no linear transformation provided for option '-half'"); { Eigen::Matrix<default_type, 4, 4> temp; temp.row(3) << 0, 0, 0, 1.0; temp.topLeftCorner(3,4) = linear_transform.matrix().topLeftCorner(3,4); linear_transform.matrix() = temp.sqrt().topLeftCorner(3,4); } } // Flip opt = get_options ("flip"); if (opt.size()) { std::vector<int> axes = opt[0][0]; transform_type flip; flip.setIdentity(); for (size_t i = 0; i < axes.size(); ++i) { if (axes[i] < 0 || axes[i] > 2) throw Exception ("axes supplied to -flip are out of bounds (" + std::string (opt[0][0]) + ")"); flip(axes[i],3) += flip(axes[i],axes[i]) * input_header.spacing(axes[i]) * (input_header.size(axes[i])-1); flip(axes[i], axes[i]) *= -1.0; } if (!replace) flip = input_header.transform() * flip * input_header.transform().inverse(); linear_transform = linear_transform * flip; linear = true; } Stride::List stride = Stride::get (input_header); // Detect FOD image for reorientation opt = get_options ("noreorientation"); bool fod_reorientation = false; Eigen::MatrixXd directions_cartesian; if (!opt.size() && (linear || warp.valid() || template_header.valid()) && input_header.ndim() == 4 && input_header.size(3) >= 6 && input_header.size(3) == (int) Math::SH::NforL (Math::SH::LforN (input_header.size(3)))) { CONSOLE ("SH series detected, performing apodised PSF reorientation"); fod_reorientation = true; Eigen::MatrixXd directions_az_el; opt = get_options ("directions"); if (opt.size()) directions_az_el = load_matrix (opt[0][0]); else directions_az_el = DWI::Directions::electrostatic_repulsion_300(); Math::SH::spherical2cartesian (directions_az_el, directions_cartesian); // load with SH coeffients contiguous in RAM stride = Stride::contiguous_along_axis (3, input_header); } // Modulate FODs bool modulate = false; if (get_options ("modulate").size()) { modulate = true; if (!fod_reorientation) throw Exception ("modulation can only be performed with FOD reorientation"); } // Rotate/Flip gradient directions if present if (linear && input_header.ndim() == 4 && !warp && !fod_reorientation) { try { auto grad = DWI::get_DW_scheme (input_header); if (input_header.size(3) == (ssize_t) grad.rows()) { INFO ("DW gradients detected and will be reoriented"); Eigen::MatrixXd rotation = linear_transform.linear(); Eigen::MatrixXd test = rotation.transpose() * rotation; test = test.array() / test.diagonal().mean(); if (!test.isIdentity (0.001)) WARN ("the input linear transform contains shear or anisotropic scaling and " "therefore should not be used to reorient diffusion gradients"); if (replace) rotation = linear_transform.linear() * input_header.transform().linear().inverse(); for (ssize_t n = 0; n < grad.rows(); ++n) { Eigen::Vector3 grad_vector = grad.block<1,3>(n,0); grad.block<1,3>(n,0) = rotation * grad_vector; } DWI::set_DW_scheme (output_header, grad); } } catch (Exception& e) { e.display (2); } } // Interpolator int interp = 2; // cubic opt = get_options ("interp"); if (opt.size()) { interp = opt[0][0]; if (!warp && !template_header) WARN ("interpolator choice ignored since the input image will not be regridded"); } // Out of bounds value float out_of_bounds_value = 0.0; opt = get_options ("nan"); if (opt.size()) { out_of_bounds_value = NAN; if (!warp && !template_header) WARN ("Out of bounds value ignored since the input image will not be regridded"); } auto input = input_header.get_image<float>().with_direct_io (stride); // Reslice the image onto template if (template_header.valid() && !warp) { INFO ("image will be regridded"); if (get_options ("midway_space").size()) { INFO("regridding to midway space"); std::vector<Header> headers; headers.push_back(input_header); headers.push_back(template_header); std::vector<Eigen::Transform<default_type, 3, Eigen::Projective>> void_trafo; auto padding = Eigen::Matrix<double, 4, 1>(1.0, 1.0, 1.0, 1.0); int subsampling = 1; auto midway_header = compute_minimum_average_header (headers, subsampling, padding, void_trafo); for (size_t i = 0; i < 3; ++i) { output_header.size(i) = midway_header.size(i); output_header.spacing(i) = midway_header.spacing(i); } output_header.transform() = midway_header.transform(); } if (interp == 0) output_header.datatype() = DataType::from_command_line (input_header.datatype()); auto output = Image<float>::create (argument[1], output_header).with_direct_io(); switch (interp) { case 0: Filter::reslice<Interp::Nearest> (input, output, linear_transform, Adapter::AutoOverSample, out_of_bounds_value); break; case 1: Filter::reslice<Interp::Linear> (input, output, linear_transform, Adapter::AutoOverSample, out_of_bounds_value); break; case 2: Filter::reslice<Interp::Cubic> (input, output, linear_transform, Adapter::AutoOverSample, out_of_bounds_value); break; case 3: Filter::reslice<Interp::Sinc> (input, output, linear_transform, Adapter::AutoOverSample, out_of_bounds_value); break; default: assert (0); break; } if (fod_reorientation) Registration::Transform::reorient ("reorienting", output, output, linear_transform, directions_cartesian.transpose(), modulate); } else if (warp.valid()) { if (replace) throw Exception ("you cannot use the -replace option with the -warp or -warp_df option"); if (!template_header) { for (size_t i = 0; i < 3; ++i) { output_header.size(i) = warp.size(i); output_header.spacing(i) = warp.spacing(i); } output_header.transform() = warp.transform(); add_line (output_header.keyval()["comments"], std::string ("resliced using warp image \"" + warp.name() + "\"")); } auto output = Image<float>::create(argument[1], output_header).with_direct_io(); if (warp.ndim() == 5) { Image<default_type> warp_deform; // Warp to the midway space defined by the warp grid if (get_options ("midway_space").size()) { warp_deform = Registration::Warp::compute_midway_deformation (warp, from); // Use the full transform to warp from the image image to the template } else { warp_deform = Registration::Warp::compute_full_deformation (warp, template_header, from); } apply_warp (input, output, warp_deform, interp, out_of_bounds_value); if (fod_reorientation) Registration::Transform::reorient_warp ("reorienting", output, warp_deform, directions_cartesian.transpose(), modulate); // Compose and apply input linear and 4D deformation field } else if (warp.ndim() == 4 && linear) { auto warp_composed = Image<default_type>::scratch (warp); Registration::Warp::compose_linear_deformation (linear_transform, warp, warp_composed); apply_warp (input, output, warp_composed, interp, out_of_bounds_value); if (fod_reorientation) Registration::Transform::reorient_warp ("reorienting", output, warp_composed, directions_cartesian.transpose(), modulate); // Apply 4D deformation field only } else { apply_warp (input, output, warp, interp, out_of_bounds_value); if (fod_reorientation) Registration::Transform::reorient_warp ("reorienting", output, warp, directions_cartesian.transpose(), modulate); } // No reslicing required, so just modify the header and do a straight copy of the data } else { if (get_options ("midway").size()) throw Exception ("midway option given but no template image defined"); INFO ("image will not be regridded"); Eigen::MatrixXd rotation = linear_transform.linear(); Eigen::MatrixXd temp = rotation.transpose() * rotation; if (!temp.isIdentity (0.001)) WARN("the input linear transform is not orthonormal and therefore applying this without the -template" "option will mean the output header transform will also be not orthonormal"); add_line (output_header.keyval()["comments"], std::string ("transform modified")); if (replace) output_header.transform() = linear_transform; else output_header.transform() = linear_transform.inverse() * output_header.transform(); auto output = Image<float>::create (argument[1], output_header).with_direct_io(); copy_with_progress (input, output); if (fod_reorientation) { transform_type transform = linear_transform; if (replace) transform = linear_transform * output_header.transform().inverse(); Registration::Transform::reorient ("reorienting", output, output, transform, directions_cartesian.transpose()); } } }
void RGBDOdometry::getIncrementalTransformation(Eigen::Vector3f & trans, Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & rot, const bool & rgbOnly, const float & icpWeight, const bool & pyramid, const bool & fastOdom, const bool & so3) { bool icp = !rgbOnly && icpWeight > 0; bool rgb = rgbOnly || icpWeight < 100; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev = rot; Eigen::Vector3f tprev = trans; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr = Rprev; Eigen::Vector3f tcurr = tprev; if(rgb) { for(int i = 0; i < NUM_PYRS; i++) { computeDerivativeImages(nextImage[i], nextdIdx[i], nextdIdy[i]); } } Eigen::Matrix<double, 3, 3, Eigen::RowMajor> resultR = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Identity(); if(so3) { int pyramidLevel = 2; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> R_lr = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Identity(); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero(); K(0, 0) = intr(pyramidLevel).fx; K(1, 1) = intr(pyramidLevel).fy; K(0, 2) = intr(pyramidLevel).cx; K(1, 2) = intr(pyramidLevel).cy; K(2, 2) = 1; float lastError = std::numeric_limits<float>::max() / 2; float lastCount = std::numeric_limits<float>::max() / 2; Eigen::Matrix<double, 3, 3, Eigen::RowMajor> lastResultR = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Identity(); for(int i = 0; i < 10; i++) { Eigen::Matrix<float, 3, 3, Eigen::RowMajor> jtj; Eigen::Matrix<float, 3, 1> jtr; Eigen::Matrix<double, 3, 3, Eigen::RowMajor> homography = K * resultR * K.inverse(); mat33 imageBasis; memcpy(&imageBasis.data[0], homography.cast<float>().eval().data(), sizeof(mat33)); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_inv = K.inverse(); mat33 kinv; memcpy(&kinv.data[0], K_inv.cast<float>().eval().data(), sizeof(mat33)); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_R_lr = K * resultR; mat33 krlr; memcpy(&krlr.data[0], K_R_lr.cast<float>().eval().data(), sizeof(mat33)); float residual[2]; TICK("so3Step"); so3Step(lastNextImage[pyramidLevel], nextImage[pyramidLevel], imageBasis, kinv, krlr, sumDataSO3, outDataSO3, jtj.data(), jtr.data(), &residual[0], GPUConfig::getInstance().so3StepThreads, GPUConfig::getInstance().so3StepBlocks); TOCK("so3Step"); lastSO3Error = sqrt(residual[0]) / residual[1]; lastSO3Count = residual[1]; //Converged if(lastSO3Error < lastError && lastCount == lastSO3Count) { break; } else if(lastSO3Error > lastError + 0.001) //Diverging { lastSO3Error = lastError; lastSO3Count = lastCount; resultR = lastResultR; break; } lastError = lastSO3Error; lastCount = lastSO3Count; lastResultR = resultR; Eigen::Vector3f delta = jtj.ldlt().solve(jtr); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rotUpdate = OdometryProvider::rodrigues(delta.cast<double>()); R_lr = rotUpdate.cast<float>() * R_lr; for(int x = 0; x < 3; x++) { for(int y = 0; y < 3; y++) { resultR(x, y) = R_lr(x, y); } } } } iterations[0] = fastOdom ? 3 : 10; iterations[1] = pyramid ? 5 : 0; iterations[2] = pyramid ? 4 : 0; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev_inv = Rprev.inverse(); mat33 device_Rprev_inv = Rprev_inv; float3 device_tprev = *reinterpret_cast<float3*>(tprev.data()); Eigen::Matrix<double, 4, 4, Eigen::RowMajor> resultRt = Eigen::Matrix<double, 4, 4, Eigen::RowMajor>::Identity(); if(so3) { for(int x = 0; x < 3; x++) { for(int y = 0; y < 3; y++) { resultRt(x, y) = resultR(x, y); } } } for(int i = NUM_PYRS - 1; i >= 0; i--) { if(rgb) { projectToPointCloud(lastDepth[i], pointClouds[i], intr, i); } Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero(); K(0, 0) = intr(i).fx; K(1, 1) = intr(i).fy; K(0, 2) = intr(i).cx; K(1, 2) = intr(i).cy; K(2, 2) = 1; lastRGBError = std::numeric_limits<float>::max(); for(int j = 0; j < iterations[i]; j++) { Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Rt = resultRt.inverse(); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R = Rt.topLeftCorner(3, 3); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> KRK_inv = K * R * K.inverse(); mat33 krkInv; memcpy(&krkInv.data[0], KRK_inv.cast<float>().eval().data(), sizeof(mat33)); Eigen::Vector3d Kt = Rt.topRightCorner(3, 1); Kt = K * Kt; float3 kt = {(float)Kt(0), (float)Kt(1), (float)Kt(2)}; int sigma = 0; int rgbSize = 0; if(rgb) { TICK("computeRgbResidual"); computeRgbResidual(pow(minimumGradientMagnitudes[i], 2.0) / pow(sobelScale, 2.0), nextdIdx[i], nextdIdy[i], lastDepth[i], nextDepth[i], lastImage[i], nextImage[i], corresImg[i], sumResidualRGB, maxDepthDeltaRGB, kt, krkInv, sigma, rgbSize, GPUConfig::getInstance().rgbResThreads, GPUConfig::getInstance().rgbResBlocks); TOCK("computeRgbResidual"); } float sigmaVal = std::sqrt((float)sigma / rgbSize == 0 ? 1 : rgbSize); float rgbError = std::sqrt(sigma) / (rgbSize == 0 ? 1 : rgbSize); if(rgbOnly && rgbError > lastRGBError) { break; } lastRGBError = rgbError; lastRGBCount = rgbSize; if(rgbOnly) { sigmaVal = -1; //Signals the internal optimisation to weight evenly } Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_icp; Eigen::Matrix<float, 6, 1> b_icp; mat33 device_Rcurr = Rcurr; float3 device_tcurr = *reinterpret_cast<float3*>(tcurr.data()); DeviceArray2D<float>& vmap_curr = vmaps_curr_[i]; DeviceArray2D<float>& nmap_curr = nmaps_curr_[i]; DeviceArray2D<float>& vmap_g_prev = vmaps_g_prev_[i]; DeviceArray2D<float>& nmap_g_prev = nmaps_g_prev_[i]; float residual[2]; if(icp) { TICK("icpStep"); icpStep(device_Rcurr, device_tcurr, vmap_curr, nmap_curr, device_Rprev_inv, device_tprev, intr(i), vmap_g_prev, nmap_g_prev, distThres_, angleThres_, sumDataSE3, outDataSE3, A_icp.data(), b_icp.data(), &residual[0], GPUConfig::getInstance().icpStepThreads, GPUConfig::getInstance().icpStepBlocks); TOCK("icpStep"); } lastICPError = sqrt(residual[0]) / residual[1]; lastICPCount = residual[1]; Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_rgbd; Eigen::Matrix<float, 6, 1> b_rgbd; if(rgb) { TICK("rgbStep"); rgbStep(corresImg[i], sigmaVal, pointClouds[i], intr(i).fx, intr(i).fy, nextdIdx[i], nextdIdy[i], sobelScale, sumDataSE3, outDataSE3, A_rgbd.data(), b_rgbd.data(), GPUConfig::getInstance().rgbStepThreads, GPUConfig::getInstance().rgbStepBlocks); TOCK("rgbStep"); } Eigen::Matrix<double, 6, 1> result; Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_rgbd = A_rgbd.cast<double>(); Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_icp = A_icp.cast<double>(); Eigen::Matrix<double, 6, 1> db_rgbd = b_rgbd.cast<double>(); Eigen::Matrix<double, 6, 1> db_icp = b_icp.cast<double>(); if(icp && rgb) { double w = icpWeight; lastA = dA_rgbd + w * w * dA_icp; lastb = db_rgbd + w * db_icp; result = lastA.ldlt().solve(lastb); } else if(icp) { lastA = dA_icp; lastb = db_icp; result = lastA.ldlt().solve(lastb); } else if(rgb) { lastA = dA_rgbd; lastb = db_rgbd; result = lastA.ldlt().solve(lastb); } else { assert(false && "Control shouldn't reach here"); } Eigen::Isometry3f rgbOdom; OdometryProvider::computeUpdateSE3(resultRt, result, rgbOdom); Eigen::Isometry3f currentT; currentT.setIdentity(); currentT.rotate(Rprev); currentT.translation() = tprev; currentT = currentT * rgbOdom.inverse(); tcurr = currentT.translation(); Rcurr = currentT.rotation(); } } if(rgb && (tcurr - tprev).norm() > 0.3) { Rcurr = Rprev; tcurr = tprev; } if(so3) { for(int i = 0; i < NUM_PYRS; i++) { std::swap(lastNextImage[i], nextImage[i]); } } trans = tcurr; rot = Rcurr; }