| 1847 | |
| 1848 | template <class Float> |
| 1849 | void ComputeDiagonalBlock(size_t ncam, size_t npts, float lambda, bool dampd, |
| 1850 | const Float* jc, const int* cmap, const Float* jp, |
| 1851 | const int* pmap, const int* cmlist, const Float* sj, |
| 1852 | const Float* wq, Float* diag, Float* blocks, |
| 1853 | int radial_distortion, bool jc_transpose, int mt1 = 2, |
| 1854 | int mt2 = 2, int mode = 0) { |
| 1855 | const int vn = radial_distortion ? 8 : 7; |
| 1856 | const size_t bc = vn * 8; |
| 1857 | float lambda1 = dampd ? 0.0f : lambda; |
| 1858 | float lambda2 = dampd ? (1.0f + lambda) : 1.0f; |
| 1859 | |
| 1860 | if (mode == 0) { |
| 1861 | const size_t bsz = bc * ncam + npts * 6; |
| 1862 | const size_t dsz = 8 * ncam + npts * POINT_ALIGN; |
| 1863 | bool use_jq = wq != NULL; |
| 1864 | /////////////////////////////////////////// |
| 1865 | SetVectorZero(blocks, blocks + bsz); |
| 1866 | SetVectorZero(diag, diag + dsz); |
| 1867 | |
| 1868 | //////////////////////////////// |
| 1869 | if (use_jq) ComputeDiagonalAddQ(ncam, wq, diag, sj); |
| 1870 | ComputeDiagonalBlockC(ncam, lambda1, lambda2, jc, cmap, cmlist, diag, |
| 1871 | blocks, vn, jc_transpose, use_jq, mt1); |
| 1872 | ComputeDiagonalBlockP(npts, lambda1, lambda2, jp, pmap, diag + 8 * ncam, |
| 1873 | blocks + bc * ncam, mt2); |
| 1874 | } else if (mode == 1) { |
| 1875 | const size_t bsz = bc * ncam; |
| 1876 | const size_t dsz = 8 * ncam; |
| 1877 | bool use_jq = wq != NULL; |
| 1878 | /////////////////////////////////////////// |
| 1879 | SetVectorZero(blocks, blocks + bsz); |
| 1880 | SetVectorZero(diag, diag + dsz); |
| 1881 | |
| 1882 | //////////////////////////////// |
| 1883 | if (use_jq) ComputeDiagonalAddQ(ncam, wq, diag, sj); |
| 1884 | ComputeDiagonalBlockC(ncam, lambda1, lambda2, jc, cmap, cmlist, diag, |
| 1885 | blocks, vn, jc_transpose, use_jq, mt1); |
| 1886 | } else { |
| 1887 | blocks += bc * ncam; |
| 1888 | diag += 8 * ncam; |
| 1889 | const size_t bsz = npts * 6; |
| 1890 | const size_t dsz = npts * POINT_ALIGN; |
| 1891 | /////////////////////////////////////////// |
| 1892 | SetVectorZero(blocks, blocks + bsz); |
| 1893 | SetVectorZero(diag, diag + dsz); |
| 1894 | |
| 1895 | //////////////////////////////// |
| 1896 | ComputeDiagonalBlockP(npts, lambda1, lambda2, jp, pmap, diag, blocks, mt2); |
| 1897 | } |
| 1898 | } |
| 1899 | |
| 1900 | template <class Float> |
| 1901 | void ComputeDiagonalBlock_(float lambda, bool dampd, const avec<Float>& camerav, |
no test coverage detected