| 1899 | |
| 1900 | template <class Float> |
| 1901 | void ComputeDiagonalBlock_(float lambda, bool dampd, const avec<Float>& camerav, |
| 1902 | const avec<Float>& pointv, const avec<Float>& meas, |
| 1903 | const vector<int>& jmapv, const avec<Float>& sjv, |
| 1904 | avec<Float>& qwv, avec<Float>& diag, |
| 1905 | avec<Float>& blocks, bool intrinsic_fixed, |
| 1906 | int radial_distortion, int mode = 0) { |
| 1907 | const int vn = radial_distortion ? 8 : 7; |
| 1908 | const size_t szbc = vn * 8; |
| 1909 | size_t ncam = camerav.size() / 16; |
| 1910 | size_t npts = pointv.size() / POINT_ALIGN; |
| 1911 | size_t sz_jcd = ncam * 8; |
| 1912 | size_t sz_jcb = ncam * szbc; |
| 1913 | avec<Float> blockpv(blocks.size()); |
| 1914 | SetVectorZero(blockpv); |
| 1915 | SetVectorZero(diag); |
| 1916 | ////////////////////////////////////////////////////// |
| 1917 | float lambda1 = dampd ? 0.0f : lambda; |
| 1918 | float lambda2 = dampd ? (1.0f + lambda) : 1.0f; |
| 1919 | |
| 1920 | Float jbufv[24 + 8]; // size_t offset = ((size_t) jbufv) & 0xf; |
| 1921 | // Float* jxc = jbufv + ((16 - offset) / sizeof(Float)); |
| 1922 | Float* jxc = (Float*)ALIGN_PTR(jbufv); |
| 1923 | Float *jyc = jxc + 8, *jxp = jxc + 16, *jyp = jxc + 20; |
| 1924 | |
| 1925 | ////////////////////////////// |
| 1926 | const int* jmap = &jmapv[0]; |
| 1927 | const Float* camera = &camerav[0]; |
| 1928 | const Float* point = &pointv[0]; |
| 1929 | const Float* ms = &meas[0]; |
| 1930 | const Float* sjc0 = sjv.size() ? &sjv[0] : NULL; |
| 1931 | const Float* sjp0 = sjv.size() ? &sjv[sz_jcd] : NULL; |
| 1932 | ////////////////////////////////////////////// |
| 1933 | Float *blockpc = &blockpv[0], *blockpp = &blockpv[sz_jcb]; |
| 1934 | Float *bo = blockpc, *bi = &blocks[0], *di = &diag[0]; |
| 1935 | |
| 1936 | ///////////////////////////////////////////////////////// |
| 1937 | // diagonal blocks |
| 1938 | for (size_t i = 0; i < jmapv.size(); i += 2, jmap += 2, ms += 2) { |
| 1939 | int cidx = jmap[0], pidx = jmap[1]; |
| 1940 | const Float *c = camera + cidx * 16, *pt = point + pidx * POINT_ALIGN; |
| 1941 | ///////////////////////////////////////////////////////// |
| 1942 | JacobianOne(c, pt, ms, jxc, jyc, jxp, jyp, intrinsic_fixed, |
| 1943 | radial_distortion); |
| 1944 | |
| 1945 | /////////////////////////////////////////////////////////// |
| 1946 | if (mode != 2) { |
| 1947 | if (sjc0) { |
| 1948 | const Float* sjc = sjc0 + cidx * 8; |
| 1949 | ScaleJ8(jxc, jyc, sjc); |
| 1950 | } |
| 1951 | ///////////////////////////////////////// |
| 1952 | Float* bc = blockpc + cidx * szbc; |
| 1953 | AddBlockJtJ(jxc, bc, vn); |
| 1954 | AddBlockJtJ(jyc, bc, vn); |
| 1955 | } |
| 1956 | |
| 1957 | if (mode != 1) { |
| 1958 | if (sjp0) { |
no test coverage detected